v

Medical Robotics
Achim Schweikard
Institute for Robotics and Cognitive Systems
Universität zu Lübeck
Ratzeburger Allee 160
23538 Lübeck, Germany
schweikard@rob.uni-luebeck.de
Phone: +49 – (0)451 – 5005201
Fax: +49 – (0)451 – 5005202

Floris Ernst
Institute for Robotics and Cognitive Systems
Universität zu Lübeck
Ratzeburger Allee 160
23538 Lübeck, Germany
ernst@rob.uni-luebeck.de
Phone: +49 – (0)451 – 5005208
Fax: +49 – (0)451 – 5005202
Contents

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.1 Robots for Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.2 Movement Replication . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
1.3 Robots for Imaging . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
1.4 Rehabilitation and Prosthetics . . . . . . . . . . . . . . . . . . . . . . 24
Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

2 Describing Spatial Position and Orientation . . . . . . . . . . . 33

2.1 Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
2.2 Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
2.3 Linkages . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
2.4 Three-Joint Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
2.5 Standardizing Kinematic Analysis . . . . . . . . . . . . . . . . . . 49
2.6 Computing Joint Angles . . . . . . . . . . . . . . . . . . . . . . . . . . 53
2.7 Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

3 Robot Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
3.1 Three-joint robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
3.2 Six-joint robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
3.3 Inverse Solution for the Seven-Joint DLR-Kuka Robot . 89
3.4 Eight-Joint Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

vii
viii Contents

3.5 C-arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103

3.6 Center-of-arc kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . 119
3.7 Surgical microscopes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
3.8 Kinematics and Dexterity . . . . . . . . . . . . . . . . . . . . . . . . . 120
Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128

4 Joint Velocities and Jacobi-Matrices . . . . . . . . . . . . . . . . . . 131

4.1 C-arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
4.2 Jacobi-Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
4.3 Jacobi-matrices and velocity functions . . . . . . . . . . . . . . 142
4.4 Geometric Jacobi-matrix . . . . . . . . . . . . . . . . . . . . . . . . . . 145
4.5 Singularities and Dexterity . . . . . . . . . . . . . . . . . . . . . . . . 161
Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 168
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 168

5 Navigation and Registration . . . . . . . . . . . . . . . . . . . . . . . . . 169

5.1 Digitally reconstructed radiographs . . . . . . . . . . . . . . . . . 171
5.2 Points and Landmarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173
5.3 Contour-based Registration . . . . . . . . . . . . . . . . . . . . . . . . 177
5.4 Intensity-based Registration . . . . . . . . . . . . . . . . . . . . . . . 181
5.5 Image Deformation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192
5.6 Hand-eye Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201
Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 206
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 210
Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 212
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 213

6 Treatment Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 219

6.1 Planning for Orthopedic Surgery . . . . . . . . . . . . . . . . . . . 219
6.2 Planning for Radiosurgery . . . . . . . . . . . . . . . . . . . . . . . . . 223
6.3 Four-Dimensional Planning . . . . . . . . . . . . . . . . . . . . . . . 241
Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 245
Contents ix

Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 247

7 Motion Correlation and Tracking . . . . . . . . . . . . . . . . . . . . . 251

7.1 Motion Correlation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 252
7.2 Regression and normal equations . . . . . . . . . . . . . . . . . . . 255
7.3 Support Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 258
7.4 Double Correlation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 277
Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 279
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 285
Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 286
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 287

8 Motion Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 291

8.1 The MULIN Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . 294
8.2 Least Means Square Prediction . . . . . . . . . . . . . . . . . . . . . 298
8.3 Wavelet-based LMS Prediction . . . . . . . . . . . . . . . . . . . . . 301
8.4 Support Vectors for Prediction . . . . . . . . . . . . . . . . . . . . . 310
8.5 Fast-Lane Methods and Performance Measures . . . . . . . 313
Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 317
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 320
Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 320
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 321

9 Motion Replication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325

9.1 Tremor filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 327
9.2 Forces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 329
9.3 Joint Torques and Jacobi-Matrices . . . . . . . . . . . . . . . . . . 335
Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 342
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 342
Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 343
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 344

10 Applications of Surgical Robotics . . . . . . . . . . . . . . . . . . . . . 347

10.1 Radiosurgery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 347
10.2 Orthopedic Surgery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 350
10.3 Urologic Surgery and Robotic Imaging . . . . . . . . . . . . . . 352
10.4 Cardiac Surgery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354
x Contents

10.5 Neurosurgery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 355

10.6 Control Modes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 355
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 357
Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 357
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 358

11 Rehabilitation, Neuroprosthetics and Brain-Machine

Interfaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363
11.1 Rehabilitation for Limbs . . . . . . . . . . . . . . . . . . . . . . . . . . 363
11.2 Brain-Machine Interfaces . . . . . . . . . . . . . . . . . . . . . . . . . 364
11.3 Steerable Needles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 370
Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 373
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 374

A Image Modalities and Sensors . . . . . . . . . . . . . . . . . . . . . . . . 377

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384

B Selected Exercise Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . 385

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 398

C Geometric Jacobian for the Six-Joint Elbow Manipulator 399

D Geometric Jacobian for the Seven-Joint DLR-Kuka

Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 409

E Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 419

List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 431

List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 433

Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435
Preface

Medical robotics is an interdisciplinary field, with methods from com-

puter science, mathematics, mechanical engineering and medicine.
The field emerged in the 1980s as a new branch of robotics. Robotics
itself was then a branch of artificial intelligence. However, a number
of technical and mathematical problems had to be solved to bring ro-
bots to routine clinical use. These problems were far outside the scope
of artificial intelligence, and this supported the emergence of the new
field.

When comparing medical robotics to industrial robotics, we see that

the latter field is the one that sells most robots. By contrast, some
of the most challenging research problems arise in medical robotics:
there is a need for improving the accuracy of surgical procedures, and
image guidance has become a central element of this. If we imagine
that robotic exoskeletons can help paralyzed patients, it becomes clear
that we will need methods for motion learning and brain-computer
interfaces.
We wrote this book as a text book for a one-semester class on medical
robotics. Students are undergraduates in computer science, mechan-
ical engineering or technical mathematics. When writing this book,
we were guided by two thoughts:
• Computer scientists and engineers should learn to understand ap-
plication domains, and medicine is an ideal application domain for
this purpose.
• The book should be suitable as a first course in robotics.

1
2 Preface

Comparing to standard text books on robotics, four elements have

been added here: (1) seven-joint robots. (2) navigation, calibration
and registration. (3) connection to machine learning. (4) applications
in surgical robotics, rehabilitation robotics and neuroengineering. The
text relies entirely on the most elementary mathematical tools, and we
give a detailed introduction for each new method. At the end of each
chapter, we provide exercises, preparing the grounds for the tools in
the next chapters, while linking to the methods in the current chapter.
Chapter 1 introduces the main applications of medical robotics.
Chapter 2 presents basic methods for describing position and orient-
ation as well as forward robot kinematics. This includes matrices,
angles, and the analysis of simple linkages.
Chapter 3 introduces inverse kinematics for robots. We develop the in-
verse kinematics for a seven-joint light-weight robot, called the DLR-
Kuka arm. This robot is designed for applications requiring direct in-
teraction with an operator. To obtain an inverse solution, we first solve
the kinematic equations for a standard six-joint robot with revolute
joints, called the elbow manipulator with spherical wrist. We obtain
a building-block strategy with which common types of robots can be
analyzed.
Geometric methods for inverse kinematics are an alternative to algeb-
raic methods. We illustrate geometric methods for the kinematic ana-
lysis of common medical devices.
In chapter 4 we consider Jacobi-matrices. There are two types of Jac-
obians: the analytic Jacobian and the geometric Jacobian. The ana-
lytic Jacobian offers alternative methods for the inverse analysis of
robots. The geometric Jacobian is a basic tool for velocity kinemat-
ics, and for analyzing the relationship between joint torques and static
forces/torques acting at the tool. We apply the geometric Jacobian to
problems involving C-arm x-ray imaging and robot design.
Chapter 5 establishes a connection to the classical tools from med-
ical imaging, i.e. MR, CT, ultrasound and x-ray imaging. With this
tool set, we address several problems: navigation, registration, image
calibration and robotic hand-eye calibration.
Chapter 6 describes methods for treatment planning. Computer pro-
grams have been used for treatment planning in radiation oncology
since the 1950s. Until the 1990s, conventional systems for radiation
Preface 3

oncology irradiated tumors from a small set of beam directions (typ-

ically 3-5 directions). To move the beam source, five-joint mechan-
isms with revolute and prismatic joints were used. This changed with
the introduction of robotic radiosurgery. Up to 1,000 distinct direc-
tions can now be used in a single treatment. This greatly increased the
complexity of treatment planning.
Chapter 7 is the first chapter (in a series of three chapters) with basic
methods from machine learning. We address the problem of tracking
anatomical motion (heartbeat, respiration) with a robot. To this end,
we learn the motion of a difficult-to-image anatomical target via the
motion of a surrogate.
In chapter 8, we predict respiratory motion to compensate for the time
lag of the robot, while tracking a target. Again, machine learning is
one of the main elements. In addition, we need a connection to basic
methods from signal processing.
In chapter 9, we consider motion replication. Robots for motion rep-
lication are the most commonly used surgical robots. The surgeon
moves a passive robot, and thereby issues motion commands to a
small replicator robot. We apply tools from machine learning to clas-
sify different types of motions, i.e. intended motion, tremor, and noise.
In the replication process, we must separate these types of motions (all
of which are part of the motion signal). In the same context, we apply
the geometric Jacobian to the analysis of static forces and torques.
The three applications in chapters 7-9 all converge to a set of methods,
which we term “motion learning”. Humans learn motion, and it is
obvious that we need dedicated tools for motion learning not only in
medical robotics.
Chapter 10 discusses integrated systems in medical robotics. The
methods developed in chapters 1-9 are the building blocks for such
systems. It should be noted that the methods in chapters 1-9 have
already found their way to the clinic, and have become routine tools,
especially in oncology, but also in orthopedics and neurology, most of
them via the connection to medical imaging and motion learning.
Chapter 11 gives an overview of methods for neuroprosthetics, brain-
machine interfaces and rehabilitation robotics.
4 Preface

In the appendix, we derive the geometric Jacobian matrix for the six-
joint elbow manipulator and for the DLR-Kuka seven-joint robot. We
also include solutions to selected exercises.

Video recordings from classes are available at:

https://medrob book.rob.uni-luebeck.de

Acknowledgements

We thank Max Wattenberg for converting many of the drawings to

TikZ-format, Robert Dürichen for implementing several of the meth-
ods for motion prediction and for discussions on the subject, Ulrich
Hofmann and Christian Wilde for their help with writing the text
on brain-machine interfaces, Ivo Kuhlemann for his help with im-
plementing and testing inverse kinematics algorithms, and Cornelia
Rieckhoff for proof-reading.
We offer special thanks to Rainer Burgkart for discussions and sug-
gestions on orthopedic navigation, John R. Adler for many discus-
sions on navigation in neurosurgery and radiosurgery, and our doctoral
students Norbert Binder, Christoph Bodensteiner, Christian Brack,
Ralf Bruder, Markus Finke, Heiko Gottschling, Max Heinig, Robert
Hanne, Matthias Hilbig, Philip Jauer, Volker Martens, Lars Matthäus,
Christoph Metzner, Lukas Ramrath, Lars Richter, Stefan Riesner, Mi-
chael Roth, Alexander Schlaefer, Stefan Schlichting, Fabian Schwar-
zer, Birgit Stender, Patrick Stüber, Benjamin Wagner, and Tobias Wis-
sel, for their help with experiments and graphics, and for reading
drafts of the book.
Finally, we thank Mohan Bodduluri, Gregg Glosser and James Wang
for their help with implementing robotic respiration tracking and treat-
ment planning for a clinical standard system.
Chapter 1
Introduction

Robots are now used in many clinical sub-domains, for example:

neurosurgery, orthopedic surgery, dental surgery, eye surgery, ear-nose
and throat surgery, abdominal surgery / laparoscopy, and radiosurgery.
This gives rise to a large number of new methods. However, medical
robotics is not limited to surgery. In recent years, four main types of
medical robots have emerged:
1. Robots for Navigation. The surgical instrument is moved by a ro-
bot arm. This allows precise positioning, based on pre-operative
imaging. The motion of anatomic structures (e.g. caused by res-
piration and pulsation) can be tracked.
2. Robots for Motion Replication. The robot replicates the surgeon’s
hand motion, via a passive robotic interface. Thus we can down-
scale the motion, reduce tremor and improve minimally invasive
methods.
3. Robots for Imaging. An imaging device is mounted to a robotic
arm, to acquire 2D or 3D images.
4. Rehabilitation and Prosthetics. Mechatronic devices can support
the recovery process of stroke patients. Robotic exoskeletons con-
trolled by brain-computer interfaces can replace or support dam-
aged anatomical structures.
We will discuss basic methods for each of the four cases. In the follow-
ing section, we will begin by looking at several examples for surgical
navigation.

5
6 1 Introduction

1.1 Robots for Navigation

A first example of a medical robot is shown in figure 1.1. A robot

guides a surgical saw. Before the intervention, the surgeon defines a
cutting plane for a bone cut. During the operation, the robot places the
saw in the predefined plane, and the surgeon can move the saw within
this plane. This restricts the motion of the saw, and allows for placing
the cuts with high precision.
In the figure, we see two types of robot joints: revolute joints and pris-
matic joints. For a revolute joint, a rigid link rotates about an axis. A
prismatic link slides along a translational axis. The last joint in figure
figure 1.1 is a prismatic joint. Here, the prismatic joint is passive, i.e. it
is not actuated by a motor. Thus, the surgeon moves the saw by hand,
while the motion plane is given by the robot.

Prismatic joint

Tool

Revolute joints

Fig. 1.1: Surgical saw mounted to a robotic arm. The arm itself consists
of revolute joints. The saw is mounted to a passive component (prismatic
joint). The surgeon moves the saw manually (sliding motion only). By
construction, the manual motion of the saw is restricted to a single axis.
[1]
1.1 Robots for Navigation 7

Remark 1.1

Figure 1.2 illustrates a schematic notation for jointed mechanisms,

for the robot in figure 1.1. Revolute joints are denoted by cylinders,
where the cylinder axis specifies the axis of rotation. Prismatic joints
are denoted by boxes. The lid of the box indicates the direction of
motion.

z0
y0
x0

Fig. 1.2: Schematic notation for the robot in figure 1.1.

In the figure, notice that the first two joint axes (revolute joints) in-
tersect in space. Furthermore, the axes of joints two, three and four
are parallel. We also see the axes x0 , y0 and z0 of a base coordinate
system.

(End of Remark 1.1)

1.1.1 Navigation for Orthopedic Surgery

We will now show several examples for navigation problems arising

in orthopedic surgery.
Figure 1.3 shows a femur bone. The region shown blue is the target
area (e.g. a tumor). The instrument for removing the tumor is a sur-
gical drill. The skin cut is made on the right side (an arrow indicates
8 1 Introduction

Drilling direction

Fig. 1.3: Navigating a surgical drill. Femur figure from [2, Fig. 245].

the drilling direction). The target is on the left side. The tumor is well
visible in the CT image, but often not visible in an x-ray image. To
remove the tumor, we must guide the drill. Notice also that the drill
must not penetrate the joint surface. Here, the pre-operative 3D im-
ages must be matched to the intra-operative situation.
Figure 1.4 shows the head of a femur bone. A small gap between the
head and the rest of the bone is visible (see arrow). Epiphyseolysis is
a condition, in which a fracture along this gap results in a downward
slippage of the femur head (figure 1.4) [3].
To stabilize the femur head, screws are used (figure 1.5). However,
placing the screws is difficult. The reason is that only the surface of
the bone consists of hard material, the cortical bone. The interior of
the bone, called cancellous bone, or spongy bone, is much softer. The
screw tip must reach (but not penetrate) the cortical bone surface.
Figure 1.6 shows a cross-section (slice) of a CT-data set, side-by-side
with the corresponding MRI slice. The cortical bone is visible as a
ring (white/light grey in the CT image, and black in the MRI data).
The spongy bone is inside this ring. When placing a screw (as in fig-
ure 1.5), the tip of the screw should reach, but not penetrate the thin
cortical bone layer.
Figure 1.7 shows a similar application in spine surgery. Two or more
vertebrae must be stabilized. Screws through the pedicle hold a fixa-
1.1 Robots for Navigation 9

(a) Epiphysial lines of the femur,

shown in black. Source: [2, Fig.
253].

Fig. 1.4: Epiphyseolysis. Left images: epiphysial lines of the femur.

Source: [2, Fig. 253]. Right image: x-ray image of epiphyseolysis. The
femur bone is displaced against the head, the tips of the two arrows
should coincide.

tion plate. Likewise, in dental surgery, implants are held by screws in

delicate bony structures of the chin.
In figure 1.8, a wedge of the bone must be removed to obtain a better
positioning of the bone axis. The navigation system guides the sur-
geon to locate the cutting planes during the operation.

1.1.2 Radiologic Navigation

During an intervention, the visibility of anatomic target structures is

often limited. Thus, for example, a CT image is taken before an opera-
tion. During the acutal operation, only x-ray imaging is available. The
10 1 Introduction

Fig. 1.5: Treatment of epiphyseolysis.

Fig. 1.6: Cortical bone and spongy bone in CT (left) and MRI (right).
In the CT scan, the cortical bone shows up white (black in MRI). The
spongy bone is inside.
1.1 Robots for Navigation 11

Body
Costal fovea

Pedicle or root of Spinal canal

vertebral arch

Lamina Superior articular process

Fig. 1.7: Placement of pedicle screws. The insertion path for the screws
(red lines) must not touch the spinal canal. Source of drawing [2, Fig.
82].

Bony wedge

Cutting plane

Fig. 1.8: Corrective osteotomy for the femur bone. Source of the draw-
ing: [2, Fig. 245].
12 1 Introduction

CT image shows more detail than an intraoperative x-ray image, but

does not show the current position of the surgical instrument. For pre-
cise navigation, we would need a CT image showing both the instru-
ment and the target at the same time. Thus, we need a virtual marker
visualizing the instrument position in the CT image.
One first method for surgical navigation is called radiologic navig-
ation. Radiologic navigation is based on x-ray imaging. During the
operation, x-ray images are taken with a C-arm (figure 1.9). A C-arm
is a mobile x-ray imaging device with five joints. There are two pris-
matic joints, and three revolute joints. A C-arm allows for taking x-ray
images from varying angles during an operation.

Fig. 1.9: C-arm x-ray imaging. A C-shaped structure carries an x-ray

source and an x-ray detector. The C is mounted to a jointed mechanism
with five joints [4].

Figure 1.10 shows the joints of a C-arm, in the notation introduced in

remark 1.1.
A second tool needed for radiological navigation is infrared tracking
(IR tracking). An infrared tracking system consists of a camera, and
one or more infrared markers, either reflective (i.e. illuminated by the
camera) or active IR-LEDs (Figure 1.11). The system tracks positions
and orientations of the markers in space. Markers can be attached to
objects and tools in the operating room. The camera of the tracking
system is attached to the wall of the operating room, and provides the
1.1 Robots for Navigation 13

J5

J3 J4

J2

J1

Fig. 1.10: Joints of a C-arm. J2 , J4 and J5 are revolute joints. J1 and J3

are prismatic.

base coordinate system. Notice that infrared tracking not only outputs
the position of the pointer tip of the marker, but also the orientation of
the marker in space. This means we not only know the tip coordinates
(e.g. as x−, y−, z-coordinates) with respect to the camera coordinate
system, but we also obtain information about the angle and the point-
ing direction of the marker in space. To output such angular inform-
ation, standard tracking systems use matrices or quaternions. Typical
systems report the marker position to within 0.1 mm.
Figure 1.12 shows the principle of radiological navigation. A first
marker of the infrared tracking system is attached to the C-arm. The
second marker is attached to the saw. After making a skin incision
(opening the skin), a third marker is rigidly attached to the bone. Then
an x-ray image of the bone is taken with the C-arm. Given the position
of the C-arm in space, we can compute the exact spatial coordinates
of points on the bone visible in the C-arm image. These coordinates
are computed with respect to the (infrared) camera coordinate system.
We can now steer the robot to a predefined point on the bone surface,
or guide the robot to find a pre-planned angle with respect to the bone.
14 1 Introduction

Infrared tracking camera

Marker 1

Marker 2
Bone

Fig. 1.11: Infrared tracking system with two markers. One of the mark-
ers is attached to a bone.

If the bone moves during the procedure, we can calculate and correct
the displacement by subtraction.
When attaching a marker to the robot or the saw, we do not know the
spatial reference between the robot’s coordinate system, and the in-
ternal coordinate system of the infrared tracking camera. The process
of finding the spatial mapping between the two coordinate systems is
called hand-eye calibration. A similar problem occurs when calibrat-
ing C-arm images with respect to the infrared marker.
As noted above, tumors are often not visible in x-ray images, but well
visible in 3D CT images taken before the operation. The navigation
problem is now to align the bone in the x-ray image to the same bone
in the CT image. The alignment should be done such that the two im-
ages match, i.e. corresponding structures should be at the same posi-
tion. This process is called image registration. Not only x-ray images
are registered to CT images, but any pair of image modalities (CT,
MRI, ultrasound and many other modalities) can be considered in this
context.
1.1 Robots for Navigation 15

C-arm

Saw Bone

Fig. 1.12: Radiologic navigation. Markers for the infrared tracking sys-
tem are attached to the saw, the C-arm and the bone.

1.1.3 Stereotaxic Navigation

A second method for navigation is stereotaxic navigation. It is typic-

ally used in neurosurgery and allows for reaching targets in the brain
with high precision.
To this end, a small robotic mechanism with five joints is rigidly at-
tached to the head of the patient (figure 1.13). The instrument is a
needle, and can be moved by changing the settings of the joint angles.
Recall the schematic notation for robot joints. In the figure, the joints
are called J1 , J2 , ..., J5 . Here, J1 , J2 and J3 are prismatic, and J4 , J5 are
revolute joints.
During the procedure, the patient’s head is fixed in space with a ste-
reotaxic frame (figure 1.14). The frame has three parts (A, B and C).
Part A is the frame base. This part directly attaches to the head. Part B
is a box with localizers for CT/MR imaging, and attaches to the base.
The localizers are also called fiducials. Part C is the passive jointed
mechanism in figure 1.13, and part C can also be rigidly attached to
the frame base. To this end, we remove the localizer frame B.
Figure 1.14 (left side) shows the frame base with the localizer box
attached. The localizer box contains three N-shaped arrangements of
16 1 Introduction

z J1
x J3
y J2 J4
J5 Needle

Frame base

Fig. 1.13: Five-joint mechanism for stereotaxic neurosurgery.

Head-to-frame fixture
Fiducial markers
Target

Frame base
Localizer frame Fiducial markers

Fig. 1.14: Stereotaxic navigation in neurosurgery. The fiducial markers

are visible in the CT image and provide a reference coordinate system.
1.1 Robots for Navigation 17

rods. Six of these rods are horizontal, and three of them are oblique
(see figure).
After taking a CT or an MR image (with the frame and localizers in
place), we see vertical cross sections of the head (figure 1.14, right).
Vertical cross sections (with the patient on the treatment table, as in
figure 1.14) are also called axial cross sections. The three viewing
directions for imaging are shown in figure 1.15.
In the cross-sections, we will see points (indicated as small circles in
figure 1.14, right), stemming from the fiducial rods. From the distance
between two adjacent points (one from an oblique rod and one from a
horizontal rod) we derive the z-coordinate of the image cross-section.
With similar methods, we can obtain x- and y-coordinates of the target
in the image.
In the next step of the procedure, we remove the localizer box with
the fiducial rods, and attach the jointed mechanism to the frame base.
As noted above, the jointed mechanism carries the instrument (e.g. a
biopsy needle or an electrode).
Having determined the coordinates of the target in the images, we
can compute the angle settings for the jointed mechanism and insert a
needle along a predefined path. Again, calibration methods are needed
to map the CT/MR coordinate system to the frame coordinate system.

Example 1.1

The joints J4 and J5 in figure 1.13 are revolute joints. The joint angles
of J4 and J5 determine the orientation of the needle. The needle tip
moves when we change the values of the angles. Let θ4 and θ5 be
the values of the joint angles for J4 and J5 . The current values for
θ4 and θ5 can be read from a scale imprinted onto the metal frame.
We can compute the position of the needle tip for given θ4 , θ5 . The
position is described with respect to the coordinate system shown in
the figure. We can also derive a closed form expression, stating the
coordinates of the needle tip as a function of θ4 , θ5 . Similarly, we can
include the prismatic joints into the computation. This derivation of
a closed formula is one example of a forward kinematic analysis of
a mechanism. Conversely, computing angle values from a given tip
18 1 Introduction

position and needle orientation is called an inverse kinematic analysis

(see also exercise 1.1 at the end of this chapter).

(End of Example 1.1)

Coronal

Sagittal Axial

Fig. 1.15: Imaging directions: axial, sagittal, coronal

1.1.4 Non-invasive Navigation for the Head

Stereotaxic navigation with a head-frame is invasive and painful for

the patient. The frame must be attached under local anesthesia. The
next example shows a less invasive (but also less accurate) alternat-
ive for applications in neurology (figure 1.16). In transcranial mag-
netic stimulation (TMS), a magnetic coil stimulates small regions in
the brain. For diagnostic applications, the coil is held by the surgeon.
This becomes difficult if the coil must remain in the same place (with
respect to the head) for extended periods of time in certain cases more
than 30 minutes. In robotic TMS, we navigate the coil with a robot.
Surface points on the head are acquired with a marker of the infrared
tracking system. An additional infrared marker is attached to the head
with a velcro band. Before treatment, a second set of surface points
can be computed from an MR-image. The two point sets are then
matched in a registration step. In this way, we can locate a target inside
1.1 Robots for Navigation 19

Coil

Markers

Fig. 1.16: Navigated transcranial stimulation

the brain, given in MR-coordinates. During the procedure, the robot

can compensate for small motions of the head.
Beyond the fields mentioned above (orthopedic surgery, neurosurgery
and neurology), navigation methods are used in many other fields (e.g.
ENT surgery, radiosurgery, abdominal surgery and heart surgery). The
applications described above are simpler than many others, because
the target does not deform.

1.1.5 Navigation for Moving Targets

In radiosurgery, robots are used to move the radiation source. This

source weighs 200 kg and cannot be moved by the surgeon. Lung tu-
mors move as the patient breathes. The robot compensates for this
motion, i.e. the robot tracks the tumor motion, see figure 1.17.
Real-time tracking of internal organs is difficult, especially in this ap-
plication. Methods for motion correlation can overcome this problem:
suppose that the skin of the patient moves with respiration, and the tar-
get tumor also moves with respiration. The skin motion may be small
(i.e. in a range of 1-2 mm), but we can track it with fast cameras or
infrared tracking. Now assume the observable skin motion of the pa-
20 1 Introduction

tient correlates to the tumor motion. Then we can use skin motion as
a surrogate signal, to track the internal target.

Linear
accelerator

Beam
collimator
Patient
couch

Fig. 1.17: Radiosurgery: a robotic arm or a jointed mechanism moves

a medical linear accelerator. The linear accelerator generates a beam of
photon radiation.

In the same application (robotic radiosurgery), the motion of the ro-

bot must be planned. The plan must take into account the geometry
and location relationships in the area surrounding the tumor. Hence,
planning must be done on an individual basis for each treatment.

1.2 Movement Replication

As noted above, robots are not only used for navigation. Using ap-
propriate sensors, it is not difficult to record all hand motions of a
surgeon during an intervention. One can then replicate hand motion
by a robotic arm. With this, a number of problems in microsurgery
can be addressed. An example is shown in the next figure. The motion
of the surgeon can be down-scaled. Assume the motion range of the
surgeon’s hand is 1 cm, and our robot replicates the same motion, but
down-scales it to a range of 1 mm. Thus, delicate interventions can be
performed with very high accuracy, see figure 1.18.
1.3 Robots for Imaging 21

Fig. 1.18: Replicating the surgeon’s hand motion with a robotic inter-
face mounted to the patient couch (da Vinci Surgical System, Intuitive
Surgical, Inc.)

A second example is the placement of heart catheters [5]. The sur-

geon pushes the catheter through the blood vessel tree under image
guidance. A robotic interface transmits motion commands to the tip
of the catheter. The catheter tip is articulated, and can thus be steered.

1.3 Robots for Imaging

A further application of medical robots is image acquisition. The pro-

cess of computing a CT image from a set of x-ray images is one
instance of image reconstruction (see figure 1.19). As an example,
we can move the C-shaped structure of the C-arm. Then the source-
detector assembly will move along a circular arc in space. By taking a
series of x-ray images during this circular motion (figure 1.20), we ob-
tain the raw data for a three-dimensional CT image. A reconstruction
algorithm then computes the 3D image.
Above we discussed radiologic navigation. We saw that it combines
x-ray imaging and CT imaging. Image reconstruction is closely re-
22 1 Introduction

Fig. 1.19: Reconstruction. Left: 2D projection images taken from a series

of angles. Right: Reconstructed 3D image.

Detector Projection line

Source

Fig. 1.20: Rotating source-detector assembly for CT imaging or 3D ima-

ging with a C-arm.

lated to navigation, since our intra-operative reconstruction may rely

on partial or modified image data.
In a further application, we replace the C-arm by a robot moving the
source and the detector (figure 1.21).
Similar to C-arms, surgical microscopes for neurosurgery and oph-
thalmology are mounted to jointed mechanisms (figure 1.22). Repos-
itioning the microscope during the operation, even by a few milli-
meters, forces the surgeon to interrupt the operation. The surgeon
must first hand the instruments to an assistant, then unlock the mi-
croscope’s brakes and reposition the microscope manually. Then the
assistant must give the instruments back to the surgeon, and the in-
struments are reinserted into the operation cavity. If the microscope
is actuated, several of these steps become unnecessary. This not only
saves operation time, but reduces infection risk.
1.3 Robots for Imaging 23

Robotic manipulator
Flat panel
detector

X-ray source

Fig. 1.21: Robotic C-arm imaging system for angiography [6]

Fig. 1.22: Surgical microscope (MÖLLER Hi-R 1000, Möller-Wedel

GmbH)
24 1 Introduction

1.4 Rehabilitation and Prosthetics

After initial rehabilitation, most stroke patients are released to daily

life, often without regaining their original mobility. To improve this
situation, small and simple robotic training devices have been de-
veloped. Stroke patients can use these robots at home. The robots
perform simple and repetitive movements to help the patient regain
mobility.

Fig. 1.23: Robotic hand for stroke rehabilitation [7].

In neuro-prosthetics, intelligent actuator systems directly assist pa-

tients in performing motion tasks, e.g. after paralysis (see figure 1.23).
The systems can be reconfigured for various tasks, such as finger turn-
ing operations, bottle opening and grasping. In each case, the patient
applies forces and torques, which can be measured to adjust the sys-
tem parameters.
An exoskeleton is an external skeleton supporting the body. A first
application is to reduce tremor during microsurgery. A major research
goal is to provide actuated exoskeletons for paralyzed patients.
Targeted muscle reinnervation (TMR) is a surgical procedure for es-
tablishing a control channel between the patient and an exoskeleton.
In TMR, several independent nerve-muscle units are created in the
chest. These units can then be used for signal recording with external
electromyography (EMG). EMG is non-invasive, and electrodes on
the skin surface record signals from muscle contractions. With TMR,
patients can control a robotic arm prosthesis. With the artificial arms,
patients who lost both arms in accidents are able to perform a variety
1.4 Rehabilitation and Prosthetics 25

of tasks such as eating, putting on glasses and using a pair of scissors

[8]. Methods from machine learning can be applied to improve the
motion patterns on the side of the robot arm.

Exercises

Fig. 1.24: Joint angle θ4 for the neurosurgical frame in figure 1.13

Fig. 1.25: Joint angle θ5 for the frame in figure 1.13

26 1 Introduction

Exercise 1.1

The joint angles (θ4 and θ5 ) for the neurosurgical frame in figure 1.13
are shown in figure 1.25 and figure 1.24.
Place a coordinate system at the centroid of the frame base, with axes
(x-,y-,z-) as shown in figure 1.25.
Assume the distance of the needle tip (figure 1.25) from the coordinate
origin is 100 mm.
a) For angle values taken from the set θ4 ∈ {−90, 0, 90} and θ5 ∈
{0, 90, 180}verify that the expression
 
cos(θ5 )
100 · − sin(θ4 ) sin(θ5 ) (1.1)
cos(θ4 ) sin(θ5 )
gives the coordinates of the needle tip in mm.
b) Given the coordinates
 
px
p = py 
 (1.2)
pz
of a point in space, compute the joint angles θ4 and θ5 for reaching
this point.
Hint: Assume kpk = 100. Start from the equation
   
px cos(θ5 )
 py  = 100 · − sin(θ4 ) sin(θ5 ) (1.3)
pz cos(θ4 ) sin(θ5 )
The first line of equation 1.3 gives

px = 100 · cos(θ5 ) (1.4)

Solve this equation with the arccos-function. Then insert the solu-
tion into either of the remaining equations

py = −100 · sin(θ4 ) sin(θ5 ) (1.5)

and
1.4 Rehabilitation and Prosthetics 27

pz = 100 · cos(θ4 ) sin(θ5 ) (1.6)

Discuss the case θ5 = 0.

c) Discuss the case kpk 6= 100 with the prismatic joints in figure 1.13.

d) Implement your solution for part b) in a short program. Input:

point p with kpk = 100.
Output: point p’
p’ is computed as follows: Extract angles θ4 and θ5 from the input
point according to the solution in part b). Then reinsert the angles
θ4 and θ5 into equation 1.3, giving p’.

e) Extend your program to the general case with prismatic joints.

Exercise 1.2

a) Set up a flow chart for the navigation process illustrated in fig-

ure 1.12.
b) Describe the difference between radiologic navigation and stereo-
taxic navigation.
c) The terms navigation, calibration, reconstruction and registration
and their applications were described above. The process of delin-
eating organs in a medical image is called segmentation. Describe
a situation in which registration could be used to facilitate seg-
mentation. Conversely, describe a scenario in which segmentation
could help registration. Describe similar interactions between cal-
ibration and registration.

Exercise 1.3

As an example for image reconstruction, we consider the process of

computing a three-dimensional CT image from a series of x-ray im-
ages. You can think of a C-arm (figure 1.9) taking images from differ-
ent angles as it rotates the source-detector assembly.
28 1 Introduction

In figure 1.20, a projection line is shown dashed. The anatomy along

this projection line attenuates the x-rays. Each voxel along the projec-
tion line contributes to the attenuation. The sum of these contributions
(of each voxel) will then be measured at the detector. Hence, the sum
of the resulting attenuation is known. The goal is to estimate the un-
known gray level at each voxel, given all the sum values from different
angles.

Pixel value at this

Projection line x1 x2 x3 x4 detector position:
x1 + x2 + x3 + x4
Direction of
projection

Detector

Fig. 1.26: Input data for reconstruction. Variables xi correspond to

voxels (one variable for each voxel). A variable represents the attenu-
ation at this voxel.

Figure 1.26 illustrates the input data for reconstruction. For each
angle, we have a projection image. The goal is to compute a gray
level for each voxel, i.e. values for the variables xi . Input data are the
pixel values measured at the detector. In the figure, the pixel value for
the topmost pixel is the sum of the values x1 , . . . , x4 in the first row.
In the case of several projection images, we obtain a linear system of
equations of the following form

a11 x1 + a12 x2 + · · · + a1n xn = b1

..
.
am1 x1 + am2 x2 + · · · + amn xn = bm
(1.7)

Here, the values bi are the pixel values measured at the detector, the
coefficients ai j are constant, and each ai j is either 0 or 1. (We could
1.4 Rehabilitation and Prosthetics 29

use other values for ai j , for example a factor expressing the length of
the path of the ray through the corresponding voxel.) The resulting
linear equation system can be solved in many ways. The ART (algeb-
raic reconstruction technique) algorithm is particularly useful for this
application. To simplify the notation, we assume there are only two
equations and two variables.

a11 x1 + a12 x2 = b1
a21 x1 + a22 x2 = b2
(1.8)

x2 x2
a11 x1 + a12 x2 = b1
Start point
x(0) = [0, 0]T
x1 x1
a21 x1 + a22 x2 = b2 x(1)

Fig. 1.27: ART algorithm. Left: visualization of the two lines in equa-
tion 1.8. Right: Projecting the start point (0,0) onto one of the lines.

Each of the two equations corresponds to one line in the visualisation.

Start with an arbitrary estimate for the solution, for example the point
(0, 0)T (figure 1.27).
Now project this start point onto the first line (figure 1.27). Call this
new point x(1) . Proceed by projecting x(1) onto the second line. It-
eratively repeat, until the intersection point between the two lines is
reached. This is the solution of the equation system. We need to give a
closed formula for computing the projections. Set ai = (ai1 , ai2 ). Then

ai x(n) − bi
x(n+1) = x(n) − ai (1.9)
a2i
30 1 Introduction

We write the scalar product of ai and x(n) as ai x(n) instead of aTi x(n)
(see also the notation page in appendix E). Likewise, a2i denotes the
scalar product aTi ai . This iteration formula computes the (n + 1)-th
estimate x(n+1) from the n-th x(n) , i.e. it projects the point x(n) onto
the i-th line of the system, which is given by ai and bi . The line index
i can be chosen at random. The coefficients ai j form a matrix which
will be called A.
Instead of equation 1.8, we can thus write Ax = b. The ART algorithm
is now given by:
1. Start by setting x(0) = (0, 0)T .
2. Choose a random index i and compute x(n+1) from x(n) via equa-
tion 1.9
3. Compute the forward projection from the (n + 1)-th estimate, i.e.
b(n+1) = Ax(n+1)
4. Iterate steps 2 and 3 until the difference between new forward pro-
jection b(n+1) , computed in step 2, and values measured at the de-
tector is below tolerance
Notice that the process of reconstruction just described can be rever-
ted. This will give rise to interesting applications in registration and
navigation. We will discuss such applications in the chapter on regis-
tration.
a) Explain the difference between the two types of projections (pro-
jection in figure 1.26) and projection in figure 1.27 in the ART
algorithm.
b) Explain why reconstruction is needed to compute 2D slice images
(i.e. regular CT image stacks) from 2D x-ray images.
c) Explain why we cannot cancel some of the ai ’s in equation 1.9.
d) Prove equation 1.9.
e) Implement the ART algorithm for planar images. For simplicity,
first use synthetic images such as the binary image of a rectangle
shown in figure 1.28 (left) as input.
f) Compare different implementations of linear equation solvers,
such as the one provided in MATLAB (linsolve()) (or any
other linear equation system solver) to the implementation of the
ART algorithm for larger images.
References 31
0000000000000 000*000000000
0000000000000 000*000000000
0011111111100 001*111111100
0010000000100 0010*00000100
0010000000100 0010*00000100
0010000000100 0010*00000100
0011111111100 00111*1111100
0000000000000 00000*0000000
0000000000000 00000*0000000

Fig. 1.28: Left: Synthetic binary image of a rectangle, right: projection

line through the image, voxels along the projection line marked with ‘*’.

References

[1] M. Roth, C. Brack, A. Schweikard, H. Götte, J. Moctezuma, and

F. Gossé, “A new less invasive approach to knee surgery using a
vision-guided manipulator,” in International Symposium on Ro-
botics and Manufacturing (ISRAM’96), World Automation Con-
gress (WAC’96). ASME Press, 1996, pp. 731–738.
[2] H. Gray and W. H. Lewis, Anatomy of the Human Body, 20th ed.
Philadelphia, PA: Lea & Febiger, 1918. [Online]. Available:
http://www.bartleby.com/107/
[3] R. B. Salter and W. R. Harris, “Injuries involving the epiphyseal
plate,” The Journal of Bone and Joint Surgery, vol. 45, no. 3, pp.
587–622, Apr. 1963.
[4] R. Graumann, O. Schütz, and N. Strobel, “C-arm X-ray sys-
tem with adjustable detector positioning,” U.S. Patent 6,811,313,
Nov., 2004.
[5] J. Bismuth, E. Kashef, N. Cheshire, and A. B. Lumsden, “Feas-
ibility and safety of remote endovascular catheter navigation in a
porcine model.” Journal of Endovascular Therapy, vol. 18, no. 2,
pp. 243–249, apr 2011.
[6] S. Groß and D. Heinl, “C-arm mounted on a robotic arm,” U.S.
Patent Application 20 090 185 662, Jul., 2009.
[7] P. Weiss, M. Heldmann, T. F. Münte, A. Schweikard, and
E. Maehle, “A rehabilitation system for training based on visual
feedback distortion,” in Converging Clinical and Engineering Re-
32 1 Introduction

search: Proceedings of the International Conference on Neurore-

habilitation (ICNR) 2012, ser. Biosystems & Biorobotics, J. L.
Pons, D. Torricelli, and M. Pajaro, Eds., vol. 1. Springer, 2013,
pp. 299–303.
[8] T. A. Kuiken, G. A. Dumanian, R. D. Lipschutz, L. A. Miller,
and K. A. Stubblefield, “The use of targeted muscle reinnervation
for improved myoelectric prosthesis control in a bilateral shoulder
disarticulation amputee.” Prosthetics and Orthotics International,
vol. 28, no. 3, pp. 245–253, 2004.
Chapter 2
Describing Spatial Position and
Orientation

Coordinate systems describe the position of objects in space. For ex-

ample, we can define a fixed coordinate system in the room (e.g. one
corner of the room) and then refer to this coordinate system when stat-
ing coordinates of a point. If we agree that all coordinates are given in
millimeters, we obtain a common frame of reference.
Consider figure 1.2 and figure 1.3 in the introduction chapter. We need
to state the coordinates of the target point on the bone, and we also
need to know the current position of the drill tip. Furthermore, we will
need to specify the angle at which to hold the drill. In this chapter, we
will discuss the basic mathematical methods for this purpose.

2.1 Matrices

Assume we have defined a base coordinate system. Call this system

B. We are now given a second coordinate system S (figure 2.1).
In the figure, the displacement of S relative to B is denoted by the
vector p. The system S has the same orientation as the base system B,
meaning that the x-, y- and z-vectors of S point into the same directions
as the respective base vectors.
A 4 × 4 matrix M describes the position and orientation of S with
respect to the base coordinate system.
For the case in figure 2.1, the matrix M is

33
34 2 Describing Spatial Position and Orientation

zs

ys
z S xs

p = (px , py , pz )T
y

B x

Fig. 2.1: Vector p describes the position of the coordinate system S with
respect to the base coordinate system B.

 
100 px
0 1 0 py 
M=
0 0 1
. (2.1)
pz 
000 1

xs

ys
z S
zs

p
y

B x

Fig. 2.2: Coordinate system S after a change of orientation. We have

rotated S about the axis yS by −90 degrees. The vector xS now points into
the z-direction of the base, and zS points into the negative x-direction of
the base.
2.1 Matrices 35

But what if we change the orientation of the second system S? This is

shown in figure 2.2.
We must change M accordingly:
 
0 0 −1 px
0 1 0 p y 
M0 = 1 0 0 p z 
 (2.2)
00 0 1
We look at the upper left 3 × 3 part of this matrix. This part of the
matrix has a very simple interpretation, visible already in the above
examples. Specifically, the three columns of this 3 × 3-matrix each
represent a 3D-vector. The first such vector gives the direction of xS (in
terms of the base), the second gives the direction of yS , and the third
shows the direction of zS . You should verify this simple observation in
the examples of figures 2.1 and 2.2. Thus, we have rotated about the
yS -axis, which is reflected in the fact that the yS -axis does not change.
The last line of the matrix has an interesting geometric interpretation
as well, but here we ignore this, simply assuming we have added the
last line to make the matrix square.
In general, a 4 × 4 matrix representing the position of a system S with
respect to the base B has the following form:
 
nx ox ax px
ny oy ay py 
M0 = nz oz az pz 
 (2.3)
0 0 0 1
To distinguish the matrices of this form from other 4 × 4 matrices,
they are also called homogeneous 4 × 4 matrices.
We will sometimes use a shorter term for coordinate systems. Since a
coordinate system provides a frame of reference, we will simply call
it a frame.
Following the above observations, the three matrix entries nx , ny , nz
specify the orientation of xS . Likewise, the entries ox , oy , oz , ax , ay , az
specify yS and zS , respectively. Finally px , py , pz again specify the po-
sition of the origin of S. Notice again that only the 12 values nx , . . . , pz
carry the matrix information. The values in the last row of the matrix
are constant.
36 2 Describing Spatial Position and Orientation

Example 2.1

ys
S zs

z xs

B x

Fig. 2.3: Coordinate system S for a surgical saw.

Consider the surgical saw in figure 1.1 (first figure of the introduction
chapter). Our goal is to move the saw with a robot. Thus, we must
command the positions and orientations of the saw. First we attach a
coordinate system S to the saw. In figure 2.3, the saw is simply shown
as a block. The origin of S is a vertex of this block. The axes xS , yS and
zS of S coincide with edges of the block.
According to our notation, we find that the following matrix B MS de-
scribes the position of the saw:
 
0 0 1 px
B
 0 1 0 py 
MS =  −1 0 0 pz 
 (2.4)
0 00 1
The notation B MS indicates that the matrix gives the position and ori-
entation of S with respect to B.
Notice that here we have again rotated about the y-axis, but now in the
opposite direction. We see that we need a convention indicating the
direction of rotation. We will do this next, after introducing conven-
tions for rotations about fixed axes.
2.2 Angles 37

2.2 Angles

An alternative way to describe the orientation of the object coordinate

system S is to use angles. Thus in figure 2.3, we state that the saw’s
coordinate system is rotated by an angle of 90 degrees about the y-axis
of the base. To describe an arbitrary orientation in space, we use the
three angle values α, β , γ. They refer to rotations about the three axes
x, y and z of the base coordinate system. To represent a position of
the saw, we thus have the coordinates (px , py , pz )T of S, and the three
angles α, β , γ.
We must now define a convention for the direction of rotation. Thus
we must agree on the positive sense of rotation. Here we set forth the
following convention. A positive rotation about the x-axis will move
the y-axis towards the z-axis. Likewise, positive revolute motion about
the z-axis will move the x-axis towards the y-axis. The full conventions
are:
pos. sense x → y y → z z → x
neg. sense y → x z → y x → z
With this convention, our system in figure 2.3 is described by

(px , py , pz , 0, 90, 0)T (2.5)

In general, the six-vector

B
Y PRS = (px , py , pz , α, β , γ)T (2.6)
is called the Yaw-Pitch-Roll vector, or YPR-vector. However, there are
several other conventions (see the exercises at the end of this chapter).
We will soon see that the matrix representation is often preferable over
the YPR-representation.
The medical literature has its own naming conventions for rotations.
They are:
medicine engineering
flexion (+) / extension (-) yaw
varus (+) / valgus (-) pitch
rotation (+) / derotation (-) roll
38 2 Describing Spatial Position and Orientation

2.2.1 Relative Position and Orientation

Suppose we replace the saw by a drill (see figure 2.4). The drill has
its own coordinate system S. At the tip of the drill, we place a new
coordinate system S0 . We have the position of S with respect to the
base B. Now we would like to compute the position of S0 with respect
to the base system B. Thus, suppose we have defined two matrices
B M and S M 0 .
S S

yS
S zS

z xS
yS0
S0 zS 0
y

xS0
B x

Fig. 2.4: Drill tip coordinate system.

Here we have again used the convention that the matrix B MS describes
a transition from coordinate system B to coordinate system S. The
first of the two matrices (B MS ) has been computed above. The second
matrix is also easy to compute. From the figure, we see that it is simply
given by:
 
1 0 0 p0x
S
0 1 0 p0y 
MS 0 = 
0 0 1 p0z 
 (2.7)
000 1
Here p’ = (p0x , p0y , p0z )T denotes the origin of S0 , given in coordinates
of S.
2.2 Angles 39

S
BM
S

SM
S0

S0

BM
S0

Fig. 2.5: Matrix transformations. The matrix describing position and

orientation of the drill tip with respect to the base is the product of two
matrices B MS and S MS0 .

Now the matrix we are looking for (drill tip with respect to base) is
simply given by the product of the two matrices already computed,
i.e.

B
MS0 = B MS S MS0 . (2.8)
This is illustrated in figure 2.5. It is easy to verify this equation with
the above interpretation of matrix columns.
Above we looked at relative transformations between coordinate sys-
tems. The following example discusses a different situation: a vector
is given in a coordinate system S, and our goal is to express its co-
ordinates in another coordinate system.

Example 2.2

Assume the vector q in figure 2.6 is given with respect to the drill
coordinate system S. q points along the z-direction of S, and
 
0
S
q = 0 .
 (2.9)
1
40 2 Describing Spatial Position and Orientation

ys
S
q = zs

z xs

B x

Fig. 2.6: Transforming the coordinates of a point given with respect to S.

To express the fact that q is given in terms of S, we have used the

above notation with superscripts and subscripts.
Now from the figure we also see that q points along the positive x-axis
of the base coordinate system. Thus, in base coordinates q is given as
 
1
B
q = 0 .
 (2.10)
0
The matrix B MS transforming from the base system B to S was derived
in equation 2.4. In principle, we can use this matrix to compute B q
from the given S q.
However, we cannot directly multiply S q with B MS . The reason is
that S q is a three-vector and B MS is a 4 × 4-matrix. To overcome this
difficulty, we extend S q. Hence, we add a fourth coordinate, with the
value zero, i.e. we replace the three-vector S q = (qx , qy , qz )T by
 
qx
S
 qy 
q= 
 qz  . (2.11)
0
Multiplying, we obtain
2.3 Linkages 41

B
q =B MS S q. (2.12)

(End of Example 2.2)

2.3 Linkages

A linkage consists of several links and joints. The linkage moves if

we change the joint angles. Assume we mount a gripper to the tip of
the linkage. Our goal is to find a 4 × 4 matrix expressing the position
and orientation of the gripper, given the joint angles. Thus the matrix
we are looking for is a function of the joint angles. We will first look
at a one-joint linkage.

Example 2.3

Fig. 2.7: Planar robot with a single revolute joint.

Consider the robot shown in figure 2.7. It only has a single joint, and
a single link. But it does have a gripper, mounted to the end of the
single link.
This robot is not very interesting (it cannot even close its gripper) but
it illustrates how we apply coordinate systems. A null position of a
robot is a fixed reference position. Assume the null position of our
one-joint robot is the position shown. In this null position, the joint
angle is zero. If we change the joint angle, our robot will start to move.
42 2 Describing Spatial Position and Orientation

zB
yB
xB

Fig. 2.8: Base coordinate system for the robot in figure 2.7. The robot
has a single revolute joint. The axis of rotation of this joint is the z-axis
of the base system.

We place a base coordinate system in our standard orientation with the

origin on the robot plane (figure 2.8). The rotation axis of this joint is
the axis zB of our standard base system, i.e. zB is orthogonal to the
robot plane. We measure the angle of rotation by the angle θ . At the
position shown, we have θ = 0, i.e. the null position of our robot.
Thus our zB axis is the rotation axis of the joint. Also place a gripper
coordinate system with its origin into the center of the gripper (fig-
ure 2.9). Call the gripper system G. In the null position, G has the
same orientation as B. We assume G is rigidly attached to the grip-
per and moves with the robot. B does not move, since it is our base
system.
Our goal is to find the transformation matrix transforming from the
base to the gripper system. This matrix will depend on θ , i.e. matrix
entries will be functions of θ .

zG
yG
xG

Fig. 2.9: Gripper coordinate system for the robot in figure 2.7.

To find the transformation we use a two-step process. The process

resembles the computation of the drill tip matrix (see above). How-
ever, we will need an additional step. We introduce an intermediate
2.3 Linkages 43

coordinate system S1 , which moves with the robot as it rotates (fig-

ure 2.10). Note again that the base system will always stay fixed. The
origin of this intermediate system S1 remains at the origin of the base
system, but otherwise S1 rotates with the robot.

zS1
yS1
xS1

Fig. 2.10: Intermediate coordinate system S1 for the robot in figure 2.7.
Notice that the origin of S1 coincides with the origin of B. S1 rotates with
the robot, while B remains fixed.

The gripper system is fixed at a point in the center between the two
gripper jaws. This point is called the tool center point, and depends on
the tool we use, e.g. a saw, a drill, or a gripper.
Throughout this book, we will often use the term effector instead of
gripper. We can replace the gripper by a saw or a drill, or some other
tool. An effector can thus be any tool mounted to the end of a kin-
ematic chain.
The placement of the system S1 , as the robot moves, is shown in fig-
ure 2.11.

zB , zS1
yB
xS1
xB
θ

Fig. 2.11: The intermediate coordinate system S1 rotates with the robot,
(angle θ ). The two z-axes (of B and S1 ) coincide. The y-axis of S1 is not
shown.
44 2 Describing Spatial Position and Orientation

We now look at the scene from above: the placement of S1 within the
base system is shown in figure 2.12:

yB

yS1 xS1

θ sin θ
cos θ xB

Fig. 2.12: Placement of intermediate coordinate system S1 with respect

to the base coordinate system, depending on the angle θ of rotation of
the robot in figure 2.7

We consider the vector xS1 in the figure. In terms of the base, this vec-
tor has the x-coordinate cos θ , and the y-coordinate sin θ . Likewise,
we see that the y-vector of S1 is given by
 
− sin θ
(2.13)
cos θ
Putting this into a 4 × 4-matrix as described above, we obtain the pos-
ition of the intermediate system S1 with respect to the base, depending
on the joint angle θ :
 
cos θ − sin θ 0 0
B
 sin θ cos θ 0 0
MS1 = 
 0
 (2.14)
0 1 0
0 0 01
We pause a minute to look at this matrix. Clearly, the z-vector should
not move as we rotate about it. And indeed it does not move, namely
the third column of the matrix simply shows the unchanged z-vector.
The first and second columns of the matrix show the motions of the x-
and y-vectors, depending on the angle θ .
This completes the first step of our computation. For the second step,
we find the transformation from S1 to the gripper. Since this trans-
formation is only a translation by the link length (called L), the matrix
is given by:
2.3 Linkages 45

 
100L
S1
0 1 0 0 
MG =  
0 0 1 0  (2.15)
0001
Thus the desired position of the gripper in terms of the base coordinate
system is given by the matrix product
 
cos θ − sin θ 0 L cos θ
B
 sin θ cos θ 0 L sin θ 
MS1 · S1 MG = 
 0
 (2.16)
0 1 0 
0 0 0 1
Note that this matrix contains the angle θ . It expresses the location
of the gripper coordinate system, depending on the joint angle of the
robot. We have completed the forward kinematic analysis in a simple
example. In general, the goal of forward analysis is to find the location
of the effector, given values for each joint angle.

(End of Example 2.3)

To summarize things, we have used three coordinate systems in our

analysis: the base B, the gripper G, and the intermediate system S1 .
Introducing the intermediate system was indeed the main step, and it
will be extremely useful when analyzing more complex robots.
The next example shows that we can repeat the same construction to
obtain transformations for n-joint robots.
But before we get to the next example, we again look at equation 2.14.
In this equation we have derived a matrix describing a simple rotation
about the z-axis. It is now straightforward to derive the two other ele-
mentary rotation matrices, namely matrices describing rotations about
x and y. Take a minute to verify the following matrices, and note the
placements of the minus-signs in the matrices.

   
1 0 0 0 cos θ 0 sin θ 0
0 cos θ − sin θ 0  0 1 0 0
R(x, θ ) = 
0 sin θ cos θ
 R(y, θ ) = 
− sin θ

0 0 cos θ 0
0 0 0 1 0 0 0 1
46 2 Describing Spatial Position and Orientation

 
cos θ − sin θ 00
 sin θ cos θ 0 0
R(z, θ ) = 
 0
 (2.17)
0 1 0
0 0 01
Even more straightforward is the derivation of the elementary transla-
tion matrices:
   
1 0 0 px 100 0
0 1 0 0  0 1 0 py 
T (px , 0, 0) = 
0 0 1 0 
 T (0, p y , 0) = 
0 0 1 0 


000 1 000 1
 
100 0
0 1 0 0 
T (0, 0, pz ) = 
0 0 1 p z 
 (2.18)
000 1

2.4 Three-Joint Robot

Suppose now we have a robot with three joints. Such a robot is shown
in figure 2.13. We call this robot the elbow manipulator. Our goal is
to express the position and orientation of the gripper with respect to
the base coordinate system. We place a coordinate system into each
link of the robot. We obtain a chain of coordinate systems, leading
from the base system to the gripper system in several steps. Notice
that this robot has only three joints, and typical robots have six joints.
However, all computations for the three-joint robot can later be used
for more complex robots.
The three joint angles are called θ1 , θ2 , θ3 . The position shown in the
figure is the null position of the robot, meaning θ1 = θ2 = θ3 = 0.
As above, we define intermediate coordinate systems S0 = (x0 , y0 , z0 )
and Si = (xi , yi , zi ), i = 1, . . . , 3 (see figure 2.14).
Think of the first system B = S0 = (x0 , y0 , z0 ) as the base system. It
is attached to the table, and does not move, although it rests with its
origin at the upper tip of the first link. This may seem odd, but we do
2.4 Three-Joint Robot 47

Fig. 2.13: Three-joint robot (elbow manipulator). All three joints are
revolute joints

zG
yG
z0 z3
xG
y0 y3
d4
x0 x3
a2
z1 z2
x1 x2

y1 y2

Fig. 2.14: Joint coordinate systems for transforming between base sys-
tem S0 = (x0 , y0 , z0 ) via Si = (xi , yi , zi ) to the gripper coordinate system
SG = (xG , yG , zG ).
48 2 Describing Spatial Position and Orientation

this to simplify the overall matrix product, i.e. we thus do not need an
extra transformation leading from the table to the tip of the first link.
Our goal is to derive a matrix which transforms from the base co-
ordinate system B = S0 to the gripper system SG . Having defined the
intermediate matrices Si , we can partition this goal into a series of
small steps. The first step is the transformation from S0 to S1 . We will
do this next.
The rotation axis of the first joint is the axis z0 . To get from S0 to S1 ,
we must take into account any rotations about this first axis. As in the
case of our single joint robot in the above example, the matrix R(z, θ )
in equation 2.17 describes this rotation. But now we are not done,
since we have not reached S1 yet. To do so, we must rotate about the
x0 -axis by the fixed angle of -90 degrees (see figure 2.14). Overall, the
following matrix product gives the transformation from S0 to S1 .

  
cos θ1 − sin θ1 0 0 1 0 00
S0
 sin θ1 cos θ1 0 0 0 0 1 0
MS1 = R(z, θ1 )R(x, −90) =  0
 
0 1 0 0 −1 0 0
0 0 01 0 0 01
(2.19)
Hence (simplifying the notation S0 MS1 to 0 M1 ):
 
cos θ1 0 − sin θ1 0
0
 sin θ1 0 cos θ1 0
M1 = 
 0 −1
 (2.20)
0 0
0 0 0 1
Similarly (denoting the constant link lengths by a2 and d4 ) we obtain:
 
cos θ2 − sin θ2 0 a2 cos θ2
1
 sin θ2 cos θ2 0 a2 sin θ2 
M2 =  0
 (2.21)
0 1 0 
0 0 0 1
 
cos θ3 0 sin θ3 0
2
 sin θ3 0 − cos θ3 0
M3 =  0 1
 (2.22)
0 0
0 0 0 1
2.5 Standardizing Kinematic Analysis 49
 
100 0
3
0 1 0 0 
MG =  
0 0 1 d 4  (2.23)
000 1
Multiplying, we have (abbreviating s1 for sin θ1 etc.) :

0
M3 =0 M1 · 1 M2 · 2 M3 (2.24)
 
c1 (c2 c3 − s2 s3 ) −s1 c1 (c2 s3 + s2 c3 ) a2 c1 c2
s1 (c2 c3 − s2 s3 ) c1 s1 (c2 s3 + s2 c3 ) a2 s1 c2 
  (2.25)
 −s2 c3 − c2 s3 0 −s2 s3 + c2 c3 −a2 s2 
0 0 0 1
The last equation can be simplified with the trigonometric addition
formulas. Therefore (setting s12 for sin(θ1 + θ2 ), etc.):
 
c1 c23 −s1 c1 s23 a2 c1 c2
0
s1 c23 c1 s1 s23 a2 s1 c2 
M3 =   −s23 0 c23 −a2 s2 
 (2.26)
0 0 0 1
For the final result, we must multiply with the constant transformation
3 M , given above, which transforms from S to the gripper coordinate
G 3
system:
 
c1 c23 −s1 c1 s23 a2 c1 c2 + d4 c1 s23
0
s1 c23 c1 s1 s23 a2 s1 c2 + d4 s1 s23 
MG =   −s23 0 c23 −a2 s2 + d4 c23 
 (2.27)
0 0 0 1
You may wish to check equation 2.27 for various settings of the angles
θ1 , θ2 , θ3 (see also the exercises at the end of this chapter).

2.5 Standardizing Kinematic Analysis

In the previous section, we saw that kinematic equations for simple

robots can become quite long. Here we only looked at a three-joint
robot. Thus it is useful to simplify and standardize the notation, to
50 2 Describing Spatial Position and Orientation

make it as compact as possible. This is what was done in a paper by

Denavit and Hartenberg from 1955 [1]. We have already seen an ex-
ample for such a simplification: We placed the base coordinate system
at the tip of the first link in our analysis of the three-joint robot (fig-
ure 2.14), to reduce the number of elementary matrices in the matrix
product.
If we take a step back and look at the placement of the other coordin-
ate systems in the example, we see that all z-axes (in figure 2.14) coin-
cide with the rotation axes of the joints. But there is more that can be
done to standardize the placement of coordinate systems. Notice that
the transformation between two arbitrary coordinate systems in space
can require as many as six elementary transformations (rotations and
translations). The main idea in [1] is that four elementary transforma-
tions will suffice, if we place the coordinate systems in an appropriate
way. We will explain this idea in a little more detail below, but let
us first look at the rules for placing the coordinate systems. Place the
systems such that
1. B = S0 is the fixed base system.
2. The axis zi is the rotation axis of the joint with index (i + 1).
3. The axis xi is orthogonal to the axis zi−1 , and also orthogonal to zi .
By convention, the pointing direction of xi is from zi−1 towards zi .
4. Place yi in such a way that Si is a right-handed coordinate system
(see also exercise 2.2d).
These four rules are called DH-rules. You may wish to take a minute
to verify that we were consistent with these rules in our example for
the three-joint robot. For example, from rule 2, we see that z0 is the
axis of the first joint, as in our example.
Placing axes is not all. We also need to place the origin of each system.
Let Oi denote the origin of Si . In general, the two axes zi−1 and zi are
neither parallel, nor do they coincide or intersect. The DH-rules hold
for any general robot, so that we assume zi−1 and zi are on skew lines.
We connect the two lines by their joint normal, and put the origin Oi
on the intersection point of zi and this normal (figure 2.15).
Our goal is now to derive a compact transformation matrix for trans-
forming from Si−1 to Si . We will see that the above rules will lead to
a comparatively small matrix.
2.5 Standardizing Kinematic Analysis 51

Oi zi

Joint normal
zi−1

Fig. 2.15: Placement of the origin Oi of system Si .

We noted above that the main observation in the original paper by De-
navit and Hartenberg [1] is that four elementary transformations will
always suffice for transforming from Si−1 to Si . Notice that we must
follow the rules when placing Si−1 and Si . The four specific trans-
formations to be given now will not suffice, if you did not follow the
rules. (To understand why, you should check that the rules amount to
‘removing’ two degrees of freedom.)
Denavit and Hartenberg show that the following elementary transla-
tions/rotations always suffice:
1. Translate along zi−1 by an amount di
2. Rotate about zi−1 by θi
3. Translate along xi by ai
4. Rotate about xi by αi
In our example, the values di , ai , αi are constant, while θi is the (vari-
able) joint angle. We will hence call θi the joint variable. We have
given names to the elementary transformations, and we have written
them as matrices in equations 2.17 and 2.18. Thus the transformations
can be written as follows:

T (0, 0, di )
R(z, θi )
T (ai , 0, 0)
R(x, αi ) (2.28)
52 2 Describing Spatial Position and Orientation

Now we are not far from our goal. The matrix

i−1
Mi = T (0, 0, di )R(z, θi )T (ai , 0, 0)R(x, αi ) (2.29)
transforms from Si−1 to Si .
With equations 2.17 and 2.18, we can write

    
100 0 cθi −sθi 00 1 0 0 ai 1 0 0 0
  
0 1 0 0  sθi cθi 0 0   
i−1
Mi =   0 1 0 0  0 cαi −sαi 0
0 0 1 d i   0 0 1 0 0 0 1 0  0 sαi cαi 0
000 1 0 0 01 000 1 0 0 0 1
(2.30)
Finally, we obtain
 
cθi −cαi sθi sαi sθi ai cθi
i−1
 sθi cαi cθi −sαi cθi ai sθi 
Mi = 
 0 sαi
. (2.31)
cαi di 
0 0 0 1
We see that the matrix describing a robot with six joints is a product
of six matrices as in equation 2.31. Although we have taken care to re-
duce the number of elementary transformations, the matrix product is
not simple. However, having set up the rules for the placement of co-
ordinate systems, we can arrive at a yet more compact description of a
robot. We simply set up a table containing the parameters αi , ai , di , θi .
Each line in the table corresponds to one joint, and we obtain a table
of the form in table 2.1.

i αi ai di θi
1 α1 a1 d1 θ1
2 α2 a2 d2 θ2
.. .. .. .. ..
. . . . .

Table 2.1: DH-table

The tables for defining kinematic constructions, as in table 2.1, can be

applied to our three-joint robot. We obtain table 2.2.
2.6 Computing Joint Angles 53
i αi ai di θi
1 −90 0 0 θ1
2 0 a2 0 θ2
3 90 0 0 θ3
G 0 0 d4 0

Table 2.2: DH-table for the three-joint robot

It should be noted that a number of special cases can arise when ap-
plying the DH-rules. For example, the axes zi−1 and zi can intersect
or be parallel, or even coincide. In all such cases, the rules can be re-
laxed, and we obtain two or more equivalent ways for fixing the axis
xi or the origin Oi .

2.6 Computing Joint Angles

We saw that we can describe the position of the gripper, given angles
θ1 , θ2 .... We now wish to do the opposite. Namely, given a desired
position for the gripper, how do we choose the angles? In general, this
is what we need to program a robot. The next example discusses a
simple case.

Example 2.4

We return to the one-joint robot in example 2.2. Here the problem

just stated is very simple. Fix a point on the table. Now, which angle
must we prescribe to reach that point? The situation is illustrated in
figure 2.16.
There is an easy answer to our question. We are given the target
point p = (px , py )T , and we wish to grasp this point with our grip-
per. Elementary mathematics will help us to find θ , given p. We have
cos θ = px and sin θ = py . (Here we have assumed that our robot link
has unit length, i.e. L = 1 in figure 2.16 and kpk = 1) We can solve
this for θ by dividing the two equations:
54 2 Describing Spatial Position and Orientation
 
px
p=
y py
L θ
x

Fig. 2.16: Calculating the joint angle θ for reaching a given point p

sin θ py
= (2.32)
cos θ px
p p
From this, we could conclude tan θ = pxy , and θ = arctan pxy .
But this simple solution has a drawback. By definition, the function
arctan returns values in the range (−π/2, π/2). Thus, it can never
return a value larger than π/2. By consequence, our robot can never
reach the point p shown in the figure, since its null position points
along the x-axis. In fact, the entire half-plane to the left of the y-axis
is lost for our robot.
There is a standard way to repair this. We extend the function arctan.
The function atan2 is defined on the basis on the arctan-function. This
function atan2 takes two arguments x and y, hence the name. It is
defined by distinguishing several cases for the values of the arguments
x and y.

atan2(y, x) = (2.33)
0 for x = 0, y = 0
π/2 for x = 0, y > 0
3 · π/2 for x = 0, y < 0
arctan(y/x) for x > 0, y ≥ 0
2π + arctan(y/x) for x > 0, y < 0
π + arctan(y/x) for x < 0, y ≥ 0
π + arctan(y/x) for x < 0, y < 0

With this definition, we obtain

θ = atan2(py , px ). (2.34)
2.7 Quaternions 55

You should verify that our one-joint robot can reach all positions on a
full circle with this definition.
A matrix equation as in equation 2.27 is called the forward kinematic
solution of the robot. The result in equation 2.34 shows a simple
example of an inverse kinematic solution. Thus the inverse solution
states the required joint angles necessary for reaching a given position
and orientation.
We will discuss the forward and inverse kinematics for medical link-
ages in more detail in chapter 3.

2.7 Quaternions

In a typical navigation scenario, we combine a robot arm with a track-

ing device. Most tracking systems are equipped with a pointer tool.
Then the tracking system works like this: we hold the pointer in a fixed
position in space. The tracking system outputs the coordinates of this
point. (It also outputs the spatial direction of the pointer.) When we
move the pointer, the system follows with a high sampling rate, which
can be as high as 4 kHz. Internally, most tracking systems do not work
with matrices or YPR-angles. Instead, they work with quaternions.
Thus, they will output a quaternion to the host computer, not a mat-
rix, while the robot usually works with matrices or YPR angles. With
quaternions we can represent orientations just as with 3 × 3-matrices
or YPR-angles.
A quaternion is a 4-vector of the form

a = (a1 , a2 , a3 , a4 )T (2.35)
We can add quaternions just like regular 4-vectors i.e.

a + b = (a1 + b1 , a2 + b2 , a3 + b3 , a4 + b4 )T (2.36)
We would like to define a multiplication for quaternions. One may
argue that we already have a multiplication, i.e. the standard scalar
product for two vectors. However, this is not what we need. We need
a multiplication for two quaternions such that the result is also a qua-
ternion, i.e. a 4-vector. The cross-product for 3-vectors could be a can-
56 2 Describing Spatial Position and Orientation

didate, but it is only defined for 3-vectors. Here we define for two
quaternions a, b:
 
a1 b1 − a2 b2 − a3 b3 − a4 b4
a1 b2 + a2 b1 + a3 b4 − a4 b3 
ab = 
a1 b3 − a2 b4 + a3 b1 + a4 b2 
 (2.37)
a1 b4 + a2 b3 − a3 b2 + a4 b1
This is certainly not easy to memorize. To simplify memorizing the
formula in equation 2.37, quaternions can be written in a slightly dif-
ferent, but equivalent way.
Instead of a = (a1 , a2 , a3 , a4 )T , we write

a = a1 + a2 i + a3 j + a4 k (2.38)
Note that i, j and k are symbols, not numbers, and we have a table
stating symbolic product rules for i, j and k. The symbolic rules are

ii = −1 j j = −1 kk = −1
ij = k jk = i ki = j (2.39)
ji = −k k j = −i ik = − j

Now we can multiply i, j and k just as they were numbers, and one can
easily verify that the product ab for quaternions defined in this way is
equivalent to this new definition.
The new rules are somewhat easier to memorize than the original
definition in equation 2.37, and they are reminiscent of the rules for
the positive/negative sense of rotation. For all purposes other than re-
membering the specific multiplication above, we will remain with our
first definition in equation 2.37. Hence, from now on, a quaternion is
simply a 4-vector.
Notice that in general ab 6= ba.
Just as for any 4-vector, we have a norm, namely
q
kak = a21 + a22 + a23 + a24 (2.40)
Unit quaternions are quaternions having norm 1. We represent rota-
tions in the following way. Given a 3-vector p = (px , py , pz )T . Set
2.7 Quaternions 57

p = (0, px , py , pz )T , and we have turned our 3-vector p into a qua-

ternion. For convenience, we will also call it p. We noted above that
quaternions are similar to matrices, and one similarity is that a qua-
ternion also has an inverse.
Namely, set

(a1 , −a2 , −a3 , −a4 )T

a−1 = , (2.41)
kak
for a = (a1 , a2 , a3 , a4 )T . (You should check that aa−1 really gives the
quaternion e = (1, 0, 0, 0)T .)
Now we can define the three-product apa−1 , and its result is a qua-
ternion q. Here, q will always have the form q = (0, q2 , q3 , q4 )T , i.e.
its first element will always be zero. Hence we can regard q as a 3-
vector. We interpret q as the vector obtained by rotating p under the
rotation defined by a.
Hence, to summarize

q = apa−1 (2.42)
defines quaternion rotation, where a is the (unit) quaternion by which
we rotate, and p is the object of rotation. Thus the quaternion a takes
the role of a 3 × 3-rotation matrix. Notice that a quaternion is much
more compact than a 3 × 3-matrix, which is why quaternions are also
often used in graphics.
To understand exactly which rotation is represented by a given unit
quaternion, we observe that for a = (a1 , a2 , a3 , a4 )T , the component
a1 defines the angle, and the 3-vector (a2 , a3 , a4 )T defines the axis of
the rotation. Details are explained in the following example.

Example 2.5

Suppose we wish to rotate about the z-axis of the base system by the
angle θ = π. To select the axis of rotation, we set p = (0, 0, 1)T . Then
the quaternion
 
p T
a = cos π/2, sin π/2 (2.43)
kpk
58 2 Describing Spatial Position and Orientation

describes this rotation. In our case p is already normalized, so that

a = (0, 0, 0, 1)T describes the desired rotation. Equation 2.43 describes
the general case, i.e. how to get from an angle and an axis to the cor-
responding quaternion.

(End of Example 2.5)

We will further discuss quaternions in the exercises for this chapter.

Specifically, we will need a method for converting the different rep-
resentations (angles, matrices, quaternions). This will be done in ex-
ercise 2.7.

Exercises

Exercise 2.1 Matrix Transformation

In equation 2.11, we added a fourth component with the constant value

zero to the vector q. Discuss the effect of adding a fourth component
with value 1 instead. With this result, provide an interpretation for the
last row of a homogeneuos 4 × 4-matrix.

Exercise 2.2 Coordinate Systems

Imaging directions for medical imaging are specified in reference to

the human body. Figure 1.15 shows the conventions.

a) Define a base coordinate system B as in figure 2.1. Derive YPR

angle values for the three imaging directions in figure 1.15. In each
case, the z-axis should be orthogonal to the image plane.
b) Derive the same orientations as in part a), but now according to the
conventions for matrices.
c) Assume the five joints of a C-arm are given as in figure 1.10. De-
rive joint angle values for J1 , ..., J5 corresponding to the orienta-
tions in figure 1.15, (ignoring the issues of mechanical reachabil-
ity). The angle values should refer to the null position of the C-arm
shown in figure 1.10.
2.7 Quaternions 59

d) Typical coordinate systems in engineering are so-called right-

handed coordinate systems. However, medical imaging often re-
lies on left-handed coordinate systems. The base coordinate sys-
tem in figure 2.1 is a right-handed system. This means: x-axis of
the system = thumb of your right hand, y-axis = index finger right
hand, z-axis = middle finger right hand. Derive a mathematical
test for deciding whether three given orthogonal unit vectors form
a right-handed system or a left-handed system.

Exercise 2.3 Orthogonal matrices and the inverse of a homogen-

eous 4 × 4 matrix

The 3 × 3 matrix  
nx ox ax
R = ny oy ay  (2.44)
nz oz az
is called orthogonal if the product RRT is the 3 × 3 unit matrix. Here
RT denotes the transpose of R. The upper left 3 × 3 matrix in a ho-
mogeneous matrix must always be orthogonal, since the first three
columns in the homogeneous matrix represent vectors in an ortho-
gonal coordinate system.
a) Show that for an orthogonal matrix R as in equation 2.44, we have
RxRx = xx for any 3-vector x. Interpret this observation.
b) From the definition of an orthogonal matrix, we see that the inverse
of an orthogonal matrix is its transpose, i.e. RT = R−1 . Based on
this, derive the inverse of a homogeneous 4 × 4 matrix.
c) Show that RxRy = xy for any 3-vectors x, y, if R is orthogonal.

Exercise 2.4 Linkages

a) Verify the result in equation 2.16 for θ = 0 and θ = π/2.

b) Verify the result in equation 2.27 for the settings θ1 = 0, θ2 =
0, θ3 = 0, and θ1 = π/2, θ2 = 0, θ3 = −π/2, and θ1 = 0, θ2 =
−π/2, θ3 = π/2.
60 2 Describing Spatial Position and Orientation

Exercise 2.5 Angle/matrix conversion

a) Given the three angles α, β , γ (defined as YPR-angles), find the

3 × 3 rotation matrix  
nx ox ax
ny oy ay  (2.45)
nz oz az
corresponding to these angles.
b) Given the rotation matrix in equation 2.45, extract the YPR-angles
from that matrix.
Hint: The YPR angles all refer to rotations in the base coordinate
system! In this case, we multiply the corresponding matrices from
right to left, i.e. we start from the matrix equation
 
nx ox ax
R(z, γ)R(y, β )R(x, α) = ny oy ay  (2.46)
nz oz az
Here, the x-rotation is the first rotation, followed by the y-rotation,
then the z-rotation.
Now insert the elementary rotation matrices from equation 2.17,
and evaluate the matrix product on the left side of equation 2.46.
You will then have nine equations containing the three angles α,
β , and γ.
To solve for the angles in the resulting set of equations, you may
find the procedure in the derivation of the function atan2 helpful.
c) Implement the conversions from part a) and part b) of this exer-
cise, and test your implementation for appropriate angle ranges.
Comment on the case β = ±π/2.

Exercise 2.6 Euler angles

Euler angles are an alternative to YPR-angles. We again define three

angles α, β and γ. But here we choose the axes of rotation in the fol-
lowing way: Now α is a rotation about the z-axis of the base system,
β is a rotation about the new y-axis, and γ is a rotation about the new
z-axis. You may wonder why one would rotate about the same axis
2.7 Quaternions 61

(z) twice. But notice that now our rotations do not refer to the axes
of the base system, but in each case we rotate about the new axes. In
this case (as opposed to the YPR angles in the previous exercise), we
multiply the matrices from left to right.
a) Given base and saw coordinate systems as in figure 2.3, derive the
Euler angles for the saw coordinate system.
Compare to the YPR angles for the same systems.
b) Derive and implement a conversion between 3×3 rotation matrices
and Euler angles. The derivation closely follows exercise 2.4, with
one difference: you will have to start from
 
nx ox ax
R(z, α)R(y, β )R(z, γ) = ny oy ay  (2.47)
nz oz az
Notice here that the matrices appear in reverse order from what
you would expect! This corresponds to the fact that now we rotate
about the new axes each time.
Notice also that some text books define Euler angles with the angle
conventions z, x, z instead of z, y, z. In fact there are 12 different
ways to define the Euler angles, all equivalent.

Exercise 2.7 Quaternions

a) Verify the rotation results in example 2.4 for the input vectors
(1, 0, 0)T and (0, 0, 1)T .
b) Derive a conversion from a quaternion to a 3 × 3 rotation matrix.
Hint: If u = (ux , uy , uz )T is a given axis, and θ is a given angle, we
set c = cos θ and s = sin θ .
Then the 3 × 3 matrix

 
c + u2x (1 − c) ux uy (1 − c) − uz s ux uz (1 − c) + uy s
R = uy ux (1 − c) + uz s c + u2y (1 − c) uy uz (1 − c) − ux s
uz ux (1 − c) − uy s uz uy (1 − c) + ux s c + u2z (1 − c)
(2.48)
describes a rotation about u by θ . Given a quaternion q, extract
the axis u and the angle θ , and insert into the definition for R.
62 2 Describing Spatial Position and Orientation

c) Derive a conversion from a 3 × 3 rotation matrix to a quaternion.

Hint: Start from the equation
 
nx ox ax
ny oy ay  = R (2.49)
nz oz az
where R is the angle-axis matrix defined in equation 2.48.
Solve this matrix equation for the angle θ in c = cos θ and s =
sin θ , and then for the axis (ux , uy , uz )T .

Exercise 2.8 Six-joint robot

a) The robot in figure 2.17 is very similar to the three-joint robot in

figure 2.13, and also has three revolute joints. The only difference
is that the last joint has a different axis of rotation. Set up the co-
ordinate systems and the DH-matrix for this robot.

Fig. 2.17: Variant of the three-joint robot in figure 2.13

Fig. 2.18: Hand assembly with three revolute joints.

2.7 Quaternions 63

b) Replace the gripper of the elbow manipulator in figure 2.13 by a

tool flange, i.e. a disk for mounting tools. Assume now a small
copy of the robot in figure 2.17 was mounted to this tool flange.
This results in a six-joint robot. This six-joint robot has the mech-
anical structure of standard commercial robots (KUKA, Mitsubishi,
Kawasaki, GM Fanuc, Adept and Stäubli robots). The transform-
ation matrix expressing the position of the full six joint robot can
now be obtained in a very simple way. We simply multiply the two
transformation matrices. Set up this matrix for the six-joint robot
and verify your result for a small set of angles.
c) The robot in figure 2.18 is the wrist of the six-joint robot in b).
The three joint axes of the wrist intersect in a single point q. The
tool center point (TCP) is the center of the gripper. As we move
the joints of the wrist, we see that the TCP remains on a sphere.
For this reason, we call this wrist a spherical wrist. Following the
YPR-convention for angles, joints 1 and 3 of the wrist will be
called roll-joints (R-joints) and joint 2 is called a pitch joint (P-
joint). Thus the wrist has an R − P − R structure. Discuss the al-
ternatives for three-joint wrists with intersecting axes, following
this R − P alphabet.

Exercise 2.9 Kinematics of the Neurosurgical Frame

Derive the forward equation (equation 1.3) for the neurosurgical frame
in figure 1.13.

Summary

A homogeneous 4 × 4 matrix describes the position and orientation of

a coordinate system S with respect to the base system B.
According to the YPR-convention, the position and orientation of S
with respect to B is represented by a 6-tuple (equation 2.6).
To analyze linkages and jointed mechanisms, we distinguish between
forward and inverse kinematics. Forward kinematic analysis is the
process of finding the matrix, depending on the joint angle values,
which describes the position of the gripper. Inverse kinematics finds
64 2 Describing Spatial Position and Orientation

the joint angles for a given position and orientation. Inverse kinemat-
ics typically requires a forward analysis. Stated in simpler terms (with
the notation in figure 2.16), forward kinematics finds the position and
orientation of the effector, given the joint angles, while inverse kin-
ematics finds the joint angles given the target position and orientation.
For the kinematic analysis of a single-joint robot, we introduced three
coordinate systems, namely the base system, gripper system and an
intermediate system. The coordinate systems thus form a chain. We
assign a matrix to each pair of consecutive systems in this chain. Then
the product matrix gives the transformation from the base to the grip-
per. The analysis is very similar for robots with several joints.
The atan2-function is an extension of the arctan function. It takes two
arguments and implements the inverse kinematics for a robot with
a single revolute joint. More complex robots can be analyzed with
this function as well, again on the basis of a forward analysis. Fur-
thermore, the atan2-function is a general tool for solving kinematic
equations.
Quaternions are 4-vectors. To add two quaternions, we simply add
their components. The multiplication of two quaternions resembles
the cross-product for 3-vectors. Quaternion multiplication is not com-
mutative. The rotation defined by a quaternion is derived from equa-
tion 2.42 and equation 2.43. If a = [a1 , a2 , a3 , a4 ]T is a quaternion, then
the first element a1 describes the angle of rotation, while [a2 , a3 , a4 ]T
describe the axis. To convert from a (unit) quaternion to a 3x3-rotation
matrix, we must set up a rotation matrix given an angle and an axis
(exercise 2.7b). The reverse direction of the conversion consists of
extracting an angle and an axis from a rotation matrix (exercise 2.7c).

Notes

The DH-convention in the form discussed above is also called the

standard DH-convention (see also [1–4]). The text book by John Craig
[5] uses a modifed version, referred to as the modified DH-notation.
Given an unknown DH-table, you should make sure to find out which
of the two conventions it refers to.
References 65

References

[1] J. Denavit and R. S. Hartenberg, “A kinematic notation for lower-

pair mechanisms based on matrices.” Transactions of the ASME:
Journal of Applied Mechanics, vol. 22, no. 2, pp. 215–221, 1955.
[2] M. W. Spong, S. Hutchinson, and M. Vidyasagar, Robot Modeling
and Control, 1st ed. New York: John Wiley & Sons, Inc., 2005.
[3] P. Corke, Robotics, Vision and Control: Fundamental Algorithms
in MATLAB, ser. Springer Tracts in Advanced Robotics. New
York, Berlin, Heidelberg: Springer Science, Sep. 2011, vol. 73.
[4] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics:
Modelling, Planning and Control, ser. Advanced Textbooks in
Control and Signal Processing. New York, Berlin, Heidelberg:
Springer, Aug. 2009.
[5] J. J. Craig, Introduction to Robotics: Mechanics and Control,
3rd ed. Prentice Hall, 2005.
Chapter 3
Robot Kinematics

We saw that forward kinematics gives us the position (and orienta-

tion) of the gripper, when we have the joint angles. Inverse kinemat-
ics does the opposite, namely, we get the joint angles for reaching a
given position. In this chapter, we will discuss inverse kinematics. Be-
fore we look at dedicated medical devices, we will start with a simple
example, namely the three-joint robot from chapter 2. This example
shows how we can get from the forward analysis to the inverse ana-
lysis, and illustrates some basic methods.

3.1 Three-joint robot

We start from the forward matrix 0 MG of the three-joint robot in equa-

tion 2.27. We set up the matrix product 0 MG from the matrices derived
in equations 2.20-2.23.
 
nx ox ax px
0
ny oy ay py 
MG = 0 M1 · 1 M2 · 2 M3 · 3 MG =  
nz oz az pz  = T (3.1)
0 0 0 1
Here, the matrix on the right hand side is the target matrix, and we
denote it by T for short. Evaluating the matrix expression 0 M1 · 1 M2 ·
2 M · 3 M , we must solve the equation
3 G

67
68 3 Robot Kinematics
   
c1 c23 −s1 c1 s23 a2 c1 c2 + d4 c1 s23 nx ox ax px
s1 c23 c1 s1 s23 a2 s1 c2 + d4 s1 s23  ny oy ay py 
    (3.2)
 −s23 0 c23 −a2 s2 + d4 c23  = nz oz az pz 
0 0 0 1 0 0 0 1

Recall also that we set ci = cos θi , si = sin θi , ci j = cos(θi + θ j ) and

si j = sin(θi + θ j ), so that the above matrix equation becomes an equa-
tion in the variables θi . Our goal is to solve for these variables, given
constants for the values nx , ..., pz on the right side.
To solve this equation, first note that we can directly solve for θ1 .
From the second entry of the first matrix line, we obtain

− s1 = ox (3.3)
From the second entry of the second matrix line, we obtain

c1 = oy (3.4)
Again, we use our function atan2, which we have already applied in
several examples and exercises in chapter 2.
Set

θ1 = atan2(−ox , oy ) (3.5)
Once we have solved for θ1 , the value for θ1 becomes a constant.
There is an easy way to move θ1 from the left side of the equation to
the right side. We multiply both sides of the matrix equation with the
matrix 0 M−11 (see equation 2.20). This ensures that all variables are on
the left side, and the matrix on the left side is simplified considerably.
To further simplify the expressions, we remove the constant matrix
3 M−1 from the left side of the equation.
G
We thus transform the equation from which we started (see equa-
tion 3.1)

0
M1 · 1 M2 · 2 M3 · 3 MG = T (3.6)
into

1
M2 · 2 M3 = 0 M−1 3 −1
1 T · MG . (3.7)
We have
3.1 Three-joint robot 69
 
100 0
3 −1
0 1 0 0 
MG =  
0 0 1 −d4  (3.8)
000 1
and
 
nx ox ax −ax d4 + px
ny oy ay −ay d4 + py 
T · 3 M−1 
G = n o a −a d + p 
 (3.9)
z z z z 4 z
0 0 0 1
and set
   
nx ox ax −ax d4 + px nx ox ax p0x
ny oy ay −ay d4 + py  ny oy ay p0y 
    = T0 . (3.10)
nz oz az −az d4 + pz  = nz oz az p0z 
0 0 0 1 0 0 0 1
Now

0 M−1 · T · 3 M−1 = 0 M−1

1 ·T =
0
1 G
 
nx c1 + ny s1 ox c1 + oy s1 ax c1 + ay s1 p0x c1 + p0y s1
 −nz −oz −az −p0z 
 
−nx s1 + ny c1 −ox s1 + oy c1 −ax s1 + ay c1 −px s1 + py c1 (3.11)
0 0

0 0 0 1

We evaluate the matrix 1 M2 · 2 M3 on the left side to

 
c23 0 s23 a2 c2
1
s23 0 −c23 a2 s2 
M2 · 2 M3 = 0 1 0
 (3.12)
0 
0 0 0 1
We thus have all the ingredients of equation 3.7, i.e. we have evaluated
both sides of this equation.
From the last column of this equation, we obtain

a2 s2 = −p0z
a2 c2 = p0x c1 + p0y s1 (3.13)
70 3 Robot Kinematics

Thus
 
−p0z p0x c1 + p0y s1
θ2 = atan2 , , (3.14)
a2 a2
where

p0x = −ax d4 + px ,
p0y = −ay d4 + py ,
p0z = −az d4 + pz . (3.15)

Finally, from the matrix equation 3.2, we have

cos(θ2 + θ3 ) = az
sin(θ2 + θ3 ) = −nz (3.16)

so that

θ3 = atan2(−nz , az ) − θ2 (3.17)
This completes our inverse analysis of the three-joint robot.
Before we look into the analysis of more complex robots, we briefly
summarize the overall procedure in the analysis of our three-joint ro-
bot for future reference: we first set up the forward matrix equation.
In this equation, we found a pair of equations of the form si = u, and
ci = v, with constants u, v. We solved for the i-th joint angle on the
basis of the atan2-function. Then we moved the matrix i−1 Mi to the
right-hand side of the equation, since it no longer contained any un-
solved variables. To find an initial pair of equations (of the form si = u,
and ci = v), we could be somewhat more systematic. We could gen-
erate several matrix equations by moving matrices to the right side,
while looking for appropriate pairs of equations. We will call this
simple technique equation-searching.
In the next section, we will analyze a six-joint robot. We will see that
this robot can be regarded as an assembly of two three-joint robots in
our analysis, i.e. we can decouple the analysis for joints 1-3 and joints
4-6.
3.2 Six-joint robot 71

3.2 Six-joint robot

Figure 3.1 shows a six-joint robot. This six-joint robot can be obtained
by assembling two three-joint robots. If we take a closer look at the
kinematic structure, we see that the base (first three joints) is identical
to the three-joint elbow manipulator discussed above. We mount a
spherical wrist (with three joints) to the base. Figure 3.2 shows the res-
ulting kinematic structure, an elbow manipulator with spherical wrist
(see also exercise 2.8).

Joint 6
Joint 5
Joint 4

Joint 2
Joint 3
Joint 1

Fig. 3.1: Six-joint robot.

Forward Analysis

For the forward analysis of the six-joint robot, we must assign co-
ordinate systems to the joints. We have already derived the placement
of the coordinate systems S0 , S1 , S2 , S3 in figure 2.14.
Figure 3.3 adds the remaining coordinate systems for the hand as-
sembly (systems S4 , S5 , S6 ).
We obtain the following DH-table for the six-joint robot.
72 3 Robot Kinematics

z0
y0
x0

Fig. 3.2: Joint axes for the six-joint robot.

i αi ai di θi
1 −90 0 0 θ1
2 0 a2 0 θ2
3 90 0 0 θ3
4 −90 0 d4 θ4
5 90 0 0 θ5
6 0 0 d6 θ6
Since the hand assembly is also a (small) three joint robot, its analysis
should be simple. But can we decouple the kinematic analysis of the
two robots thus glued together? It is remarkable that this is indeed
possible.
The main idea for the inverse analysis is the following.
We have the target coordinate system S6 at the end of the hand as-
sembly (figure 3.3). We look at the z-vector of S6 . Now revert the
direction of this vector, to point into the opposite direction. We follow
this opposite direction until we reach the intersection point between
3.2 Six-joint robot 73

z6
y6
x6

z5

q y5
x5
z4
x4
y4
z0
z3
y0
x0 y3
x3

Fig. 3.3: Coordinate systems for the hand assembly of the six-joint robot.

the axes of joints five and six. Call this intersection point q, the wrist
center. q is also the origin of the coordinate systems S4 and S5 (see fig-
ure 3.3). Taking a second look at the kinematics of our robot, we see
that the axes for the last three joints intersect in one point. Thus q will
not move, when we move any of the axes four, five or six. This means
that the position of q only depends on the joints one, two and three.
It is important to note that the inverse analysis takes a homogeneous
4 × 4-matrix as an input, which gives the position and orientation of
the last coordinate system (tool system). In this matrix, we can find
the point q in the following way: we do have the gripper’s z-vector in
column three of the matrix (referred to as vector (ax , ay , az )T ) in the
matrix notation from equation 2.3). It is a unit vector. A vector point-
ing into the opposite direction is easy to find. Simply take the negat-
ive of the gripper’s z-vector, i.e. −(ax , ay , az )T . But now the distance
74 3 Robot Kinematics

between the origin of the gripper coordinate system and the point q
is a kinematic constant, and we can take it from our DH-table! Thus,
multiply the gripper’s z-vector by d6 , and subtract that from the vector
in column four of the matrix (also called vector p). Overall,

q = p − d6 a. (3.18)
All elements on the right side of this equation are known, and we
can thus compute q from the input matrix. So now we can find the
angles θ1 , θ2 , θ3 from the point q alone, by analyzing a three-joint
robot. After having found the angles θ1 , θ2 , θ3 , we rewrite the forward
matrix equation
 
nx ox ax px
0
ny oy ay py 
M1 · · · 5 M6 =  
nz oz az pz  (3.19)
0 0 0 1
to
 
nx ox ax px

3 M · 4 M · 5 M = 2 M−1 · 1 M−1 · 0 M−1 ny oy ay py 
4 5 6
 (3.20)
3 2 1 n o a pz 
z z z
0 0 0 1
This again is the kinematic equation for a three-joint robot (since
everything on the right hand side is now constant), and we have
already solved variants of such equations before.
This is the basic idea for solving the six-joint robot. However, some
subtleties are easy to overlook here: When we analyzed the three-joint
robot above, we had a full gripper matrix to start from. Now we only
have a single point, namely q. This will cause trouble, and we shall
see that we must work through the three-joint solution again, to adjust
to the new situation. The new solution will be much more detailed,
and will address the case of multiple solutions.
Specifically, look at the so-called left-shoulder and right-shoulder
configurations shown in figure 3.4.
Figure 3.5 shows two distinct configurations of the robot, where again
the resulting tool position is the same. We will call the configurations
in figure 3.5 the Elbow-up and the Elbow-down configurations.
3.2 Six-joint robot 75

Fig. 3.4: Left-shoulder and right-shoulder configurations of the robot.

The position of the tool flange with respect to the base plate is the same
in both cases.

Fig. 3.5: Elbow-up and elbow-down configuration of the robot. In both

cases, the robot is in left-shoulder configuration.

We can now combine the elbow-configurations and the shoulder-

configurations, and obtain four distinct ways for reaching a given fixed
tool placement. They are: (left, up), (left, down), (right, up), (right,
down). Similar to the configuration of the shoulder, we can configure
the hand of the robot. Thus, we can flip the hand, if we rotate joints
four, five and six appropriately (figure 3.6).
76 3 Robot Kinematics

A
x

y z
B
x
y
z
C

y
z
x
D
x

y z

Fig. 3.6: Hand configuration. First note that for configurations A and
D, the position of the hand coordinate system is the same. But the joint
angles are not the same! To see why, we move through three steps, i.e.
from A to B, then from B to C, finally from C to D. For A to B: rotate
joint J4 by 180 degrees. B to C: joint J5 rotates. C to D: rotate joint J6 ,
again by 180 degrees. Hence joints four five and six have all taken new
values.

Overall, we have three configuration parameters for the robot:

• Shoulder: (right, left) = (1,-1)
• Elbow: (up, down) = (1,-1)
• Hand: (noflip, flip) = (1,-1)
The configuration parameters are input parameters for our solution,
i.e. the user must specify the desired configuration in advance. Since
we have three parameters, we obtain 23 = 8 distinct configurations.
3.2 Six-joint robot 77

As noted above, the solution for the six-joint robot proceeds in two
steps. First we solve for the angles θ1 , θ2 , θ3 . Our sole input at this
stage will be the point q and the desired configuration. Notice that this
kinematic problem is slightly different from our above solution for the
three-joint robot (see example 3.1), where our input was a target 4x4-
matrix. Having found θ1 , θ2 , θ3 , we will then solve for the remaining
parameters via equation 3.20.
From equation 3.2 we see that q has the coordinates
   
qx a2 c1 c2 + d4 c1 s23
q = qy  =  a2 s1 c2 + d4 s1 s23  . (3.21)
qz −a2 s2 + d4 c23
But we have a second equation for q, given our target position as a
homogeneous 4 × 4 matrix T. Given T, we can take the two vectors
p and a from this matrix, and apply equation 3.18, i.e. q = p − d6 a.
Recall that d6 is a kinematic constant from the DH- table. Overall we
have:
 
a2 c1 c2 + d4 c1 s23
p − d6 a =  a2 s1 c2 + d4 s1 s23  (3.22)
−a2 s2 + d4 c23
Our next goal will thus be to extract θ1 , θ2 , θ3 from equation 3.22.

Solving for θ1

Our solution for θ1 is very simple, and we can apply the same tech-
nique as for the one-joint robot in chapter 2. We look at the robot from
above (figure 3.7).
Figure 3.8 further illustrates the situation. Projecting the entire con-
figuration onto the plane of the robot’s base plate (x-y-plane), we see
that the angle θ1 can be taken from the position of the point q directly.
After projecting q onto the x-y-plane, we see that θ1 can be computed
as

θ1 = atan2(qy , qx ) (3.23)
Equation 3.23 refers to one possible solution for θ1 . We can obtain
a second solution by changing the configuration of the shoulder (see
78 3 Robot Kinematics

Fig. 3.7: Solving for angle θ1 . The origin of the base system (O) and q
are marked (black dots).

y0 y0
q q

θ1 θ1 + π
O x0 O x0

a) b)

Fig. 3.8: Solutions for the angle θ1 . The robot has several ways for reach-
ing the given point q. Figure a shows the so-called left-shoulder config-
uration. To reach the right shoulder configuration (figure b) we move
the first joint to the angle θ1 + π, and then flip the second joint.

figure 3.8). The process of moving from a left-shoulder configuration

to a right-shoulder configuration resembles the process of flipping the
hand, illustrated in figure 3.6. The second solution for θ1 is given by

θ1 = atan2(qy , qx ) + π. (3.24)
From the definition of the atan2-function, we see that

atan2(qy , qx ) + π = atan2(−qy , −qx ). (3.25)

3.2 Six-joint robot 79

We will next solve for θ3 rather than for θ2 , since this will simplify
the solution path.

Solving for θ3

We take a step back and look at the robot. In figure 3.9 we see that
the distance between q and the origin of the base system should only
depend on the angle θ3 , but not on the joint angles θ1 and θ2 .

z0 x0 -z0 -plane
y0
x0
q

θ2 ||q|| θ3

Fig. 3.9: Solving for angle θ3 . The distance kqk between the origin of the
base system and the point q only depends on θ3 .

We take the square of the norm on both sides of equation 3.21, and
obtain:

kqk2 = q2x + q2y + q2z = (3.26)

(a2 c1 c2 + d4 c1 s23 )2 +
(a2 s1 c2 + d4 s1 s23 )2 +
(−a2 s2 + d4 c23 )2
With sin2 α + cos2 α = 1, the right hand side simplifies to

a22 + d42 + 2a2 d4 (c2 s23 − s2 c23 ). (3.27)

But this expression reduces to
80 3 Robot Kinematics

a22 + d42 + 2a2 d4 s3 ., (3.28)

and we see that θ2 has vanished from the expression. Thus, indeed (as
expected from the figure) our distance expression only depends on θ3 .
Now

kqk2 − a22 − d42

s3 = ,
2a2 d4
q
c3 = ± 1 − s23 ,
θ3 = atan2(s3 , c3 ). (3.29)

From the latter equation, we again obtain two solutions, controlling

the configuration of the robot.

Solving for θ2

To solve for θ2 , we return to equation 3.21. We have already solved

for θ1 and for θ3 , but it is still not immediately obvious how to isolate
the angle θ2 .
We assume cos(θ1 ) 6= 0. Then from equation 3.21 we have

x = a2 cos(θ2 ) + d4 sin(θ2 + θ3 ),
z = −a2 sin(θ2 ) + d4 cos(θ2 + θ3 ) (3.30)

where x = qx / cos(θ1 ) and z = qz .

In case cos(θ1 ) = 0, we can base the argument which will follow on

y = a2 cos(θ2 ) + d4 sin(θ2 + θ3 ),
z = −a2 sin(θ2 ) + d4 cos(θ2 + θ3 ) (3.31)

with y = qy / sin(θ1 ).
Furthermore, we set
3.2 Six-joint robot 81

u = a2 + d4 s3 ,
v = d4 c3 . (3.32)

Then we have

x = u cos(θ2 ) + v sin(θ2 ),
z = −u sin(θ2 ) + v cos(θ2 ). (3.33)

You may wish to check equation 3.33 by inserting the definitions for
u, v above into equation 3.33. The result should lead back to equa-
tion 3.30 via the trigonometric addition formulas.
Now we set
p
r = u2 + v2 . (3.34)
For r 6= 0 we set

γ = atan2(u/r, v/r). (3.35)

Notice from the definition of the atan2-function ,

γ = atan2(u, v). (3.36)

Then (again if r 6= 0)

u/r = sin(γ),
v/r = cos(γ) (3.37)

and thus

x = r sin(γ)c2 + r cos(γ)s2 ,
z = −r sin(γ)s2 + r cos(γ)c2 . (3.38)

By the trigonometric addition formulas this is the same as

82 3 Robot Kinematics

x = r sin(γ + θ2 ),
z = r cos(γ + θ2 ). (3.39)
Now we have

x/r = sin(γ + θ2 ),
z/r = cos(γ + θ2 ) (3.40)
or

γ + θ2 = atan2(x/r, z/r). (3.41)

From this, we see that

θ2 = atan2(x/r, z/r) − γ (3.42)

By the definition of γ, and the definition of the atan2-function, the
latter equation reduces to

θ2 = atan2(x, z) − atan2(u, v). (3.43)

Remark 3.1

A small detail has been overlooked in our derivation for θ2 . We must

be sure that the value r defined in equation 3.34 is not zero, since we
later divide by this value. A detailed discussion shows that r can only
be zero if a2 = d4 and sin(θ3 ) = 0. This is equivalent to q coinciding
with the origin, which is mechanically impossible for robots with a2 6=
d4 . (See also exercise 4 at the end of this chapter).

(End of Remark 3.1)

Remark 3.2

For both θ1 and θ3 we each have two solutions. This gives rise to a
total of four solutions, which correspond to the four configurations of
the robot obtained by choosing values ±1 for each of the two variables
shoulder and elbow.
3.2 Six-joint robot 83

For θ2 , we have a single solution, which will depend on the configur-

ations chosen via θ1 and θ3 .

(End of Remark 3.2)

In summary, we obtain the following values for θ1 , θ2 and θ3 .

θ1 = atan2(shoulder · qy , shoulder · qx ) (3.44)

If cos(θ1 ) 6= 0

θ2 = atan2(−shoulder · elbow · qx / cos(θ1 ), −shoulder · elbow · qz )

− atan2(−shoulder · elbow · u, −shoulder · elbow · v)
(3.45)
else

θ2 = atan2(−shoulder · elbow · qy / sin(θ1 ), −shoulder · elbow · qz )

− atan2(−shoulder · elbow · u, −shoulder · elbow · v)
(3.46)
where

u = a2 + d4 s3 (3.47)
and
v = d4 c3 . (3.48)
Then

kqk2 − a22 − d42

s3 = (3.49)
2a2 d4
and
q
θ3 = atan2(s3 , −shoulder · elbow · 1 − s23 ). (3.50)
We must now solve for the remaining angles θ4 , θ5 and θ6 . As we shall
see, this is much simpler than the case of θ1 , θ2 and θ3 . All we need
here is the technique which we called equation-searching.
84 3 Robot Kinematics

Solving for θ4 , θ5 and θ6

We start from equation 3.20. We rewrite this equation in a shorter form

to

3 −1
M6 = 0 M3 · T. (3.51)
Here again, T denotes our target matrix as in equation 3.1, i.e.
 
nx ox ax px
ny oy ay py 
T= 
nz oz az pz  . (3.52)
0 0 0 1
Let M denote the matrix on the right hand side of equation 3.51.
Notice that we can now assume that M is constant, since the angles
θ1 , θ2 , θ3 are the only variables on the right hand side of equation 3.51,
and we have already solved for these variables.
We evaluate both sides of equation 3.51, by multiplying the matrices.
First, we must compute 3 M6 :

 
c4 c5 c6 − s4 s6 −c4 c5 s6 − s4 c6 c4 s5 d6 c4 s5
3
s4 c5 c6 + c4 s6 −s4 c5 s6 + c4 c6 s4 s5 d6 s4 s5 
M6 = 
 −s5 c6
. (3.53)
s5 s6 c5 d4 + d6 c5 
0 0 0 1

Next, we compute 0 M−1

3 :
 
c23 c1 c23 s1 −s23 −a2 c3
0 −1
 −s1 c1 0 0 
M3 =  s23 c1 s23 s1 c23 −a2 s3 
 (3.54)
0 0 0 1

Finally, we obtain the first and second columns of 0 M−1

3 T as follows:
3.2 Six-joint robot 85

nx c23 c1 + ny c23 s1 − nz s23 ox c23 c1 + oy c23 s1 − oz s23

−nx s1 + ny c1 −ox s1 + oy c1
(3.55)
nx s23 c1 + ny s23 s1 + nz c23 ox s23 c1 + oy s23 s1 + oz c23
0 0

Columns three and four of the product 0 M−1

3 T are:

ax c23 c1 + ay c23 s1 − az s23 px c23 c1 + py c23 s1 − pz s23 − a2 c3

−ax s1 + ay c1 −px s1 + py c1
(3.56)
ax s23 c1 + ay s23 s1 + az c23 px s23 c1 + py s23 s1 + pz c23 − a2 s3
0 1

Looking at equation 3.53, we see that we have an equation for c5 from

the element (3,3) of this matrix. Specifically,

c5 = ax s23 c1 + ay s23 s1 + az c23 . (3.57)

For any angle θ , we have sin2 θ + cos2 θ = 1. Thus
p
sin θ5 = ± 1 − cos2 θ5 , (3.58)
and we again apply the atan2-function.
Set

v = ax s23 c1 + ay s23 s1 + az c23 (3.59)

and
p
u = ± 1 − v2 . (3.60)
Given the ±-sign in equation 3.60, we have two solutions:

θ5 = atan2(hand · u, v) (3.61)
Here the parameter ’hand’ represents the two solutions of equa-
tion 3.58, and the two possible configurations of the hand (see fig-
ure 3.6). As for the values ’shoulder’ and ’elbow’, ’hand’ is an input
value, to be specified by the user.
We return to equation 3.51.
86 3 Robot Kinematics

We compare the elements (3,1) and (3,2) in this matrix equation, and
conclude that

c4 s5 = m13 ,
s4 s5 = m23 ; (3.62)

or (assuming s5 6= 0), that

c4 = m13 /s5 ,
s4 = m23 /s5 . (3.63)

We consider three cases: s5 is zero, negative or positive.

We first discuss the cases s5 being strictly positive or strictly negative.
The latter two cases correspond to the two possible configurations of
the hand (see figure 3.6). Both cases are valid, and we can choose
either one.

θ4 = atan2(m23 , m13 ) (3.64)

or

θ4 = atan2(−m23 , −m13 ). (3.65)

This is similar to the case of θ1 , and we summarize it to

θ4 = atan2(hand · m23 , hand · m13 ). (3.66)

Recall that the input parameter ’hand’ takes the values ±1.
Using equation 3.55, we can resolve this in the following way:
Set

u4 = −ax s1 + ay c1 ,
v4 = ax c23 c1 + ay c23 s1 − az s23 . (3.67)

Then
3.2 Six-joint robot 87

θ4 = atan2(hand · u4 , hand · v4 ). (3.68)

The last remaining case is the case s5 = 0. It will imply θ5 = 0 since

positions θ5 = ±π are mechanically unreachable. From the mechan-
ical construction of the robot, we see that for θ5 = 0, the angles θ4
and θ6 can compensate for each other. This means that for s5 = 0, the
number of solutions for θ4 is infinite, and we can, for example, choose
θ4 = 0. Notice, however, that the case s5 = 0 represents an exceptional
case, and our choice of θ4 = 0 is arbitrary. Such exceptional cases do
occur in practice. For example the zero position of the robot is such a
case.
We will discuss the case s5 = 0 after having stated a solution for θ6
(see remark 3.3).

Solving for θ6

In our matrix in equation 3.53, we see that the same method which led
to the solution for θ4 can also give us θ6 . Specifically, the entries m31
and m32 of the matrix give us:

−s5 c6 = nx s23 c1 + ny s23 s1 + nz c23

s5 s6 = ox s23 c1 + oy s23 s1 + ox c23 (3.69)

Now, we can set

u6 = ox s23 c1 + oy s23 s1 + ox c23 ,

v6 = nx s23 c1 + ny s23 s1 + nz c23 (3.70)

and obtain:

θ6 = atan2(hand · u6 , −hand · v6 ). (3.71)

Note that we have not yet addressed the case s5 = 0.

88 3 Robot Kinematics

Remark 3.3

As noted above, an exception occurs whenever s5 = 0. This implies

that θ5 is zero or an integer multiple of π. From the construction of
the robot we see that in this case, joints 4 and 6 can compensate for
each other. Due to mechanical joint limits for joint 5, it suffices to look
at the case θ5 = 0. In this case, our matrix 3 M6 (see equation 3.53)
reduces to
 
c4 c6 − s4 s6 −c4 s6 − s4 c6 0 0
3
s4 c6 + c4 s6 −s4 s6 + c4 c6 0 0 
M6 = 
 (3.72)
0 0 1 d4 + d6 
0 0 0 1
or
 
c46 −s46 0 0
3
s46 c46 0 0 
M6 =  0 0 1 d4 + d6  .
 (3.73)
0 0 0 1
We conclude that

θ4 + θ6 = atan2(−nx s1 + ny c1 , nx c23 c1 + ny c23 s1 − nz s23 ). (3.74)

With other words, in typical situations, first choose an arbitrary value

for θ4 , (i.e. set θ4 = 0). Then θ6 can be computed from equation 3.74.
In practice, it is often important to discuss the choice of θ4 (under
s5 = 0, or s5 close to zero) in more detail, considering mechanical
joint limitations, and motion paths for the robot. Thus, θ4 = 0 may
not always be the best choice in equation 3.74.

(End of Remark 3.3)

We conclude that solutions for our six-joint robot are unique except
for cases with s5 = 0, as long as we provide input values for ’shoulder’,
’elbow’ and ’hand’. This completes the analysis of the six-joint robot.
3.3 Inverse Solution for the Seven-Joint DLR-Kuka Robot 89

3.3 Inverse Solution for the Seven-Joint DLR-Kuka

Robot

Surgical robots should be as small as possible, for two reasons. (1)

the work space in the operating room is often very limited. (2) smaller
robots typically have lower weight. Safety is an important issue, and
clearly, bigger robots can cause more damage.
However, if the robot is very small, the range of reachable points be-
comes small as well. Considering the range of reachable positions, it
quickly becomes clear that not so much the reachable positions count
here, but much more the positions reachable in arbitrary orientations.
Thus, points which can be reached in principle, but where the tool
cannot be reoriented freely are not useful.
The range of positions reachable in all orientations is surprisingly
small even for large six-joint industrial robots. Therefore, different
types of kinematic constructions have been designed. One such design
is a light-weight seven-joint serial robot.
Figure 3.10 shows the seven-joint DLR-Kuka robot. The new element
is a revolute joint between joints 2 and 3. Figure 3.11 shows the kin-
ematic structure. Following the yaw-pitch-roll-convention for angles
(see chapter 2), horizontal cylinders in the figure correspond to pitch
joints (P-joints). Vertical cylinders correspond to roll joints (R-joints).
As a result, we can describe the structure of the robot as an alternating
chain of Rs and Ps. The full kinematic chain is thus given by the string
RPRPRPR. We group this sequence into three parts, i.e. we spell it as

R − PRP − RPR. (3.75)

The robot’s wrist is the same as the wrist of the six-joint robot, i.e. a
spherical wrist.
The DH-table of the DLR-Kuka robot is shown in table 3.1:
The seven-joint robot in figure 3.10 can move the elbow in a way
similar to the human elbow. This elbow motion (for a fixed position
of the hand) is illustrated in figures 3.12- 3.14.
90 3 Robot Kinematics

Fig. 3.10: Seven-joint DLR-Kuka robot.

i αi ai di θi
1 -90 0 d1 θ1
2 90 0 0 θ2
3 -90 0 d3 θ3
4 90 0 0 θ4
5 -90 0 d5 θ5
6 90 0 0 θ6
7 0 0 d7 θ7

Table 3.1: DH-table for the DLR-Kuka robot (figure 3.10)

3.3 Inverse Solution for the Seven-Joint DLR-Kuka Robot 91

Fig. 3.11: Schematic of the seven-joint DLR-Kuka robot with R − PRP −

RPR structure.

Remark 3.4

The n angle values of a robot with n joints specify a point in an n-

dimensional space. This space is called the configuration space (or
joint space) of the robot. For our six-joint robot, the configuration
space is a 6D space, and we see that a given target position of the ef-
fector will correspond to single isolated points in configuration space.
This solution subspace typically consists of more than one point,
since we can vary the configurations of the robot (left-shoulder, right-
shoulder etc.). Nonetheless, for typical cases, the solution space will
still be a set of isolated points. For the seven-joint robot, this solution
subspace consists of curves rather than points in configuration space,
92 3 Robot Kinematics

due to the elbow motion (see also figure 3.12). Given a target matrix,
we call the curve corresponding to this target matrix the null-space of
the robot.

(End of Remark 3.4)

Fig. 3.12: The null-space of a seven-joint robot, for elbow-up and elbow-
down configurations.

J4
J3 J5

J2 J6 J7

J1 P = x0 -z0 -plane

Fig. 3.13: Elbow position redundancy of the seven-joint robot. Center

points of joints 2,4,6 are in the x0 -z0 -plane.
3.3 Inverse Solution for the Seven-Joint DLR-Kuka Robot 93

J4
J3 J5 J6 J7
J2

J1

Fig. 3.14: Elbow position redundancy of the seven-joint robot. Center

points of joints 2,4,6 are in a plane parallel to the x0 − y0 -plane.

Remark 3.5

In practice the angular ranges for revolute joints are limited. Typical
values are ±170 degrees for the R-joints and ±120 degrees for the
P-joints. Taking into account these limitations, the range of the elbow
angle (null-space in figure 3.12) is limited as well. It turns out that
the null-space is fragmented for some positions of the effector, i.e.
the null-space consists of several disconnected pieces. This effect is
illustrated in figure 3.15.

(End of Remark 3.5)

Inverse Solution
We begin with an informal overview of the solution path. We solve
for the first joint variable in much the same way as in the case of the
six-joint robot. That is, we look at the robot from above, and pro-
ject the wrist center point q onto the x-y-plane. As in the case of the
six-joint robot, we could then simply find the first joint angle as in
equation 3.44, i.e.

θ1 = atan2(shoulder · qy , shoulder · qx ). (3.76)

94 3 Robot Kinematics

500

0
500 500
0 0
-500 -500

Fig. 3.15: Null-space of the seven-joint robot, for an elbow-down config-

uration. Depending on the target matrix, the null-space can be fragmen-
ted, i.e. it consists of several disconnected intervals, given standard joint
ranges.

y0
q
∆

θ1
O x0

Fig. 3.16: Slack value ±∆ for the first joint of the seven-joint robot. The
value of ∆ controls the placement of the elbow.

But this will be not quite the solution for the first joint. Rather, we
allow for some extra ‘slack’ for choosing our first angle value. This is
illustrated in figure 3.16. The slack is denoted by ∆ .
To illustrate the solution path, we compare the seven-joint robot to
the six-joint robot under the R-P-notation introduced above. For the
six-joint robot, we had an RPPRPR structure. To find the inverse solu-
tion, we grouped the structure into two parts, namely RPP − RPR. The
robot’s wrist has an RPR-structure.
If we have assigned a value for the first joint variable (including some
slack value ∆ ), the remainder of the seven-joint robot is an PRP −
RPR structure. This is a six-joint robot, but different from the one we
analyzed above. The wrist is the same, but the robot’s base is a PRP-
3.3 Inverse Solution for the Seven-Joint DLR-Kuka Robot 95

structure. The main observation in the analysis of the seven-joint robot

is simple: we set up a matrix equation for a three-joint PRP-robot,
and extract the angles θ2 , θ3 , θ4 from this matrix equation. This will
be a straightforward exercise, very similar to the analysis of the base
for the six-joint robot. Finally, the analysis of the wrist can again be
decoupled and is the same as the analysis of the wrist for the six-joint
robot.
We will now look at the actual solution. We start from the target matrix
T, which is of course a homogeneous 4 × 4 matrix with
 
noap
T= (3.77)
0001
Set

q = p − d7 a (3.78)
where

q = (qx , qy , qz )T (3.79)
is again our wrist center point.

Solving for θ1

Set

θ1 = atan2(shoulder · qy , shoulder · qx ) + ∆ (3.80)

where ∆ ∈ [−π, π].

Solving for θ2 , θ3 , θ4

Let
 
1 5
M2 · ... · M6 (3.81)
[4]
96 3 Robot Kinematics

denote the fourth column of the matrix 1 M2 · ... · 5 M6 .

Since q is our wrist center, we have
   
0 5 q
M1 · ... · M6 = (3.82)
[4] 1
or
   
1 5 0 −1 q
M2 · ... · M6 = ( M1 ) (3.83)
[4] 1
We have already solved for the angle θ1 , so that the matrix (0 M1 )−1 ,
and with it the entire right hand side of the last equation is constant.
We define a point q’ by setting
   
0 −1 q q’
( M1 ) = (3.84)
1 1
For q’, we have
   
1 5 q’
M2 · ... · M6 = . (3.85)
[4] 1
We now evaluate the expression
 
1
M2 · ... · 5 M6 . (3.86)
[4]
To this end, we need the explicit expressions for the matrices and mat-
rix products. Appendix D lists these matrices and products, as derived
from the DH-table.
We find that
 
d5 (c2 c3 s4 + s2 c4 ) + s2 d3
  d5 (s2 c3 s4 − c2 c4 ) − c2 d3 
1 5
M2 · ... · M6 =

.
 (3.87)
[4] s3 s4 d5
1
As expected, since q is indeed the wrist center, this expression does
not contain any of the angles θ5 , θ6 , θ7 . Our goal is now to extract the
angles θ2 , θ3 , θ4 from equation 3.87. This will be very similar to the
case of the six-joint robot.
3.3 Inverse Solution for the Seven-Joint DLR-Kuka Robot 97

Solving for θ2 , θ3 , θ4

Looking at equation 3.87, we set

q0x = d5 (c2 c3 s4 + s2 c4 ) + s2 d3 ,
q0y = (s2 c3 s4 − c2 c4 ) − c2 d3 ,
q0z = s3 s4 d5 . (3.88)

Taking the square of q’, we obtain

k q’ k2 = q02 02 02
x + qy + qz . (3.89)
Evaluating the right hand side yields

k q’ k2 −d32 − d52
c4 = , (3.90)
2d3 d5
which implies
q
s4 = ± 1 − c24 , (3.91)
and finally

θ4 = atan2(s4 , c4 ). (3.92)
Having solved for θ4 , we can use the last line in equation 3.88 to solve
for θ3 . If s4 is not zero, we set

q0z
s3 = (3.93)
s4 d5
and
q
c3 = ± 1 − s23 (3.94)
so that

θ3 = atan2(s3 , c3 ). (3.95)
Otherwise (s4 = 0), we set s3 = 0.
98 3 Robot Kinematics

Finally, for the last angle (θ2 ), a procedure similar to the case of the
six-joint robot applies.
Set

u = c3 s4 d5 ,
v = c4 d5 + d3 . (3.96)

Then

q0x = uc2 + vs2 ,

q0y = us2 − vc2 . (3.97)

Define an angle γ by setting

p
r= u2 + v2
v u
γ = atan2 , (3.98)
r r
As for the six-joint robot, the case r = 0 can be excluded. We thus
have

v
= sin(γ),
r
u
= cos(γ). (3.99)
r
Therefore, we can rewrite the expressions for q0x , q0y as

q0x = r cos(γ)c2 + r sin(γ)s2 ,

q0y = r cos(γ)s2 − r sin(γ)c2 . (3.100)

With the trigonometric addition formulas, we can simplify this to

q0x = r cos(γ + θ2 ),
q0y = r sin(γ + θ2 ), (3.101)
3.3 Inverse Solution for the Seven-Joint DLR-Kuka Robot 99

so that

γ + θ2 = atan2(q0y , q0x ) (3.102)

or

θ2 = atan2(q0y , q0x ) − atan2(v, u). (3.103)

Solving for θ5 , θ6 , θ7

As a last step, it remains to solve for the three wrist angles θ5 , θ6 , θ7 .

This is again similar to the case of the six-joint robot.
We start from the matrix 4 M7 , simply obtained as

4
M7 = (0 M4 )−1 · T. (3.104)
Evaluating the left side, we find
 
c5 c6 c7 − s5 s7 −c7 s5 − c5 c6 s7 c5 s6 d7 c5 s6
4
c6 c7 s5 + c5 s7 c5 c7 − c6 s5 s7 s5 s6 d7 s5 s6 
M7 = 
 −c7 s6
. (3.105)
s6 s7 c6 d5 + d7 c6 
0 0 0 1
This matrix is of course the same matrix which we obtained for the
wrist of the six-joint arm, the only difference being the names of the
angles.
The right side of equation 3.104 is constant, and we can evaluate it,
since we have already solved for θ1 , ..., θ3 .
Thus, we can define

m13 = 4 M7(1,3) ,
m23 = 4 M7(2,3) ,
m33 = 4 M7(3,3) ,
m31 = 4 M7(3,1) ,
m32 = 4 M7(3,2) . (3.106)
100 3 Robot Kinematics

With these definitions, we can solve for θ6 :

q
θ6 = atan2(± 1 − m233 , m33 ) (3.107)
The case s6 = 0 can be addressed in the same way as for the six-joint
robot. Assuming s6 6= 0, we obtain

θ5 = atan2(m23 , m13 ) (3.108)

and

θ7 = atan2(m32 , −m31 ). (3.109)

As for the six-joint case, parameters shoulder, elbow and hand with
values ±1 control the robot configuration. With this, we summarize
the inverse analysis for the seven-joint robot:

For a given target matrix T, DH-constants di and configuration

parameters shoulder, elbow, hand, ∆ with
 
noap
T= (3.110)
0001
define
 
qx
q = qy  = p − d7 a
 (3.111)
qz
Then

θ1 = atan2(shoulder · qy , shoulder · qx ) + ∆ (3.112)

The angle θ1 is contained in 0 M1 , and we define a point q’ via
 0
  qx  
q’ q0y  0 −1 q
 
=  0  = ( M1 ) . (3.113)
1 qz 1
1
Then
3.3 Inverse Solution for the Seven-Joint DLR-Kuka Robot 101

θ4 = atan2(shoulder · elbow · s4 , c4 ) (3.114)

where we set
k q’ k2 −d32 − d52
c4 = (3.115)
2d3 d5
and
q
s4 = 1 − c24 (3.116)
For s4 6= 0

θ3 = atan2(shoulder · elbow · s3 , c3 ) (3.117)

where

q0z
s3 = (3.118)
s4 d5
and
q
c3 = 1 − s23 (3.119)
(Otherwise set θ3 = 0.)
Then

θ2 = atan2(shoulder · elbow · q0y , shoulder · elbow · q0x )

− atan2(shoulder · elbow · v, u) (3.120)

where

u = c3 s4 d5
v = d5 c4 + d3 (3.121)

For
−1
M =4 M7 = 0 M4 T, (3.122)
102 3 Robot Kinematics

we define the entries of M as mi j . Then

q
θ6 = atan2(hand · 1 − m233 , m33 ). (3.123)
For s6 6= 0:

θ5 = atan2(hand · m23 , hand · m13 ) (3.124)

and

θ7 = atan2(hand · m32 , −hand · m31 ), (3.125)

(Otherwise set θ5 = θ7 = 0.)
The DH-matrices 0 M1 , . . . ,6 M7 for the seven-joint robot (and
matrix products needed here) are shown in appendix D.

3.4 Eight-Joint Robot

Fig. 3.17: Eight-joint robot.

3.5 C-arm 103

We now consider the following eight-joint robot: Take a small robot

with only two joints. Assume this little robot has the same joints 1 and
2 as our six-joint robot before. At the end of the second link, we place
a base plate, and call that the tool flange. Call this 2-joint robot R1 .
Now we mount a copy of our six-joint robot to this flange. Call this
robot R2 (figure 3.17).
The kinematic analysis of this new eight-joint robot is very simple,
and we have already done most of the work: We are given our target
matrix T from equation 3.52. We use R1 as a positioning unit, and
choose a target position for R1 with a heuristic scheme. For example,
this heuristic scheme can be based on an analysis of the obstacles in
the surrounding workspace. The second robot R2 provides the trans-
formation between the tool flange and the target. This transformation
is a 4×4-matrix, and we already have the complete kinematic solution
for this transformation.

3.5 C-arm

For the analysis of the six-joint robot and the seven-joint robot we ap-
plied a useful method: We partitioned the joint set into two parts (base
and wrist), and solved the two parts separately. Clearly, this strategy
is not always applicable. An example is the C-arm.
Originally, C-arms were not designed to be robots. They were meant
to be positioned by hand. However, newer C-arms have actuated
joints, and can be regarded as robots. In this section we will look
at geometric methods for analyzing linkages. Thus, we will not use
our standard tools (e.g. inverting DH-matrices), but we will compute
the joint angles directly from geometric constraints. This results in a
simple solution, with several practical advantages. Geometric meth-
ods can be regarded as an alternative to the algebraic methods presen-
ted before.
104 3 Robot Kinematics

3.5.1 Forward analysis

We have introduced C-arms in the preceding chapters (see e.g. fig-

ure 1.9). Figure 3.18 shows the first two coordinate systems for the
C-arm. The point O5 is the mid-point between the source and the de-
tector. Notice that O5 is not the center of rotation of the C-arm’s C.
Rather, the center of rotation is the point O4 . The offset between O4
and O5 complicates the inverse analysis. The orbital rotation of the C
is indicated by its axis (dotted line) and its angle (θ5 ). The so-called
angulation of the C is also marked in the figure (angle θ4 ).

O4 a5 O5
a4
O3 θ5

z1

S1 y1 θ4
x1

z0
S0
y0
x0

Fig. 3.18: The first two coordinate systems S0 , S1 and the origins O3 to
O5 for the forward kinematic analysis of a C-arm.

The distance between O4 and O5 is called a5 . Likewise, a4 is the dis-

tance between O3 and O4 . Notice also that the point O5 rotates on a
circle around O4 when θ5 changes. Figure 3.19 shows the remaining
coordinate systems S2 , S3 , S4 , S5 .
3.5 C-arm 105

d3
x3
S3 /O3
S2 /O2 z2 θ4
y3 z3
x2
y2

x4
z4
S4 y4
O4 O5

z5
y5
S5 x5

Fig. 3.19: Coordinate systems S2 , S3 , S4 , S5 . Notice that O4 is the center of

rotation of the C. a4 is the distance between O3 and O4 .
106 3 Robot Kinematics

Given the coordinate systems, we can set up the DH-table for the C-
arm (see table 3.2). Recall that the first line in the table describes the
transition from the coordinate system S0 to system S1 (figure 3.18).

i αi ai di θi
1 0 0 d1 0
2 -90 0 0 θ2
3 0 0 d3 -90
4 90 a4 0 θ4
5 90 a5 0 θ5 + 90

Table 3.2: DH-Parameters for the C-arm

From the table, we derive the matrices for the forward transformation.
   
100 0 c2 0 −s2 0
0 1 0 0  1  
0
M1 =   M2 = s2 0 c2 0
0 0 1 d1   0 −1 0 0
000 1 0 0 0 1
   
0 10 0 c4 0 s4 a4 c4
−1 0 0 0  3  
2
M3 =   M4 = s4 0 −c4 a4 s4 
 0 0 1 d3  0 1 0 0 
0 00 1 0 0 0 1
 
−s5 0 c5 −a5 s5
4
 c5 0 s5 a5 c5 
M5 = 
 0 1 0 0 


0 0 0 1
We multiply the matrices just derived for the C-arm and obtain:

0
M5 [1] =
 
−c2 s4 s5 − s2 c5
−s2 s4 s5 + c2 c5 
 
 −c4 s5 
0
3.5 C-arm 107

0
M5 [2] =
 
−c2 c4
−s2 c4 
 
 s4 
0

0
M5 [3] =
 
c2 s4 c5 − s2 s5
s2 s4 c5 + c2 s5 
 
 c4 c5 
0

0
M5 [4] =
 
−a5 c2 s4 s5 − a5 s2 c5 + a4 c2 s4 − s2 d3
−a5 s2 s4 s5 + a5 c2 c5 + a4 s2 s4 + c2 d3 
 
 −a5 c4 s5 + a4 c4 + d1 
1
(3.126)

Here, 0 M5 [1], . . . ,0 M5 [4] denote the four columns of 0 M5 .

3.5.2 Inverse analysis

For some C-arms, we do have a4 = a5 = 0. Such C-arms are called

isocentric C-arms. In general, a4 , a5 6= 0, and we must take these off-
sets into account (figure 3.20).
Above we saw two basic ideas for solving inverse kinematic equa-
tions. The first idea was equation searching (applied for the three-joint
robot), the second was joint set partitioning with decoupling.
108 3 Robot Kinematics

a5
a4

Fig. 3.20: Offsets a4 and a5 for a non-isocentric C-arm.

None of these ideas will work here. We cannot decouple rotation from
translation. Also, there are no pairs of equations of the form sin(θi ) =
a, and cos(θi ) = b in equation 3.126.
Furthermore, we do not have a full 4-by-4 matrix to start from. Rather,
our target is given as a direction from which to image, and a point on
the beam’s central axis.
Thus, we assume we only have a point p and a unit vector u with
   
px ux

p = py  
and u = uy  , (3.127)
pz uz
rather than the full 4 × 4 target matrix.
Here p is our target position of the point O5 , and u is the axis of the x-
ray beam. Recall that O5 is the origin of S5 . O4 is the center of rotation
of the C-arm’s C. The vector u is the z-vector of the last coordinate
frame. The inputs for the inverse analysis are shown in figure 3.21.
Our goal is to extract the angles and translation values from p and u
alone.
3.5 C-arm 109

z0 p

x0 y0

Fig. 3.21: Target position and orientation for the inverse analysis of the
C-arm.

3.5.2.1 Geometric preliminaries

v
v⊥u u
u⊥E

v||u u u||v v

Fig. 3.22: Decomposition and projection of vectors.

We will need two definitions from analytic geometry. Let u, v be vec-

tors. We split v into two components, one orthogonal to u and one
parallel to u (figure 3.22). Let v||u denote the parallel component, and
v⊥u be the orthogonal component of v.
Then from figure 3.22 we see that

v = v||u + v⊥u (3.128)

110 3 Robot Kinematics

Here v||u is obtained by scaling u, and it can be shown that

v||u = (uv)u (3.129)

where (uv) is the scaling factor.
Thus,

v⊥u = v − v||u = v − (uv)u (3.130)

The same notation works for planes in space. Let vx = 0 be the equa-
tion of a plane E, i.e. E : vx = 0 is a plane containing the origin, and
having normal vector v. Then u⊥E is defined as u⊥E = u − (uv)v.

For the inverse analysis, we start from point O5 . As noted above, our
target is not a homogeneous matrix, but rather the position of O5 and
a direction vector u pointing along this central axis. We assume u is a
unit vector. Also, by p we denoted the coordinate vector for the point
O5 .
We place a line g along the z2 -axis (see figure 3.19). Notice in the
figure, that the axis z2 will always coincide with axis z3 . Our line g
has a direction vector, which we will call v.
The second joint of the C-arm is a revolute joint with axis z1 and joint
angle θ2 . We will now assume that we have already solved for this
angle θ2 . Hence our line g and its direction vector v is fixed from now
on. From figures 3.18 and 3.19, we see that the coordinates of v can
be calculated explicitly, as v = (− sin θ2 , cos θ2 , 0)T .
Our strategy will be the following: we solve for the open variables
d1 , d3 , θ4 , θ5 , based on the assumption that we already have a value
for θ2 . Thus we will have equations for the values of d1 , d3 , θ4 , θ5 as
functions of θ2 .

Solving for θ4

The analysis is based on the two direction vectors u and v.

Figure 3.23 shows the situation from which we set out to find a solu-
tion for θ4 . Our vector v defines a plane in space (called Fv ), i.e. v is
the normal vector of this plane Fv : vx = 0. The z-vector of our base
3.5 C-arm 111

Detector Fv
z0
θ4
u
v

Source

Fig. 3.23: Calculation of θ4 . v is the normal vector of the plane Fv (draw-

ing plane). We project u onto this plane. The projection is u⊥Fv , and we
have calculated it above. Then θ4 is the angle between u⊥Fv and z0 .

coordinate system (z0 ) is contained in this plane. We project the vector

u onto Fv . According to the notation in figure 3.22 we can obtain this
projection by setting

u⊥Fv = u − (uv)v. (3.131)

Although u has unit length, the projected vector u⊥Fv may not be a
unit vector, so we normalize it. Then the angle between z0 and this
projection is simply the inverse cosine of the product.
Thus we set
 
u⊥Fv
θ4 = sgn(θ4 ) arccos z0 . (3.132)
||u⊥Fv ||
We must define the sign of this angle by hand, since the arccos-
function alone will return only positive values for arguments from
(−1, 1). Set sgn(θ4 ) = 1, if u(v × z0 ) > 0 and sgn(θ4 ) = −1 other-
wise.
112 3 Robot Kinematics

Solving for θ5

We again look at the two vectors u and v. The two vectors span a
plane, which is the mechanical plane of the C-arm’s C. The vector
u⊥v is obtained as u − (uv)v. Then θ5 is the angle between u and u⊥v ,
hence we set
 
u⊥v
θ5 = sgn(θ5 ) arccos u . (3.133)
||u⊥v ||

θ5
u⊥v
u
v

Fig. 3.24: Computing θ5 .

Figure 3.24 illustrates the situation.

Again, we must compute the sign of θ5 from the vectors u and v. Set
sgn(θ5 ) = 1 if uv > 0 and sgn(θ5 ) = −1 otherwise.

Solving for d1

Having computed θ4 and θ5 , we can directly find d1 from the forward

equation in equation 3.126. From the last entry of the third row in the
matrix, we find that

pz = −a5 c4 s5 + a4 c4 + d1 . (3.134)
Thus, d1 = pz + a5 c4 s5 − a4 c4 .
3.5 C-arm 113

Solving for d3

The points O3 , O4 and O5 span a plane. This plane contains the vectors
u and v defined above. The situation is illustrated in figure 3.25.

z0
u
O4 a5
O5 = p
a4
v
θ4
O3

O0 y0

d3

Fig. 3.25: Computing d3 .

Notice that the lengths of the offsets are a4 and a5 , while the vectors
u⊥v and v⊥u point into the directions of these offsets (figure 3.26).
We have

v||u = (uv)u,
v⊥u = v − v||u ,
v − (uv)u. (3.135)

For the points O4 , O5 and O3 we have (see figure 3.26):

O4 = O5 − ||vv⊥u || a5 (3.136)
⊥u

O3 = O4 − ||uu⊥v || a4 (3.137)
⊥v
114 3 Robot Kinematics

u
z0

θ5
u⊥v
v

O0 y0

z0

θ4
v

O0 y0

Fig. 3.26: The vectors u⊥v and v⊥u point into the directions of the offsets.
Left: rotation about angle θ5 . Right: rotation about angle θ4 .

Thus,

O3 = O5 − ||vv⊥u || a5 − ||uu⊥v || a4 (3.138)

⊥u ⊥v

Since we have already computed u⊥v , and we also know that

O5 = p, (3.139)
3.5 C-arm 115

where p is one of our inputs, we have all the ingredients in equa-

tion 3.138.
Our unknown d3 is the distance between O2 and O3 , i.e.

d3 = ||O2 − O3 || (3.140)
and

O2 = (0, 0, d1 )T . (3.141)

Solving for θ2

Our solution for the remaining angle θ2 is very pragmatic. First, we

fix the value for θ2 by setting it to zero. We input this value, together
with the values for d1 , d3 , θ4 , θ5 (as computed above) into the for-
ward equation. From this forward transformation, we obtain a value
u0 and a value p0 as an estimate. But this is only an estimate based
on the assumption θ2 = 0. We define an error term: subtract the in-
put vector u from u0 and p from p0 . We now minimize the error term
e = (u0 − u)2 + (p0 − p)2 via grid search and/or bisection. The range
of the angle θ2 is very small for C-arms. Typically, it is ± 10 degrees.
Thus, bisection or grid search is adequate for finding θ2 . This com-
pletes our inverse kinematic analysis of the C-arm.

3.5.3 Applications

Given the forward and inverse kinematic solution for the C-arm, we
can obtain several interesting applications of robotic C-arms.
1. image stitching
2. intraoperative 3D reconstruction and CT imaging
3. 4D imaging
4. cartesian control
In the first application, we paste several images to obtain a panorama
image, e.g. of a long bone or full leg (see figure 3.27). For the second
application, we move joint five, to obtain images from a large number
116 3 Robot Kinematics

Fig. 3.27: Image stitching (without registration) for a femur bone.

of angles, and reconstruct them for a full intra-operative CT image. If

we tilt joint four, we can obtain CT images from non-standard angles
angles. By taking a series of 3D images with the C-arm, each from a
different respiratory or cardiac phase, we obtain a 4D CT.
It is often difficult to center an image, while remaining in the image
plane. Thus, suppose we take an image of a femur head. After taking
the image, the femur head is not in the center of the image, and im-
portant structures are poorly visible. Based on our inverse analysis we
can compute new joint settings, such that a new image will be centered
appropriately.
The above list is not yet complete. A further application is described
in the following example. This example combines navigation and kin-
ematics, and we obtain a method for registration-less navigation.

Example 3.1

Assume we have acquired a CT with the robotic C-arm. Assume also

we have placed a laser beam pointing along the central axis of C-arm’s
x-ray beam. Then the surgeon can mark a line in the CT image. The
C-arm can then mark this line in space with the built-in laser beam
(figure 3.28).
The advantage of this procedure is the following. The C-arm serves
both as pointing device and CT-imaging device. Thus the spatial ref-
3.5 C-arm 117

Laser beam

Fig. 3.28: Registration-less navigation with the C-arm.

erencing between the two devices (imaging and navigation) is auto-

matically given after appropriate calibration. We can navigate without
image registration. This advantage is very significant in practice. It
simplifies the procedure, while increasing its accuracy and reliability.
Commercial C-arms only have four motorized axes. This excludes the
second axis (θ2 ). The so-called wig-wag motion of the C-arm (joint
2) is difficult to motorize for mechanical reasons. But notice that four
axes is all we need: mathematically, four parameters suffice to spe-
cify a line in space. Our goal is to move four joints in such a way,
that our central axis coincides with a predefined line in space. (If you
are unclear about the mathematics behind this argument, think of the
following: We saw that six parameters specify a coordinate system
with respect to a fixed base coordinate system. But for a line, two of
those six parameters are not needed. One first parameter is lost, since
we can rotate the line about itself without changing its position. The
second parameter vanishes, because we can translate the line along
itself. Thus four parameters suffice, and we only need four motorized
axes.)
The four parameters are d1 , d3 , θ4 , θ5
We have marked a line on the CT image stack. Call this line h. As
above, u is its direction vector. We assume h is not horizontal, i.e. not
parallel to the x0 -y0 -plane. Thus define a point q as the intersection
point between h and the x-y-plane.
118 3 Robot Kinematics

We extract the angles θ4 and θ5 from the vectors u and v as above.

It thus remains to find the translational displacement values d1 and d3
from the given inputs q, u, v and the offsets a4 and a5 .
The situation is shown in figure 3.29.

z0 u
v⊥u
a5

u⊥v a4
h
v
d3
d1
  y
O0 xp 0
y p 
0

Fig. 3.29: Vector chain for the linear equation system in equation 3.142.

From the figure, we see that a chain of vectors leads from the origin
to the point q = (x p , y p , 0)T . In this chain there are three variables:
d1 , d3 and λ (scale factor of the direction vector for h). Thus, if h
is in general position, then d1 and d3 will occur as solutions of the
following linear equation system:

       
0 vx ux xp
u⊥v v⊥u
  
d1 0 + d3 vy + a4  + a5 + λ uy = y p 
  
||u⊥v || ||v⊥u ||
1 vz uz 0
(3.142)
Exercise 3.6 at the end of this chapter discusses a short program for
solving the four-axes problem for C-arms.

(End of Example 3.1)

3.6 Center-of-arc kinematics 119

3.6 Center-of-arc kinematics

We saw that the methods for kinematic analysis not only apply for
revolute joints, but also for translational joints. The C-arm is one such
example, as it consists of several translational and revolute joints.
Many medical devices are based on a so-called center-of-arc kin-
ematic construction. This means that a rotation about the patient’s
head or body is the basic motion type built into the system. The ef-
fector is thus mounted to an arc in space. This arc is frequently moun-
ted to a base, which itself can be rotated. Examples for devices follow-
ing this general convention are: neurosurgical frames for stereotaxis,
angiography systems and radiation therapy machines.
Figure 3.30 shows an example of a particularly simple center-of-arc
construction. This robotic construction is used in functional neurosur-
gery. There are four translational axes (t1 ,t2 ,t3 ,t4 ) and two revolute
axes r1 , r2 . The two revolute axes intersect in space.

Fig. 3.30: Center-of-arc kinematic construction .

It turns out that the inverse analysis for the center-of-arc mechanism
in figure 3.30 is much simpler than the analysis of the C-arm and the
six-joint robot, and is left as an exercise.
120 3 Robot Kinematics

3.7 Surgical microscopes

All robots considered so far were chains of joints, with a hand or

tool at the end of this chain. Let us assume we attach the hand to
the ground. The base joint also remains attached to the ground. Then
both ends of the chain are grounded. Now the robot can still move, as
long as there are enough joints. We call such a robot a parallel robot.
What can we do with parallel robots ? Surprisingly, such robots have a
number of practical advantages, e.g. stiffness and safety. The mechan-
ical construction of the surgical microscope in figure 1.22 is a parallel
robot.
The basic structure of the microscope is a linkage with both ends at-
tached to the same base plate. Thus, we can follow a trace from the
base of the device to the tip and then return to the base along a second
path. (Notice that the two paths will cross). The overall structure con-
sists of two parallelograms. This illustrates the name ’parallel robot’.
The kinematic solution of the surgical microscope can be found with
the methods discussed above (see also [1]).

3.8 Kinematics and Dexterity

Which is the best robot for a given application? A number of criteria

can be considered in this context. Beyond kinematics, we can look
at forces, dynamics, stiffness and accuracy. Nonetheless, if the robot
cannot reach a given position, then accuracy, stiffness and dynamics
at this position will not matter much. Thus reachability, and with that,
kinematics is the first criterion.
Assume we fix a point p in the workspace. Can the robot reach this
point p in arbitrary orientation? If not, can we quantify the range of
reachable orientations at p? The solid angle is a measure for dexterity
at p. To compute the solid angle, we plot the range of reachable ori-
entations at p as points on a unit sphere, and measure the reachable
sub-surface. A straightforward way to evaluate the solid angle at p is
to discretize the orientational range, i.e. place a large number of grid
points on the unit sphere, interpret each grid point as an orientation,
3.8 Kinematics and Dexterity 121

and check whether the robot can reach p under this orientation. The
last step can be implemented by a routine for inverse kinematics.
An alternative is to require a full sphere be reachable at p, and then
plot all points p in the work space, admissible under this measure. We
can then compare different constructions, and we will do this in the
following example.

Example 3.2

Above we looked at the seven-joint DLR-Kuka robot. Clearly, we

would expect the seven-joint arm to have better dexterity properties
than a standard six-joint elbow manipulator. But how much do we
gain by adding a seventh joint?
To answer this question, we compute the volume of points p reachable
in arbitrary orientation. We compare a seven-joint robot to a six-joint
robot. To obtain a six-joint version of the seven-joint robot, we set the
slack value ∆ in equation 3.80 to zero. As a result, we will always have
θ3 = 0, so that we obtain a six-joint version of the seven-joint robot.
To be specific with respect to the link parameters and joint ranges we
use the values in table 3.3.

i αi ai di joint range
1 -90 0 340 -170...+170
2 90 0 0 -120...+120
3 -90 0 400 -170...+170
4 90 0 0 -120...+120
5 -90 0 400 -170...+170
6 90 0 0 -120...+120
7 0 0 111 -170...+170

Table 3.3: Link parameters (in mm) and joint ranges the seven-joint
robot in the example

Figures 3.31-3.32 show the results for the six-joint version compared
to the seven-joint version. As expected, the seven-joint robot does
have a larger range of points reachable in arbitrary orientation. But
what is the influence of the joint ranges? In other words, can we in-
122 3 Robot Kinematics

Fig. 3.31: Dexterity volume for the six-joint version of the seven-joint ro-
bot. Grid points marked in black are reachable in arbitrary orientation
(full sphere). The grid points p have a grid distance of 100 mm.

Fig. 3.32: Dexterity volume for the seven-joint robot.

crease the joint ranges of the six-joint version in such a way that it
matches (or outperforms) the seven-joint version?
To answer this second question, we increase the joint ranges of the
six-joint version from

R-joints: − 170... + 170 P-joints: − 120... + 120 (3.143)

3.8 Kinematics and Dexterity 123

to

R-joints: − 170... + 170 P-joints: − 150... + 150 (3.144)

and recompute the dexterity map. The result is shown in figure 3.33.

Fig. 3.33: Dexterity volume for the six-joint robot, with joint ranges en-
hanced to −150◦ , . . . , +150◦ for the P-joints.

We see that the enhanced version of the six-joint robot (enhancement

with respect to joint ranges) clearly outperforms the seven-joint ver-
sion with the standard ranges.
Assume we must trace a path with a tool mounted to a robot arm.
Often, due to joint range limits, the robot will be forced to retract the
arm while tracing the path, and switch its configuration from elbow-
above to elbow-below, then return to the path to complete it. Cleary,
configuration changes can cause a number of problems.
Figure 3.34 shows the Dextor robot. This robot has the same DH-
structure as the standard six-joint elbow robot analyzed above. How-
ever, there is a significant difference in the shapes of the arm links.
The links are u-shaped, so that self-collisions can be excluded. Thus,
the joint ranges are unlimited. This leads to an interesting property:
the robot can trace any path within the workspace without having to
switch configurations along the path.
124 3 Robot Kinematics

z0

x0

Fig. 3.34: Six-joint Dextor robot with unlimited joint ranges. Self-
collisions are mechanically excluded.

Figure 3.35 shows a seven-joint version of Dextor. The advantage of

this robot is that the null-space is not fractionated. Thus we can push
the elbow out of the line of sight if needed, without changing the ef-
fector position/orientation.

(End of Example 3.2)

3.8 Kinematics and Dexterity 125

z0

x0

Fig. 3.35: Seven-joint version of Dextor.

126 3 Robot Kinematics

Exercises

Exercise 3.1

Verify the results in equations 3.5, 3.14 and 3.17 for the settings θ1 =
0, θ2 = 0, θ3 = 0, and θ1 = π/2, θ2 = 0, θ3 = −π/2, and θ1 = 0, θ2 =
−π/4, θ3 = −π/4.

Exercise 3.2

Find a geometric solution for the inverse kinematics of the three-joint

robot.
Exercise 3.3

Show that the value r defined in equation 3.34 is not zero, unless a2 =
d4 and sin θ3 = 0.

Exercise 3.4

Implement the kinematic solution for the six-joint robot. Inputs: tar-
get matrix T and configuration parameters shoulder, elbow and hand.
Extract angles θ1 , ..., θ6 , and resubstitute these values into the forward
kinematic equation, giving a 4×4 matrix. Compare to the input matrix
T.
Exercise 3.5

Set up a small routine which tests whether a given angle vector

(θ1 , ..., θ6 )T corresponds to an elbow-up or an elbow-down configura-
tion. Derive a routine for extracting all three configuration parameters
for the six-joint robot from a given angle vector.

Exercise 3.6

Write a short program that computes the values d1 and d3 as solutions

of equation 3.142. Your program should proceed as follows: Input val-
ues for d1 , θ2 , d3 , θ4 , θ5 . From the inputs compute u and p via the for-
3.8 Kinematics and Dexterity 127

ward transformation in equation 3.126. From u and p, compute the

intersection point (x p , y p , 0)T of h and the x-y-plane. Finally, determ-
ine d1 and d3 from equation 3.142 and compare to the input values for
d1 and d3 .

Exercise 3.7 Center-of-arc Kinematics

a) Set up the joint coordinate systems for the center-of-arc mechan-

ism in figure 3.30
b) Determine the DH-parameters for the center-of-arc mechanism
and set up the forward matrix.
c) Solve the forward matrix for the parameters r1 , r2 (rotational vari-
ables) and t1 ,t2 ,t3 (translational variables), assuming t4 is a con-
stant, such that the tool tip is at the center of the arc.

Summary

Forward kinematic analysis follows standardized rules, and is applic-

able to most given linkages without modification. By contrast, inverse
analysis requires more mathematical intuition. We saw several ex-
amples for the inverse analysis of robots. The examples illustrate two
different methods. The first method is algebraic. In this case, we dir-
ectly solve a matrix equation, i.e. the result of the forward analysis.
With the atan2-function we solve equations of the form sin θi = a and
cos θi = b, with constants a, b. If such a pair of equations can be found
in the forward matrix equation, then we can solve for θi . In some
cases, after having found a solution for one angle, we can solve for
next angles in much the same way. Here it can be useful to simplify
the matrix equation by multiplying with the matrix inverse i−1 Mi . We
solve a six-joint (elbow manipulator) and a seven-joint robot (DLR
Kuka robot) with the algebraic method. Both robots are standard ro-
bots. The solution for the seven-joint robot is based on the analysis of
the six-joint robot.
The second method for inverse kinematic analysis is geometric. The
geometric approach also relies on the forward matrix equation, but
128 3 Robot Kinematics

starts from a detailed geometric analysis of the construction. In the

most simple cases, two or more rotation axes intersect and remain
fixed with respect to each other. For surgical C-arms, the rotation axes
θ4 and θ5 do intersect, but the relative angle of the two axes does
not remain fixed. Geometric solutions can be found for isocentric and
non-isocentric C-arms.

Notes

A number of text books on robotics present different versions of in-

verse solutions for six-joint elbow robots (see e.g. [2–4]). A purely
geometric solution for joints 1-3 of the six-joint robot is discussed
in [5]. A geometric solution of the C-arm is discussed in [6–8]. An
algebraic solution for the C-arm is also possible [9].

References

[1] M. Finke and A. Schweikard, “Motorization of a surgical micro-

scope for intra-operative navigation and intuitive control,” Inter-
national Journal of Medical Robotics and Computer Assisted Sur-
gery, vol. 6, no. 3, pp. 269–280, Sep. 2010.
[2] J. J. Craig, Introduction to Robotics: Mechanics and Control,
3rd ed. Prentice Hall, 2005.
[3] R. P. Paul, Robot Manipulators: Mathematics, Programming, and
Control. Cambridge, MA, USA: MIT Press, 1982.
[4] O. Khatib and B. Siciliano, Eds., Springer Handbook of Robotics,
ser. Springer Handbooks. Berlin Heidelberg: Springer, 2008,
vol. 9.
[5] H. J. Siegert and S. Bocionek, Robotik: Programmierung intelli-
genter Roboter. Berlin: Springer, 1996.
[6] N. Binder, L. Matthäus, R. Burgkart, and A. Schweikard, “The
inverse kinematics of fluoroscopic C-arms,” in 3. Jahrestagung
der deutschen Gesellschaft für Computer- und Roboterassistierte
References 129

Chirugie (CURAC), vol. 3. München: CURAC, Oct. 8-9 2004,

pp. 1–5.
[7] ——, “A robotic C-arm fluoroscope,” International Journal of
Medical Robotics and Computer Assisted Surgery, vol. 1, no. 3,
pp. 108–116, 2005.
[8] N. Binder, “Realisierung eines robotischen Röntgen C-
Bogens - Technische Umsetzung und Applikationen,”
Dissertation, Technisch-Naturwissenschaftliche Fakultät der
Universität zu Lübeck, Sep. 2007. [Online]. Available:
http://d-nb.info/986548871
[9] L. Matthäus, N. Binder, C. Bodensteiner, and A. Schweikard,
“Closed-form inverse kinematic solution for fluoroscopic C-
arms,” Advanced Robotics, vol. 21, no. 8, pp. 869–886, Aug. 2007.
Chapter 4
Joint Velocities and Jacobi-Matrices

In the previous chapter we discussed the relationship between joint

angles and tool positions. We started from given positions for our tool,
and computed the joint angles. Suppose now, we wish to trace out a
curve in space with our tool. In the most simple case, the curve is a
line. Thus, we are given a robot with revolute joints, and we wish to
move the gripper along a line segment.
Consider the planar two-joint robot in figure 4.1.

θ2

l1 l2
y

θ1 x p

Fig. 4.1: Two link manipulator

This robot has two revolute joints. How do we move the joints, in or-
der to produce a motion of the point p along the x-axis? As a first
guess, assume we move the two joints with constant and equal velo-
cities, ignoring all necessary accelerations. The two links have equal
lengths (i.e. l1 = l2 ), and we hope to produce our linear motion in this
way. Unfortunately, a quick simulation will show that this will not
work. Instead, our tool will move along a circular arc. If we modify

131
132 4 Joint Velocities and Jacobi-Matrices

our input parameters, the arc will become bigger or smaller, or it will
be deformed. But, alas, it will be difficult to deform the arc into a line.
What now, if we not only need a line motion, but we also require
that the velocity of the tool remains constant throughout the motion?
Hence, even if we succeed to produce a line, we may not traverse
this line with a very even velocity. As an example, we again look at
figure 4.1. Assume, the tool is very near the furthest point reachable
by the linkage along the x-axis. As we move the joints to stretch out
the arm even further, the tool will move more and more slowly, until
the velocity will become zero, for the arm fully stretched.

We started the above discussion for the case of a line motion. How-
ever, there is an even simpler case for velocities. This most simple
case arises when we require to move our joints, but to keep the tool in
a fixed position. It will be discussed in the first section of this chapter.

4.1 C-arm

When taking a CT image with a C-arm, we need to rotate the C-arm’s

C, but at the same time we need to keep the target point (i.e. the mid
point of the beam axis) fixed.
Figure 4.2 shows the kinematic offsets a4 and a5 for the C-arm, as
discussed in the previous chapters.
Due to the offsets, the center of rotation is the point O4 . However, the
center point of the beam axis is the point O5 . Our goal is to move the
the two prismatic joints in such a way that the C rotates in the drawing
plane, and at the same time, O5 remains in place. Recall that the two
prismatic joints have the joint parameters d1 , d3 . Figure 4.3 illustrates
the compensating motion of joints 1 and 3 necessary for keeping O5
fixed.
To capture the motion velocities of the joints, we will now write our
joint values as functions of time.
Thus instead of the joint value d1 , we now have a function of a time
parameter t, i.e. d1 (t). Likewise, the remaining joint values are written
as functions of the same time parameter t. We assume that t moves
4.1 C-arm 133

O4 a5 O5
a4
O3

z0

y0

Fig. 4.2: The point O4 is the center of rotation of the C-arm’s C. a4 and
a5 are the offsets. To obtain a CT image from C-arm x-ray images, we
rotate the C in the drawing plane.

O5

Fig. 4.3: Rotating the C-arm’s C, while keeping the target point O5 static
requires compensating motion with the two prismatic joints. The motion
of the two prismatic joints is visualized by the two arrows.
134 4 Joint Velocities and Jacobi-Matrices

within the interval [0, 2π]. It may seem odd to choose this interval for
a prismatic joint, but the reason will soon become clear.
We insert the functions of t thus defined into the forward kinematic
matrix, and obtain a matrix, representing tool orientation and tool po-
sition as a function of time.
Can we state explicit time functions for d1 (t) and d3 (t) which will
keep the point O5 static during orbital rotation of the C-arm’s C?
First, note that in practice our orbital rotation should have constant
velocity. Hence, the derivative θ50 (t) should be constant, or, to be more
specific, we set

θ5 (t) = t (4.1)
Here θ5 runs from 0 to 2π, and this is why we chose the same range
for t. A full rotation of 2π is unrealistic for mechanical reasons, but
we will ignore this for now.

z0
a5
θ a5 sinθ
a4
a5 cosθ
d1
y0

d3

Fig. 4.4: Deriving explicit joint velocity functions d1 (t) and d3 (t).

Figure 4.4 shows how to obtain explicit joint motion functions for
d1 (t) and d3 (t). We see that the displacement required for d3 amounts
to −a5 cos(θ ), where θ is the angle marked in the figure. Likewise,
we find that the displacement for d1 over time is a5 sin(θ ).
4.1 C-arm 135

Subtracting the kinematic offset a4 (see figure 4.2), we obtain the fol-
lowing motion functions

d1 (t) = a5 sin(t) − a4 + z p
d3 (t) = −a5 cos(t) + y p (4.2)
Here the constants y p and z p denote the target position for O5 . Having
found the motion functions for the joints, we can obtain the velocities
for the prismatic joints by taking derivatives.
By d˙1 we denote the joint velocity d10 (t). With this notation, we obtain

d˙1 = a5 cos(t)
d˙3 = a5 sin(t) (4.3)

Non-vertical CT image acquisition

Above, we have assumed that the C-arm is in vertical position, (i.e.

θ4 = 0). Moving θ4 = 0 to a non-zero value, we obtain a tilted posi-
tion, i.e. the C moves out of the drawing plane (see figure 4.2). We can
incorporate the angle θ4 of the C-arm, to obtain non-vertical CT im-
ages. Again, the required joint velocities of the prismatic joints (d1 (t)
and d3 (t)) can be computed explicitly, based on an elementary geo-
metric analysis. Here θ4 of the C-arm remains fixed, i.e. θ4 (t) = α for
a fixed angle α. We obtain:

d1 (t) = cosα(a5 sin(t) − a4 ) + z p

d3 (t) = −a5 cos(t) + y p (4.4)
We have thus derived explicit motion functions and velocities for the
joints, which correspond to a specific tool motion.

Velocities and Discretized Motions

In practice, explicit joint motion functions are not always needed. In-
stead, we can discretize the required motion. For example, to move
136 4 Joint Velocities and Jacobi-Matrices

the tip of a linkage from a point p to a point q along a straight line, we

simply place intermediate points p1 , p2 , ..., pn along the line segment
p to q. But now suppose we prescribe a new orientation at point q.
That is, the 3 × 3 orientation matrices at p and q are different.
How do we interpolate between two 3 × 3 orientation matrices? As an
example, we would like to go from the orientation O to O0 , where
 
0 0 −1
O = 0 1 0  (4.5)
10 0
and
 
0 −1 0
O0 = 1 0 0 (4.6)
0 0 1
A straightforward method for finding such intermediate orientations
is described in the following example.

Example 4.1

Assume we are given two arbitrary orientation matrices, such as the

matrices in equations 4.5 and 4.6. Our goal is to interpolate between
these matrices, i.e. find a series of matrices O1 , ..., On , interpolating
for the corresponding orientations.
We first extract the yaw-pitch-roll angles from O and also from and
O0 . A method for doing this is described in exercise 2.5. This method
converts a 3 × 3 matrix to yaw-pitch-roll angles.
We obtain a triple (α, β , γ), extracted from O. Likewise (α 0 , β 0 , γ 0 )
are obtained from O0 . Now we can regard both triples as points
in a three-dimensional space. We connect the two points by a line
segment, and obtain an angle interpolation, i.e. a series of triples
(α1 , β1 , γ1 ), ..., (αn , βn , γn ). For each interpolating point, we reassemble
the orientation matrix. To assemble an orientation matrix from three
angles α, β , γ, simply multiply the three elementary rotation matrices
R(z, γ), R(y, β ), R(x, α).

(End of Example 4.1)

4.2 Jacobi-Matrices 137

Given the interpolating points p1 , ..., pn as well as the interpolating

orientation matrices O1 , ..., On , we can apply the inverse kinematic
equations for the linkage, and find a series of joint angle placements
for following the given path.
We can then obtain approximations for the local velocities along the
path. We simply divide the path increments by the time increments,
when moving from point pi to pi+1 . Here, we can obtain both the
joint velocities and the tool velocities.
This straightforward method requires inverse kinematics. In the next
section we will discuss an alternative way to follow a given path,
where no explicit inverse kinematic solution is needed. This altern-
ative relies on the so-called Jacobi-matrix.

4.2 Jacobi-Matrices

Suppose we have interpolation points along a given path, and we wish

to follow these interpolation points with our tool. If we have a large
number of such interpolation points, then the angle increments will be
small, and we can work with approximations. As a typical figure, our
increments are given as time increments, and one increment corres-
ponds to 4 milliseconds.
We now pick a fixed position and orientation along the path. We have
both the current values for all joint angles, and we also have the tool
position and orientation. The tool position and orientation are given
as a 4 × 4 matrix. We refer to this matrix as the current tool position
and orientation. Let
 
noap
(4.7)
0001
be this matrix, hence the matrix has constant entries, and


noa (4.8)
is a 3 × 3 orientation matrix, i.e. n = (nx , ny , nz )T .
To be specific, we assume our robot is a six-joint robot. The forward
kinematic equation of our robot is a matrix equation, where the left
138 4 Joint Velocities and Jacobi-Matrices

side is given by the constant matrix in equation 4.7 and the right side
is a matrix containing the six angles θ1 , ..., θ6 as variables.
This matrix equation consists of 12 individual equations, since we
have 4 × 4 matrices, where we can ignore the last matrix line. Each
equation has a constant on the left side, and variables θi on the right
side.
We take partial derivatives of these 12 equations with respect to the 6
variables. This works as follows: assume the current position of our
robot is known and given both in terms of the joint variables and in
terms of the tool matrix. When taking the partial derivative of one
such equation with respect to, say, θ1 , we insert the constant values
for all other joint variables θ2 , ..., θ6 into the equation, and obtain an
equation of a single variable (here variable θ1 ). We can now take the
derivative of this single-variable equation. To see why we have the
joint angle values for all joints, at the current position, we look at the
following argument: We step through the sequence of points p1 , ..., pn ,
and we can assume that we have the joint angles corresponding to pi ,
when we compute the joint angles for pi+1 . This argument holds for
p1 , since here we can read the encoders of our joints.
For the six-joint robot, we obtain a matrix of the form
 ∂ m11 ∂ m11 
∂ θ ... ∂ θ6
 1 
 ∂ m21 ∂ m21 
 ∂ θ ... ∂ θ 
J= 1 6 . (4.9)
 .. 
 . 
∂ m34 ∂ m34
∂ θ1 ... ∂ θ6
J is called the Jacobi-matrix for our robot, or the Jacobian, for short.
We can now set up an equation, relating joint increments to path in-
crements:
 
dnx  
dny  dθ1
   .. 
 ..  = J  .  . (4.10)
 . 
dθ6
d pz
Notice that the equation gives an approximation. Thus, we require the
increments to be small.
4.2 Jacobi-Matrices 139

Now J is a matrix of partial derivatives. Each matrix entry is a function

of a single variable θi . We fill all such variables with the joint value for
the current position (recall that the joint angles for pi are known, and
that the joint angles for pi+1 are yet unknown, and we compute them
in the process). Then J becomes a constant matrix. Furthermore, equa-
tion 4.10 is a linear system of equations, where the unknown values
are the joint angle increments dθ1 , ..., dθ6 , leading from pi to pi+1 .
However, equation 4.10 is not yet complete, i.e. we do not have all
ingredients yet. Specifically, the increments on the left hand side
are not all known. What we do know are the position increments
d px , d py , d pz . Thus, we know three values out of twelve. The matrix
increments dnx , ..., daz are yet unknown.
A simple case arises when the orientations at p1 and pn are the same.
Then we would have dnx = ... = daz = 0. However, this will not al-
ways be the case. So first note we can obtain a series of intermediate
orientations from the technique in example 4.2. Subtracting matrix Oi
from Oi+1 will give us the required matrix increments dnx , ..., daz .
An alternative way out (i.e. a second method for finding dnx , ..., daz )
will be described next. This alternative relies on so-called skew-
symmetric matrices, which are related to Jacobi-matrices.
Here the scenario is slightly different. We start from given angular
increments dα, dβ , dγ, referring to the axes of the base coordinate
system. Our goal is to find matrix increments dn, do, da, such that


n + dn o + do a + da (4.11)
describes the next orientation along the path. Here again,

(n o a) (4.12)
is the current (known) orientation.
We assume our angular increments dα, dβ , dγ refer to the x-, y- and
z-axes respectively. Hence, we can set up a matrix with these incre-
ments.
The matrix is obtained by multiplying the elementary rotations, i.e.
140 4 Joint Velocities and Jacobi-Matrices

R(z, dγ)R(y, dβ )R(x, dα) =

   
cdγ −sdγ 0 cdβ 0 sdβ 1 0 0
sdγ cdγ 0  0 1 0  0 cdα −sdα  . (4.13)
0 0 1 −sdβ 0 cdβ 0 sdα cdα

We now evaluate the matrix product in equation 4.13. To do this we

will use two simplifications, both arising from the fact that our angles
are small. Firstly, we set the cosine of each angle to 1. Thus we set
cdα = cdβ = cdγ = 1. Likewise, we use the approximation

sdα = dα (4.14)
for small angles. Furthermore, we set any products of two small angles
to zero.
Then, after multiplying, we obtain the approximation matrix D, ap-
proximating the matrix in equation 4.13, where

 
1 −dγ dβ
D =  dγ 1 −dα  . (4.15)
−dβ dα 1

To compute the unknown matrix increments dn, do, da we now pro-

ceed as follows. Set

D · (n o a) = (n + dn o + do a + da). (4.16)
We rewrite this equation:

(D − I) · (n o a) = (dn do da). (4.17)

where I is the 3-by-3 unit matrix.
This gives

    
0 −dγ dβ nx ox ax dnx dox dax
 dγ 0 −dα  ny oy ay  = dny doy day  . (4.18)
−dβ dα 0 nz oz az dnz doz daz
4.2 Jacobi-Matrices 141

All matrix entries on the left side of equation 4.18 are known. We
can thus compute the unknowns on the right hand side, simply by
multiplying the two matrices on the left side.

Remark 4.1

The matrix

 
0 −dγ dβ
S =  dγ 0 −dα  (4.19)
−dβ dα 0
is obtained as S = D − I, and has a special property, namely

S + ST = 0 (4.20)
Matrices with this property are called skew-symmetric. Notice that S
is not orthogonal, and does not represent a rotation.

(End of Remark 4.1)

After having found all constant values, equation 4.10 is a linear equa-
tion system, and we can solve for the variables dθ1 , ..., dθ6 .

Remark 4.2

The approach described in this section also provides a straightforward

method for inverse kinematics: given a 4 × 4 matrix, denoting a de-
sired goal position and orientation for our tool, we can move to this
goal incrementally. To this end, we simply place a line segment con-
necting the current position and the goal position. We then follow the
line segment with the above interpolation scheme. The Jacobi-matrix
defined above is not symmetric, nor does it have an inverse or a de-
terminant. In the form in equation 4.9, a Jacobi-matrix is simply ob-
tained by taking component-wise derivatives of a kinematic matrix.
However, the concept of component-wise derivatives can be exten-
ded. In the next section, we will see such extensions. This will help us
to find explicit velocity functions for the joints in a more systematic
way. (End of Remark 4.2)
142 4 Joint Velocities and Jacobi-Matrices

4.3 Jacobi-matrices and velocity functions

When looking at the six-joint robot in figure 3.1 or even the eight-
joint robot in figure 3.17 we see that the most important element is
the recurrent two-link sub-assembly with parallel joint axes. We have
discussed this two-link sub-assembly in several examples throughout
the preceding chapters and also in the introduction of this chapter (fig-
ure 4.1).
Why is this element the most important element? We have already
observed that our six-joint robot consists of two three-joint sub-
assemblies. In the kinematic analysis, we separated these two sub-
assemblies.
We now look at the first of these sub-assemblies. It is the well-known
three-joint robot from chapter 2. Here, the base joint determines a
coarse pre-positioning in the work space. Recall that the kinematic
analysis for this joint was particularly simple, and we were able to
decouple it from the entire rest of the robot. Now the next two joints
(joints 2 and 3 for the six-joint robot) do the actual manipulation at the
target, while all remaining joints only provide hand orientation. Thus,
roughly speaking, joints 2 and 3 provide the main elements of the
robot, when it comes to handling and manipulation. We give a name
to the corresponding sub-assembly, and refer to it as the (planar) two-
link manipulator.
We compute the forward matrix for a planar two-link manipulator. We
obtain

 
c12 −s12 0 l2 c12 + l1 c1
0
s12 c12 0 l2 s12 + l1 s1 
M2 = 
0 0 1
.
 (4.21)
0
0 0 0 1

Notice that here we refer to the two joints of our two-link manipulator
as joint 1 and joint 2, ignoring the fact that the two-link manipulator
may be part of a larger robot with more joints.
4.3 Jacobi-matrices and velocity functions 143

Our goal is to find explicit velocity functions for the two-link ma-
nipulator. We have already done this for the case of a C-arm, in the
introduction of this chapter, but this was a particularly simple case,
and we did not need any dedicated methods.
We consider the elements (1,4) and (2,4) of the matrix in equa-
tion 4.21. These two elements give the x- and y-positions of our tool.
Hence, we have

x = l2 c12 + l1 c1 ,
y = l2 s12 + l1 s1 . (4.22)

Restating these equations as functions of time, as the joints move, we

obtain

x(t) = l2 cos(θ1 (t) + θ2 (t)) + l1 cos(θ1 (t)),

y(t) = l2 sin(θ1 (t) + θ2 (t)) + l1 sin(θ1 (t)). (4.23)

Here θ1 (t) and θ2 (t) are two separate functions, describing the mo-
tions of the two joints over time.
We differentiate these two functions with respect to the time parameter
t.
This gives

x0 (t) = −l1 sin(θ1 (t)) · θ10 (t) − l2 sin(θ1 (t) + θ2 (t)) · (θ10 (t) + θ20 (t)),
y0 (t) = l1 cos(θ1 (t)) · θ10 (t) + l2 cos(θ1 (t) + θ2 (t)) · (θ10 (t) + θ20 (t)).
(4.24)

We recall the notation from equation 4.3. With this notation (replace
θi0 (t) by θ̇i ) and the familiar short forms si = sin(θi ) we rewrite equa-
tion 4.24 to

ẋ = −l1 s1 · θ̇1 − l2 s12 · (θ̇1 + θ̇2 )

ẏ = l1 c1 · θ̇1 + l2 c12 · (θ̇1 + θ̇2 ) (4.25)
144 4 Joint Velocities and Jacobi-Matrices

We now assemble the two functions into a matrix. This is done in a

purely symbolic way!
We set

   
x θ̇
p= , θ̇ = 1 . (4.26)
y θ̇2

Define a matrix J by setting

 
−l1 s1 − l2 s12 − l2 s12
J= . (4.27)
l1 c1 + l2 c12 l2 c12

Now J is arranged in such a way that it allows for writing equa-

tion 4.25 in a shorter form, namely

ṗ = Jθ̇ . (4.28)
You may wish to take a minute to multiply the product Jθ̇ and see that
this will indeed result in the right hand side of equation 4.25.
The matrix J is a 2-by-2 matrix, and has an interesting property: we
can invert it! Thus we can write

θ̇ = J−1 ṗ. (4.29)

Now, let us compute the inverse of J explicitly.
We recall that the inverse of an arbitrary 2-by-2 matrix A with
 
ab
A= (4.30)
cd
is given by
 
−1 1 d −b
A = . (4.31)
ad − bc −c a
Thus
 
−1 1 l2 c12 l2 s12
J = . (4.32)
l1 l2 s2 −l1 c1 − l2 c12 − l1 s1 − l2 s12
4.4 Geometric Jacobi-matrix 145

Now we can derive explicit velocity functions for the joints of our
two-link manipulator.
Suppose we wish to move the tool along the x-axis. Thus we set the
velocity along the x-axis to 1 and the velocity along the y-axis to 0.
Then
 
1
ṗ = . (4.33)
0
Multiplying, we obtain

 c12 
l1 s2
θ̇ = J ṗ =
−1
. (4.34)
− l2cs12 − lc112s2

This shows that the velocity will tend to infinity as our angle θ2 tends
to 0, i.e. as the arm stretches.
In equation 4.34 we have explicitly stated the non-linear joint velocity
functions which will move the tip along a straight line.

Remark 4.3

From equation 4.31 we see that we cannot invert the matrix A in equa-
tion 4.30 if ad − bc = 0. In this case the inverse A−1 does not exist,
and A is called singular. We apply this to equation 4.32. Here, J has
no inverse, whenever s2 = 0. This means that J is singular if θ2 = 0
or θ2 = π/2. Looking at the robot, we see that such a configuration
occurs, when the two links are aligned, i.e. the arm is stretched.

(End of Remark 4.3)

4.4 Geometric Jacobi-matrix

In literature, the matrix J defined in equation 4.27 is also called the

Jacobian, although it is somewhat different from the matrix defined in
equation 4.9. In the process of obtaining equation 4.9, we differenti-
ated with respect to all six joint angles. When deriving equation 4.27,
146 4 Joint Velocities and Jacobi-Matrices

we differentiated with respect to a single variable, namely the time

parameter t. Notice that the 2-by-2 matrix in equation 4.27 is invert-
ible, and has a determinant. The matrix in equation 4.9 is a 6-by-12
matrix, hence has no inverse, nor determinant.
But a further difference between the two Jacobians should be men-
tioned. This regards the process of setting up the matrix J in order to
arrive at the shorter form ṗ = Jθ̇ of the equations in equation 4.25.
This process did not involve taking partial derivatives. It is not imme-
diately clear how this process would generalize to the case of other
robots, with more joints.
Hence, we saw two versions of Jacobi-matrices. With the second ver-
sion, we were able to derive explicit functions for the joint velocities.
Our goal is now to derive a standardized version of the Jacobi-matrix.
This standardized version will also be called the geometric Jacobian.
To obtain a standard version, it is necessary to specify the goals of the
standardization. Our goal will be to find two 3 × n matrices, which we
will call Jv and Jω . (n is again the number of joints.) The two matrices
will then relate the joint velocities θ10 (t), ...θn0 (t) to the velocity of the
effector. We again write θ̇i as a shorthand for the time derivate θi0 (t),
and likewise, in vector notation,
 
θ̇1
 .. 
θ̇ =  .  . (4.35)
θ˙n
To describe the velocity of the effector, we will be somewhat more
specific than above. In particular, we will distinguish two types of
velocities, namely linear velocity and angular velocity. We next define
the latter two terms.
Suppose we have a coordinate frame S rotating within the base frame.
The axis of rotation is a (unit) vector a, given in the base frame. Also,
assume S has its origin at the origin of the base frame. We define the
angular velocity of the rotating frame as follows.

ω = θ̇ a (4.36)
Here θ̇ is a scalar function of time. Thus, the angular velocity ω is
simply a vector, the length of which gives the magnitude of the velo-
4.4 Geometric Jacobi-matrix 147

city, and the direction gives the axis of the rotation (here unit vector
a).
Now suppose we have a point p which is given in the coordinates of
the frame S, and rotates with S. We define the linear velocity of p as

v = ω × p. (4.37)
This definition may seem odd, and we visualize the effects of the latter
definition for the case of a small example.

θ1
z0
p1 v

x0 -y0 -plane

Fig. 4.5: Linear velocity v resulting from an angular velocity ω. Notice

that v is a vector in the x0 -y0 -plane. v is the cross-product of θ̇1 z0 and p1 .

Example 4.2

Figure 4.5 shows a one-joint robot, which moves in the x0 -y0 -plane.
The robot rotates about the z0 -axis. The joint angle is θ1 . The position
of the end effector is given by the point p1 in the center of the grip-
per. Now we move the only joint. The angular velocity is the scaling
factor θ̇1 of the vector z0 . The linear velocity of p1 is v. This linear
velocity v is the cross-product of θ̇1 z0 and p1 . We see that an increase
of the link length of the robot, or an increase of θ̇1 will increase the
linear velocity. By contrast, an increase in link length will not increase
angular velocity.
Thus, our linear velocity is in fact a linear velocity resulting from an
angular velocity, and it is an instantaneous velocity. Notice that the
vector v in figure 4.5 changes direction as the robot moves.
In figure 4.6 we see a planar robot with several revolute joints, again
moving in the x0 -y0 -plane. Suppose now only a single joint of this
148 4 Joint Velocities and Jacobi-Matrices

pi−1 /zi−1
v = ω × (pn − pi−1 )
θi
y0 pn

z0 x0

Fig. 4.6: Linear velocity v of the point pn , resulting from the motion
of a single joint (here joint i, with axis zi−1 ). The axes z0 and zi−1 are
orthogonal to the drawing plane, and are indicated by circles with dots.

robot is moving, namely joint i. According to the DH-rules, the joint

axis is the axis zi−1 . Here zi−1 is again orthogonal to our drawing
plane, and parallel to z0 , the z-axis of the base frame. The vectors pi
and pn give the coordinates of the origins of the DH-frames i and n,
respectively. Then the angular velocity of the DH-frame i is given by

ω = θ̇i zi−1 . (4.38)

The linear velocity v of the point pn , resulting from the rotation at
joint i, is then

v = ω × (pn − pi−1 ) = zi−1 × (pn − pi−1 )θ̇i . (4.39)

Notice now, that although the example in figure 4.6 is planar, the same
holds for the three-dimensional case, i.e. when the axis zi−1 is no
longer parallel to z0 .

(End of Example 4.2)

We should also note that the angular velocity, according to the above
definition, is a property of a frame, while the linear velocity is a prop-
erty of a point.
4.4 Geometric Jacobi-matrix 149

Composition of Angular Velocities

Above, we have defined an angular velocity as a vector. We have not

yet defined a composition for two angular velocities. It would seem
natural to define the composition of two angular velocities as the sum
of the two vectors. We will do just this now, but our definition will
require that the two vectors to be added must both be given in base
coordinates. Hence, for our definition, suppose a frame S1 rotates with
respect to the base frame S0 . Let ω0 be the angular velocity.
Then further, a frame S2 rotates with respect to S1 . Let ω1 be the latter
angular velocity.
We assume that S1 and S2 have their origin at the origin of the base
frame S0 .
Then we express the position of S1 with respect to S0 by a matrix 0 R1 .
0 R is a 3 × 3-matrix depending on the parameter θ . Hence, we will
1
write 0 R1 (θ ).
We now define the combined angular velocity as

ω = ω0 + 0 R1 (θ )ω1 . (4.40)
Then ω is again a vector, given with respect to the base frame, so that
we can iterate the definition, for more (revolute) joints.
Here we have simply defined the composite angular velocity by a vec-
tor addition. We must now confirm that this definition is in agree-
ment with our intuitive understanding of a combined rotation of two
coordinate frames. This will be done in the following example. The
example will also illustrate the application of angular velocities.

Example 4.3

We are given a robot with four revolute joints. The four joints form a
kinematic chain, as in typical robots. Now suppose the four rotations
are the following:
1. Rotation about the z-axis of the base frame S0 (angle α)
2. Rotation about the x-axis of the new coordinate system S1 (angle
β)
3. Rotation about the x-axis of S2 (angle −β )
4. Rotation about the z-axis of S3 (angle −α)
150 4 Joint Velocities and Jacobi-Matrices

Assume also that

α(t) = t,
β (t) = t. (4.41)

Hence,

−α(t) = −t,
−β (t) = −t. (4.42)

Then we have
   
0 1
ω0 = 0 ,
 ω1 = 0 ,
 (4.43)
1 0
and
   
−1 0
ω2 =  0  , ω3 =  0  . (4.44)
0 −1
From chapter 3 we know that the combined rotation is simply given
by the matrix product

R(z, α(t))R(x, β (t))R(x, −β (t))R(z, −α(t))). (4.45)

Multiplying these matrices, we obtain

 
100
R(z, α(t))R(x, β (t))R(x, −β (t))R(z, −α(t))) = 0 1 0 (4.46)
001

i.e. a zero rotation. But we are not supposed to know this yet. Instead
we would like to apply the composition rule for angular velocities. We
should then obtain a total angular velocity of zero.
Recall that we must express all angular velocities in the common base
frame. Now ω0 is already given in base coordinates.
To express ω1 in base coordinates, we simply multiply with the matrix
4.4 Geometric Jacobi-matrix 151

0
R1 = R(z, α(t)). (4.47)
Thus, the angular velocity of S2 with respect to S1 , given in base co-
ordinates is

R(z, α(t))ω1 . (4.48)

Likewise, ω2 is base coordinates is

R(z, α(t))R(x, β (t))ω2 . (4.49)

Finally, ω3 in base coordinates is

R(z, α(t))R(x, β (t))R(x, −β (t))ω3 = R(z, α(t))ω3 . (4.50)

Adding and evaluating, we obtain

ω0 + R(z, α(t))ω1 + R(z, α(t))R(x, β (t))ω2 + R(z, α(t))ω3 =

        
0 cα −sα 0 1 cα −sα 0 1 0 0 −1
0 + sα cα 0 0 + sα cα 0 0 cβ −sβ   0 
1 0 0 1 0 0 0 1 0 sβ cβ 0
    
cα −sα 0 0 0
+ sα cα 0  0  = 0 .
0 0 1 −1 0
(4.51)

(End of Example 4.3)

With the definitions of angular and linear velocity, we can specify our
goals with respect to the geometric Jacobian. Our goal is thus to find
two 3 × n matrices Jv and Jω , such that the linear velocity v and the
angular velocity ω of the end effector can be expressed in terms of the
joint velocity vector θ̇ as

v = Jv θ̇ ,
ω = Jω θ̇ . (4.52)
152 4 Joint Velocities and Jacobi-Matrices

The two parts Jv and Jω of the geometric Jacobian will then be as-
sembled into a single matrix
 
Jv
J= . (4.53)
Jω
We then have
 
v
= Jθ̇ . (4.54)
ω
This is a direct extension of equation 4.28. Notice again that in this
equation, both the angular and the linear velocity (of our effector) are
given in terms of the base frame.

Theorem 4.1:

For a robot with n revolute joints, the geometric Jacobian is given by

   
Jv z0 × (pn − p0 ) z1 × (pn − p1 ) . . . zn−1 × (pn − pn−1 )
J= = .
Jω z0 z1 ... zn−1
(4.55)
Here again, pi denotes the position of the origin of the i-th DH-frame,
and can be read from the last column of the DH-matrix 0 Mi (see
chapter 3). Likewise, zi is short for
 
0
0  
Ri 0 , (4.56)
1
where 0 Ri is the 3 × 3 rotational part of 0 Mi . Clearly, both pi and zi
can be computed with the forward analysis methods from chapter 3.
Notice that the theorem requires robots with revolute joints. In case
joint i is a prismatic joint, we replace the column
 
zi−1 × (pn − pi−1 )
(4.57)
zi−1
by the column
4.4 Geometric Jacobi-matrix 153
 
zi−1
. (4.58)
0
Before we prove the theorem, we will look at two examples.

Example 4.4

We begin with the one-joint robot from figure 4.5. Here, we have
 
c1
pn = p1 = s1  .
 (4.59)
0
Also,
 
0
z0 = 0 . (4.60)
1
Thus

     
0 c1 −s1
z0 × (p1 − p0 ) = z0 × p1 = 0 × s1  =  c1  . (4.61)
1 0 0
This gives Jv1 .
Moving joint 1 with a magnitude of θ̇1 will hence result in a linear
velocity of p1 of
 
−s1
θ̇1  c1  . (4.62)
0
We see that this is consistent with the linear velocity v in our defini-
tion, which we had looked at in example 4.2 (see equation 4.39).
For Jω1 , we have
 
0
Jω1 = z0 = 0 .
 (4.63)
1
Thus the angular velocity (again joint 1 moving at magnitude θ̇1 ) is
154 4 Joint Velocities and Jacobi-Matrices

 
0
 0 . (4.64)
θ̇1
Therefore the full geometric Jacobian is
 
−s1
 
   c1 
Jv  0 
=  0 .
 (4.65)
Jω  
 0 
1
Very similarly, for the two-link manipulator (see figure 4.1), we obtain
 
−l1 s1 − l2 s12 −l2 s12
 
   l1 c1 + l2 c12 +l2 c12 
Jv  0 0 
J= = 
. (4.66)
Jω  0 0  
 0 0 
1 1
We compare to the matrix derived in equation 4.27. Clearly, the first
two lines are the same. The third line in equation 4.66 has only zeros.
This is simply due to the fact that we now look at the Jacobi-matrix for
a full three-dimensional robot, and our planar two-link manipulator
can never leave the plane. Finally, the three lines at the bottom of
equation 4.66 describe angular velocity, and we see that we add the
angular velocity from the two joints.
Finally, we look at an extension of the two-link manipulator. We add
a third revolute joint at the end of the two-link manipulator. The entire
mechanism can only move in the plane. The length of the third link is
l3 .
In this case, we obtain the geometric Jacobi-matrix
4.4 Geometric Jacobi-matrix 155
 
−l1 s1 − l2 s12 − l3 s123 −l2 s12 − l3 s123 −l3 s123
 l1 c1 + l2 c12 + l3 c123 l2 c12 + l3 c123 l3 c123 
 
 0 0 0 

J= . (4.67)
0 0 0 
 
 0 0 0 
1 1 1

(End of Example 4.4)

We now turn to the proof of theorem 4.1, for revolute joints.

Proof of Theorem 4.1 (Revolute Joints)

The matrix in equation 4.55 consists of two lines. We will first con-
sider the second line.
We will see that it follows directly from the composition rule for an-
gular velocities.
We must show (compare equation 4.54) that
 
θ̇1
 .. 
ω = Jω  .  . (4.68)
˙
θn
Given the definition of J in equation 4.55, we have


Jω = z0 z1 . . . zn−1 , (4.69)
and we must show that

ω = z0 θ̇1 + z1 θ̇2 + . . . + zn−1 θ̇n . (4.70)

From the composition rule (equation 4.40), we have

ω = ω0 + 0 R1 ω1 + . . . + 0 Rn−1 ωn−1 . (4.71)

Each individual rotation is a rotation about the z-axis of a DH-frame,
hence (written in coordinates of DH-frame Si ) we have
156 4 Joint Velocities and Jacobi-Matrices
 
0
ωi = 0 θ̇i+1
 (4.72)
1
for 0 ≤ i < n.
We insert this into equation 4.71, and obtain
     
0 0 0
0 0
ω = 0 θ̇1 + R1 0 θ̇2 + . . . + Rn−1 0 θ̇n .
     (4.73)
1 1 1
But by the definition in equation 4.56,
 
0
0 = z0 (4.74)
1
and
 
0
0  
Ri 0 = zi . (4.75)
1
Comparing to equation 4.56 gives the desired result for the second
matrix line.
We now turn to linear velocities, i.e. the first line of the matrix in the
theorem.
We must show

v = z0 × (pn − p0 )θ̇1 + z1 × (pn − p1 )θ̇2 + . . . + zn−1 × (pn − pn−1 )θ̇n

(4.76)
We first assume, only a single joint, namely joint i, is moving. All
other joints do not move. We observe that in this case, we already
have a proof of the result. If we move only a single joint, then what
we must show simply reduces to equation 4.39.
The case of multiple moving joints will now directly follow from the
composition rule for angular velocities. Recall that we defined the lin-
ear velocity v of a point p (resulting from an angular velocity) in equa-
tion 4.37. i.e. v = ω × p.
4.4 Geometric Jacobi-matrix 157

Assume now we have two linear velocities v1 and v2 both acting on a

point p.
Further assume v1 and v2 stem from angular velocities ω1 and ω2
respectively, so that

v1 = ω1 × p, (4.77)
v2 = ω2 × p. (4.78)
(4.79)

But then v1 and v2 are vectors, and we can add them:

v1 + v2 = ω1 × p + ω2 × p (4.80)
By linearity, the right hand side of the last equation reduces to

ω1 × p + ω2 × p = (ω1 + ω2 ) × p. (4.81)
We see that the vector v1 + v2 also represents a linear velocity, act-
ing on the same point p. But this new linear velocity stems from an
angular velocity ω, for which we have

ω1 + ω2 = ω. (4.82)
Similar to the composition rule for angular velocities, we thus obtain
a composition rule for linear velocities (stemming from angular velo-
cities), which holds whenever the vectors are defined within the same
base frame. But now the desired result in equation 4.76 follows from
the fact that all vectors zi and pi are given with respect to the base
frame.

(End of Proof)

In the following example we will set up the geometric Jacobian for a

C-arm.

Example 4.5

In equation 4.3 we derived explicit velocity functions for CT-imaging

with the C-arm. Recall that we rotate the orbital axis of a C-arm such
158 4 Joint Velocities and Jacobi-Matrices

that the center point of the beam axis (point p) remains at the same
position. We saw that we must compensate with the two prismatic
joints.
The derivation in equation 4.3 was elementary, and we did not use
Jacobi-matrices.
We will now set up the geometric Jacobian for CT-imaging with the
C-arm. We can simplify the construction. As discussed in the intro-
duction of this chapter, only three joints are needed in this case. Thus,
we set the joint angles θ2 and θ4 to zero. Figure 4.7 shows the null
position and the coordinate systems.

x2 z3 y3
z2
a5
y2 p x3

a4
d3

z1
x1
y1
d1
z0

y0
x0

Fig. 4.7: Null position and coordinate systems for CT imaging with the
C-arm.

We obtain the DH-parameters shown in the following table.

4.4 Geometric Jacobi-matrix 159

i αi ai di θi
1 −90 0 d1 0
2 90 a4 d3 −90
3 90 a5 0 θ5 + 90
From the DH-parameters, we obtain the matrices
 
1 0 0 0
0
0 0 1 0 
M1 =  
0 −1 0 d1  , (4.83)
0 0 0 1
 
0 0 −1 0
1
−1 0 0 −a4 
M2 =  0 1 0 d3  ,
 (4.84)
0 0 0 1
 
−s5 0 c5 −a5 s5
2
 c5 0 s5 a5 c5 
M3 =  0 1 0 0 .
 (4.85)
0 0 0 1
Multiplying, we find the following:
 
0 0 −1 0
0
0 1 0 d3 
M1 · 1 M2 =  
1 0 0 a 4 + d 1  (4.86)
00 0 1
 
0 −1 0 0
0
 c5 0 s5 a5 c5 + d3 
M1 · 1 M2 · 2 M3 =  
−s5 0 c5 −a5 s5 + a4 + d1  (4.87)
0 0 0 1
Hence,
   
0 0
p0 = 0 ,
 p1 = 0 
 (4.88)
0 d1
and
160 4 Joint Velocities and Jacobi-Matrices
   
0 0
p2 =  d3  , p3 =  a5 c5 + d3  . (4.89)
a4 + d1 −a5 s5 + a4 + d1
Finally,
   
0 0

z0 = 0 , z1 = 1
 (4.90)
1 0
and
   
−1 0
z2 =  0  , z3 = s5  . (4.91)
0 c5
The first two joints are prismatic. Thus


Jv = z0 z1 z2 × (p3 − p2 ) . (4.92)
Evaluating, we obtain
 
00 0
Jv = 0 1 −a5 s5  . (4.93)
1 0 −a5 c5
We have

ṗ = Jv θ̇ . (4.94)
Since we require p to remain fixed, we have
 
0
ṗ = 0 . (4.95)
0
Inserting this into equation 4.94, we obtain
    
0 00 0 d˙1
0 = 0 1 −a5 s5  d˙3  . (4.96)
0 1 0 −a5 c5 θ˙5
Furthermore, we move the orbital joint (joint 5) with constant velocity
during CT-imaging. Therefore, we can assume that
4.5 Singularities and Dexterity 161

θ˙5 = θ50 (t) = 1. (4.97)

Hence, the last two matrix lines of equation 4.96 show that

d˙1 − a5 cos(t) = 0,
d˙3 − a5 sin(t) = 0 (4.98)
or that

d˙1 = a5 cos(t),
d˙3 = a5 sin(t). (4.99)
We have thus confirmed the result in equation 4.3 using the geomet-
ric Jacobian. The approach taken in equation 4.3 was elementary. We
see that the geometric Jacobian is a general tool for finding velocity
functions. We will see other applications of the geometric Jacobian
in later chapters. Specifically, the geometric Jacobian will be helpful
when analyzing forces and joint torques.

(End of Example 4.5)

Example 4.6

Appendix C lists the DH-matrices for the six-joint elbow manipulator

and also for the Kuka-DLR seven-joint robot discussed in chapter 3.
From the DH-matrices we derive the geometric Jacobians for both
robots.

(End of Example 4.6)

4.5 Singularities and Dexterity

An n×n-matrix J is called regular if the inverse J−1 exists. Otherwise,

J is called singular. In remark 4.3 we saw that the Jacobian matrix of
162 4 Joint Velocities and Jacobi-Matrices

a given robot can be singular or regular, depending on the robot con-

figuration. We will call a robot configuration singular if the Jacobian
for this configuration is singular. Singular configurations will also be
called singularities, for short.
We saw above that the Jacobian is not always an n × n-matrix. Rather,
for the case of the three-joint robot we obtained a 6 × 3-matrix (see
equation 4.67). In this case, (n × m-matrix, m < n) we would also like
to define singularities, in much the same way. Here, a singularity is a
configuration in which the Jacobian has rank less than m.
Extending this approach, we can look at products of the form JT J.
Assume J is a 6 × 3-matrix. Then JT J is a 3 × 3-matrix. We now use
the value d defined in the following equation as a measure for the
dexterity of the robot at a given configuration:
p
d= | det (JT J)| (4.100)
Clearly, this definition will work not just for 6 × 3-matrices, but for
general n × m-matrices.
Comparing to the dexterity measures defined in chapter 3, we see that
computing the solid angle requires evaluating orientations in a large
grid, while the measure d gives an analytic description of the local
dexterity.

Exercises

Exercise 4.1 Velocity Kinematics

Insert the functions from equations 4.1 and 4.2 into the forward 4 × 4-
matrix for the C-arm, and verify that the target point O5 remains fixed
while moving t from 0 to 2π.

Exercise 4.2 Linearized Rotation Matrix

Derive the matrix D in equation 4.15.

4.5 Singularities and Dexterity 163

Exercise 4.3 Interpolating Matrixes

Given two orientation matrices, such as the matrices O and O0 in equa-

tions 4.5 and 4.6, find a series of interpolation matrices, and visualize
the result in a small program.

Exercise 4.4 Joint Velocities

Write a short program for verifying the result in equation 4.34. Your
result should look like the image in figure 4.8.

0.05 tool position

J2 increments
0.04

0.03

0.02

0.01

−0.01

−0.02

−0.03

−0.04

−0.05

140 150 160 170 180 190 200

Fig. 4.8: Joint velocity (for joint 2) for moving the tool tip of a two-link
manipulator along a line segment, see equation 4.34.

Exercise 4.5 Analytic Jacobians

Consider the two-link manipulator in figure 4.1. Assume the two link
lengths are equal to 1. For this robot, the position of the tool tip is
given by the equation
   
px c1 + c12
= . (4.101)
py s1 + s12
164 4 Joint Velocities and Jacobi-Matrices

We now assume our current position is given by θ1 = π/2 and θ2 =

−π/2. This is the position shown in figure 4.9.
For this position of the robot, we evaluate the matrix equation
  !
∂ px ∂ px  
dx ∂ θ1 ∂ θ2 dθ1
= ∂ py ∂ py . (4.102)
dy dθ2
∂ θ1 ∂ θ2
Show that at the current position the matrix
∂p ∂p
!
x x
∂ θ1 ∂ θ2
∂ py ∂ py (4.103)
∂ θ1 ∂ θ2
evaluates to
 
−1 0
. (4.104)
1 1
We thus have

dx = −dθ1 ,
dy = dθ1 + dθ2 . (4.105)
Solving this for dθ1 and dθ2 , we obtain

dθ1 = −dx,
dθ2 = dx + dy. (4.106)
Visualize this result by moving the robot tip to four positions in the
vicinity of the current tip position.

Exercise 4.6 Geometric Jacobians

Verify the results in equations 4.66 and 4.67 by direct calculation.

Exercise 4.7 Velocity Kinematics for the C-arm

The C-arm has 5 joints. We fix two of these joints, namely joints 2
and 4, and set the joint angles for these two joints to zero. Then the re-
maining mechanism has 3 joints. For non-vertical imaging, we include
4.5 Singularities and Dexterity 165

l2
p

l1
y

Fig. 4.9: Two link manipulator for position θ1 = π/2 and θ2 = −π/2

the angulation axis, i.e. joint 4. Set up the upper part of the geometric
Jacobian matrix Jv for arbitrary non-vertical positions. Derive the ex-
plicit velocity functions for non-vertical CT-acquisition based on the
Jacobian. Compare to the result from section 4.1.
166 4 Joint Velocities and Jacobi-Matrices

Exercise 4.8 Trajectories

Suppose we have a six-joint robot with revolute joints. The so-called

joint space of the robot is a six-dimensional space. Each coordin-
ate corresponds to one joint angle. Thus, a position of the robot
(given angle values for all joints) can be represented by a point
p = (a1 , ..., a6 )T in joint space. Likewise, a curve in joint space rep-
resents a motion of the robot.
It is often necessary to specify a robot motion by a set of via-points.
The via-points are points in joint space. Our goal is now to connect
the via-points by a continuous path. We assume the total time for the
motion is 1 second. In the most simple case, the only via-points are
the start and the goal point, denoted by p and q.
Both p and q are six-dimensional vectors. (Notice we still assume a
six-joint robot here. For a seven-joint robot, our joint space is seven-
dimensional.) We can connect p and q in a variety of ways.
Let

p = (a1 , ..., a6 )T ,
q = (b1 , ..., b6 )T .
(4.107)

We look at the path of a single angle, e.g. the first one. Then we must
move θ = θ1 , i.e. the first angle, from a1 to b1 .
Furthermore, we can constrain the velocity of the joint during the mo-
tion. It is useful to require θ 0 (0) = 0 and θ 0 (1) = 0, i.e. the velocity
at the two end points of the motion should be zero. We thus have four
constraints on the trajectory (for the first joint).
Thus

θ (0) = a1 ,
θ (1) = b1 ,
θ 0 (0) = 0,
θ 0 (1) = 0.
(4.108)
4.5 Singularities and Dexterity 167

A cubic polynomial is a polynomial of the form

θ (t) = c0 + c1t + c2t 2 + c3t 3 . (4.109)

a) Compute values for the coefficients ci of the polynomial such that
the constraints in equation 4.108 are satisfied.
Hint: from the constraints, you can derive four linear equations
with the variables ci .
b) Above we had assumed the total time for the path to be 1s. Gener-
alize to the case of arbitrary total time.
c) Generalize the result from part a) to the case of n via-points.
Hint: assume we specify a non-zero velocity u at the start point
of the trajectory. Likewise, v denotes the velocity at the end point.
We first generalize the equations derived in a) to this case. Then a
path with several via points is simply obtained by requiring zero
velocities at the start and end point, and appropriate non-zero velo-
cities at the intermediate points. Two consecutive segments share
the velocity value at the common point.

Summary

Similar to the forward analysis of a manipulator via the DH-rules, the

Jacobi-matrix in equation 4.27 can be derived in a systematic way, for
arbitrary kinematic linkages. The Jacobi-matrix is given by

 
z0 × (pn − p0 ) z1 × (pn − p1 ) . . . zn−1 × (pn − pn−1 )
J= .
z0 z1 ... zn−1
(4.110)
With the Jacobi-matrix, we can express the angular and linear velo-
cities of the end effector as functions of the joint velocities. In appro-
priate cases, the Jacobi-matrix is invertible. By inverting the Jacobi-
matrix, we can do the opposite, namely derive explicit functions for
the joint motions needed to produce a given velocity at the effector.
The Jacobi-matrix is useful for many other applications. Thus, we can
characterize singular configurations of the robot. As a further applic-
168 4 Joint Velocities and Jacobi-Matrices

ation, in chapter 9 we will derive expressions for the force applied by

the end effector, given joint torques.

Notes

The text book by Spong, Hutchinson and Vidyasagar [1] gives a de-
tailed analytical derivation of the geometric Jacobi-matrix (including
prismatic joints), together with an analysis of singularities and ma-
nipulability for many robot constructions. This includes an analytical
proof for the composition rule for angular velocity, based on algebraic
properties of skew-symmetric matrices. A similar approach is taken in
the text book by John Craig [2]. Our description summarizes the res-
ults from [1] and [2].

References

[1] M. W. Spong, S. Hutchinson, and M. Vidyasagar, Robot Modeling

and Control, 1st ed. New York: John Wiley & Sons, Inc., 2005.
[2] J. J. Craig, Introduction to Robotics: Mechanics and Control,
3rd ed. Prentice Hall, 2005.
Chapter 5
Navigation and Registration

In the preceding chapters we discussed mathematical methods for ana-

lyzing robot motion. We will now see how a robot can interact with
its environment in a medical application.

Input image A Input image B

Output: θ, Dx, Dy

Fig. 5.1: Registration

169
170 5 Navigation and Registration

Typical medical navigation systems rely on medical imaging. Med-

ical imaging is a vast field of its own, but here we only need a basic
connection to robotics. The main elements for establishing this con-
nection are image registration and calibration.
We saw two examples for navigation methods in the introduction
(chapter 1). The examples were: stereotaxic navigation and radiolo-
gic navigation. In both cases registration was needed.
Registration brings structures into alignment (figure 5.1). In the most
simple case, we register two images showing the same object, e.g. a
femur bone.
Assume the two images are given as data files. Since the two im-
ages are different, the bone will show up in two different positions.
We place the second image on top of the first, and manually ro-
tate/translate the top image in the graphical user interface until align-
ment is reached. We then output the rotation angle θ , as well as the
translational displacement (∆ x, ∆ y) in x- and y-directions (figure 5.1).

Fig. 5.2: Registration of a femur head with C-arm x-ray images (2D-3D
registration)

In this example, the registration is done manually. We now consider a

more complex example (figure 5.2). This example illustrates the con-
5.1 Digitally reconstructed radiographs 171

nection between navigation and registration. Here, the goal is to re-

gister the position of a femur bone in space, during surgery.
We first observe that the head of a femur bone has a spherical shape.
Thus we can proceed as follows. We take two x-ray images of the
femur head from two different angles with a C-arm. We record the po-
sition of the C-arm for each of the two images, via markers attached to
the C-arm. In each x-ray image, we mark the femur head by drawing
a circle onto the image. The software then constructs two cones. The
cones are defined by the C-arm’s source points (in two distinct posi-
tions for the two cones) and the circles delineating the femur head. By
intersecting the two cones, we obtain a sphere in space, which corres-
ponds to the current position of the femur head.
This example (figure 5.2) illustrates several basic concepts. Firstly,
we have an example of stereo x-ray imaging as a basis for navigation.
Secondly, we have an example of a 2D-3D registration. The two im-
ages (x-ray) are two dimensional, and the structure to be registered is
three-dimensional. Strictly speaking, we do not register two images in
this example. Instead, we register a pair of images (x-ray images) to a
model of an anatomical target. This model is a sphere.
Notice that the general problem of registration has a much larger range
of applications than just navigation. However, surgical navigation re-
lies on dedicated registration methods. We will focus on this type of
registration in this chapter.
Several different types of registration methods are frequently used:
landmark-based registration, point-based registration, contour-based
registration, intensity-based registration and elastic registration. We
will see examples for each type in this chapter.

5.1 Digitally reconstructed radiographs

We now consider digitally reconstructed radiographs (DRRs). They

form the basis for a widely used 2D-3D registration method.
Recall the reconstruction of a CT image from a series of x-ray images,
(figures 1.19 and 1.20, see also exercise 1.2). The reconstruction is
based on a series of x-ray images from a range of different angles.
172 5 Navigation and Registration

CT image stack

synthetic X-ray image

Fig. 5.3: Digitally reconstructed radiographs

Now we invert the reconstruction process. We take the CT data set,

and project it onto a 2-dimensional surface (figure 5.3). We project
along a fixed direction in space. This resembles the process of taking
an x-ray image, but now our x-ray image is synthetic. We repeat the
projection for a large number of distinct directions. Thus we obtain a
set of digitally reconstructed x-ray images (or radiographs), in which
each image corresponds to one direction.
To apply DRRs in registration we proceed as follows. We compute
the DRRs for the anatomical region before the intervention, and then
take stereo x-ray images during the intervention (figure 5.4). The in-
teroperative x-rays are called live images. To find an alignment posi-
tion, we search for the pair of DRRs which matches the live images.
The matching step simply consists of subtracting the synthetic images
from the live images, which gives the result of our registration.
The subtraction proceeds pixel by pixel. This gives an error signal
and we determine the DRR pair giving the minimal error signal. Since
DRRs were computed from different directions, we can then extract
the registration information (angles and translational displacements)
for the live images.
5.2 Points and Landmarks 173
X-ray source A X-ray source B

target

X-ray detector B X-ray detector A

Fig. 5.4: Stereo x-ray imaging. Two fixed x-ray sources and two fixed
detectors.

One typical application of DRR-registration is the registration of the

human skull for treating brain tumors. Here fixed x-ray sources and
detectors are used, and typically 400-600 pairs of DRRs are matched
to intra-operative live shots.

5.2 Points and Landmarks

Assume we have two clouds of points in space. Call the clouds A and
B. Our goal is to bring the two clouds into registration. This means
we must move cloud B in such a way that the two clouds match. We
allow the point cloud B to move under rotation and translation, but we
do not allow B to deform. Thus our goal is to compute a translation
vector t and a 3 × 3 orientation matrix R such that the set of points
Rb + t with points b from B will match A. We will also call B the
floating point set, and A the fixed point set.
For simplicity we will first assume that the number of points in both
clouds is the same, and that we know in advance which point b from
B should be matched to which point a from A. With other words,
we know the correspondence between the two sets. We thus assume
the points in A and B are ordered in such a way that the corres-
pondences are simply given by the indexing, i.e. A = (a1 , . . . , an ) and
B = (b1 , . . . , bn ), where ai should by matched to bi .
174 5 Navigation and Registration

We are looking for a position, in which the sum of squared distances

between the point pairs (ai , bi ) is minimal. Then the following ap-
proach computes a matrix R and a vector t.
Since B is moving under the transformations R and t, we have an
expression f containing point-wise distances, with

f =k Rb1 + t − a1 k2 + · · · + k Rbn + t − an k2 , (5.1)

and our goal is to minimize the value of f . To simplify the problem,
we first look at two-dimensional point sets. Then the matrix R has the
form
 
cos θ − sin θ
R= . (5.2)
sin θ cos θ
The minimization function f has three parameters θ ,tx ,ty where θ is
the angle in the matrix R and tx ,ty are the coordinates of the vector t:

f (θ ,tx ,ty ) =k Rb1 + t − a1 k2 + · · · + k Rbn + t − an k2 (5.3)

For small angles, we can replace the expressions sin θ and cos θ (oc-
curing in R) by linear approximations: Set cos θ to 1 and sin θ to θ .
We thus replace R from equation 5.2 by
 
1 −θ
. (5.4)
θ 1
The function f is now a quadratic function in the three parameters tx ,ty
and θ . Notice that the coordinates of the points ai , bi are constants
here.
Taking the derivatives with respect to tx , then with respect to ty and
with respect to θ gives three linear expressions. We set these three lin-
ear expressions to zero and obtain a system of three linear equations
with three variables. The solution of this system gives an approxima-
tion for the desired parameters tx ,ty and θ .
In the three-dimensional case, we can use the same method with one
minor difference. Here the matrix R depends on three angles α, β , γ.
We can linearize this matrix in the following way. Replace R by
5.2 Points and Landmarks 175
 
1 −γ β
D =  γ 1 −α  . (5.5)
−β α 1
We see that the matrix D is the matrix from equation 4.15. Thus, recall
that D − I is a skew-symmetric matrix. The derivation of the matrix D
is not difficult, and is given in chapter 4.
We again obtain a linear system of equations after taking derivatives.
In this case we have a linear system of six equations in the six vari-
ables tx ,ty ,tz , α, β , γ.

Example 5.1

Suppose we wish to register the head of a patient to preoperative MR-

data. This means, we have MR images of the patient. During the op-
eration, we must move an instrument to a position marked on this MR
image.

+ o
+
+ o
+ + o
+ o
+ +

Fig. 5.5: Registration with landmarks

Figure 5.5 illustrates registration with landmarks, based on point

cloud matching.
To register patient data during the intervention, we proceed as follows.
We mark four landmark points on the head in the MR image data set
(see figure). Additionally, we delineate the target in the brain on one
or more of the MR slices. During the intervention, we touch the same
four points (in the same order!) with the pointer of a tracking system,
and record the positions (points marked as small circles in the figure).
We can then infer the position of the internal target point in the brain
from the current position of the marker points.
176 5 Navigation and Registration

In this example we consider the robotic system for transcranial mag-

netic stimulation in figure 1.16. A magnetic coil is held by a robot
arm. The coil generates a short magnetic pulse, and acts as a stimu-
lator for small regions on the cortex. The surgeon delineates a target
on an MR image of the brain. The registration then provides a way to
move the coil to the target. Repeated registration then allows for keep-
ing the coil in the planned position during small involuntary motions
of the patient.

(End of Example 5.1)

5.2.1 Iterative Closest Points

In many applications, point-to-point correspondences between the sets

A and B are not known in advance. In addition, the sets A and B may
have different sizes (figure 5.6).

+ + + + o o
+ + o o
+ + o o oo
+ ++ + ++
+ + + oo
+
+ + +
+ +
+ + + o o o
+ + o

Fig. 5.6: Registration with surface points

Here the points are on the head surface, automatically extracted from
an MR data set. A second cloud of points is acquired later, and
our goal is to register the two point sets. In this case, an additional
procedure must iteratively compute the best set of correspondences.
Only heuristic methods are known for this problem, since the number
of possible pairwise correspondences for two sets of n points is n!.
5.3 Contour-based Registration 177

The iterative-closest-point method (ICP method) is an approximation

method for this purpose.
ICP proceeds as follows. We again have two point sets A and B, but
now the number of points in the two sets does not necessarily have
to be the same. ICP alternates between a matching step and a moving
step. The matching step determines a set of correspondences between
points in A and B. The moving step computes a rotation matrix R and
a translation vector t.
Set R = I (the identity matrix), t = 0 and a random matching mnew
between points A and B.
Repeat
1. Set mold = mnew
2. Moving step: Given this matching mnew , compute R and t.
3. Matching step: Compute a new matching mnew between the point
sets after having moved the points in B by R and t.
Until mnew = mold
The stopping criterion can be modified, depending on the application.
The moving step has been discussed above. For the matching step,
proceed as follows. For each a in A, take the closest point in B, and
add this match to the set of correspondences. Obviously, since the
sizes of the sets A and B may be different, this will typically not result
in a one-to-one matching. In this case, we can simply stop adding
correspondences whenever one of the two sets has been exhausted.
For the matching step, we compute the nearest neighbors of the points
by k-d-trees (see exercise 5.2 at the end of this chapter).

5.3 Contour-based Registration

Contour-based registration is a direct extension of the above point-

based registration method. Instead of registering points in a set A to
points in a set B, we now have contours in one or both sets. Suppose
we are given a surface model of a bone reconstructed from CT data
and projected bone contours from an x-ray image. Our goal is to es-
timate the spatial position of the bone.
178 5 Navigation and Registration

To this end we want to place the surface model in such a way that the
2D bone contours match optimally.
Contour-based methods need contours, and hence segmented im-
ages to start from. Let us first assume the entire problem was two-
dimensional. This 2D version of the problem does not have much
practical value. But the discussion can be simplified quite substan-
tially. Thus, we assume we have a 2D object and take images of this
object from two camera positions C and C0 (figure 5.7).
In the 2D case the projected object contours each consist of two points
and there are four tangents la , lb , la0 , lb0 to the object. We can determine
these four tangents, since C and C0 are known (figure 5.7).
We manually place the bone model in an approximate position over
the x-ray images. This gives us an initial estimate for the bone posi-
tion. Of course, this initial placement is not optimal and needs to be
refined. In many cases, CT images are taken in standard positions, and
x-ray images are also taken from standard angles, so that this estimate
for the pose of the model is already known, and the step of manually
placing the model is not needed.

la lb

la0
C0
lb0

Fig. 5.7: Two-dimensional illustration of contour-based registration.

Known camera positions C,C0 . Tangents touch the contour of the object.

After having obtained the initial estimate, we can extract four points
a, b, a0 , b0 on the bone model in the following way: We extract a
second set of four tangents, but now these tangents touch the bone
model (in the estimated pose).
5.3 Contour-based Registration 179

We then minimize the point-line-distances between the original tan-

gents la , lb , la0 , lb0 and the surface points a, b, a0 , b0 on the model.

Tangent for the

Tangent point a0
estimated pose

Initial estimate la0 C0

Fig. 5.8: Estimated bone position. We refine the estimate by minimizing

the point-line distance between the original tangents (dashed lines) and
the tangent points a, b, a0 , b0 .

As in the case of point-based registration, R is a rotation matrix with

parameter θ and t is the translation vector with entries tx and ty .
The minimization problem is a slight variation of the function in equa-
tion 5.3:

f (θ ,tx ,ty ) = d(Ra + t, la )2 + · · · + d(Rb0 + t, lb0 )2 (5.6)

d(p, l) is the distance of point p from line l. We can compute the
distance of the point a from the line la via:

d(a, la )2 = (a − u − λ0 (v − u))2 , (5.7)

for appropriate λ0 , and la = u + λ (v − u).
We minimize this expression by taking the derivative with respect to
λ , and setting to zero:

(a − u − λ (v − u))(v − u) = 0. (5.8)
The point on the line la which is closest to the point a thus has the
λ -value
180 5 Navigation and Registration
(a − u)(v − u)
λ0 = . (5.9)
(v − u)2
Thus, for λ = λ0 we obtain a closed formula for the quadratic distance
d(a, la )2 . We insert the values for λ0 into the above function f . This
way, we can resolve the distance expressions:

d(a, la )2 = (a − u − λ0 (v − u))2 . (5.10)

Notice that we cannot cancel the term (v − u) in equation 5.9. The
reason is: a, v and u are vectors, and canceling would result in a divi-
sion of vectors, which is undefined! But the nominator and denomin-
ator in equation 5.9 are both scalars, thus division is permitted.
As above, we linearize rotation (see equation 5.4). After this step, the
function f is a quadratic function in the three parameters tx ,ty and
θ , and we can use the same approach as above to solve the resulting
linear system of equations.
A minor problem remains. Tangent points may move by a large
amount after a small rotation (see figure 5.9). To address this prob-
lem, we do not take just one initial position and solve the system of
equations for this particular position. We take several initial positions,
each with a slightly modified orientation.

la la
a

Fig. 5.9: Small rotations may move tangent points by large amounts.
5.4 Intensity-based Registration 181

5.3.1 Three-dimensional case

In the three-dimensional case, the contours of the projections become

curves in two dimensions. Now the contour does not consist of two
points, but rather of a curve, or a set of image points. As in the 2D case,
tangents are drawn connecting each contour point with the camera
origin, and the tangent points are determined. These points lie on the
surface model.
As above, our target function f has six parameters tx ,ty ,tz , α, β , γ, and
we use the linear version of the rotation matrix given above to obtain
a linear system of equations.

Example 5.2

In MR images, the surface of a bone is often not well visible. This is

due to the fact that the tissue on the bone surface, or close to the bone
surface gives the same MR-signal as the cortical bone. However, the
inner contour of the bone (contour between cortical bone and spongy
bone ) is typically very clear in MR images (see also figure 1.6). This
inner contour is also visible in x-ray images. This observation allows
for a contour-based x-ray to MR registration.

(End of Example 5.2)

5.4 Intensity-based Registration

For contour-based registration, it is necessary to find the contours

first. To avoid this, another class of registration algorithms has been
developed: intensity-based registration algorithms. No contours are
needed and we only use the gray levels of the image pixels. The idea
is shown in figure 5.10.
Obviously, for the example in figure 5.10, we obtain the registration
position for the MR image by rotating in clockwise direction (90 de-
grees).
182 5 Navigation and Registration
CT MR

1 1 3 3 3 3 3 3

1 1 3 3 3 3 3 3

2 2 3 3 1 1 2 2

2 2 3 3 1 1 2 2

Fig. 5.10: Abstract example images. 16 pixels, only three potential gray
values.

However, images from two different sources might not assign the
same gray levels to a given structure. As an example, look at the im-
ages from figure 1.6, showing two images of the same structure (femur
bone) imaged with CT (left) and MR (right).
Since CT is x-ray-based, the bone appears white in the image, whereas
it is black in the MR image. Then it would seem hard to match the
structures, based on the pixel gray levels.

CT MR

1 1 3 3 2 2 2 2

1 1 3 3 2 2 2 2

2 2 3 3 3 3 1 1

2 2 3 3 3 3 1 1

Fig. 5.11: Abstract example images. An appropriate matching of gray

levels must be computed by the registration algorithm.

Look at the example in figure 5.11. Which should be the correct re-
gistration position of the images from figure 5.11? The answer is that
the MR image should again be rotated clockwise by 90 degrees. But
now, we must additionally map the pixel intensities according to the
following table:
5.4 Intensity-based Registration 183

CT MR
1 →3
2 →1
3 →2

The mutual information algorithm (MI algorithm) solves both prob-

lems in an elegant way (computing the registration position and the
table for the intensity mapping).
The mapping does not have to be one-to-one. Notice that all this can
be achieved only by looking at the intensity values of the pixels, no
contour is needed! Thus MI registration is an ideal method for regis-
tering 3D CT images to 3D MR images.
One important application of MI registration is radiation therapy. CT
images contain information on the Hounsfield units, i.e. the tissue
density with respect to radiation. Therefore, CT is the standard image
modality for radiation therapy planning. However, MR images often
show a better contrast for soft tissues. Thus, a better delineation of
tumors is possible.

5.4.1 Basic definitions for MI-Registration

Assume an image is generated by a random process. We move through

the pixels of the image line by line, from left to right. At each pixel
we stop, and a random generator decides what color to give this pixel.
More formally, pA (a) is the probability that the random experiment
A gives result a. Then pA (a) is the probability that the current pixel
obtains gray value a.
For two random variables A, B, the joint probability pA,B (a, b) is the
probability that A gives outcome a and B gives outcome b. We will
write (a, b) as an abbreviation for (a and b).
For two images, we have two random variables A and B. For simpli-
city, we also call the images A and B.
Now assume image A shows a landscape, and image B shows a dog.
Thus the two images are completely different. Assume also we have
not yet seen the images, we have only heard about them. Then know-
ing something about one pixel in image A will not tell us much about
184 5 Navigation and Registration

the corresponding pixel in image B. With other words, image A does

not contain much information about image B.
Next, consider the opposite case: assume the two images are exactly
the same, i.e. A = B. Again, we have not seen either of the two images.
But here, knowing the value of one pixel in image A informs us about
image B as well. We know that the gray level of this pixel must be the
same.
In terms of the random variables producing the images, we have the
following situation. In the first case (landscape and dog), the two ran-
dom variables are independent.
In the second case (A = B), the random variables are maximally de-
pendent.
We return to the problem of registration. We do the following experi-
ment. Images A and B are still the same. But now, B is slightly moved
to the left, i.e. the images are slightly out of registration. Now what
will happen to the random variables? The random variables will no
longer be maximally dependent. But since we only moved image B
by a very small amount, they will be close to maximally dependent.
Thus we could simply measure the degree of dependency of the two
random variables, to find the registration position.

5.4.2 Mutual information I(A, B)

Mutual information I(A, B) is a measure related to the concept of in-

dependence. I(A, B) is defined by

pA,B (a, b)
I(A, B) = ∑ pA,B (a, b) log . (5.11)
a,b pA (a)pB (b)
The sum in this definition ranges over all outcomes a, b of the two
random variables A, B. It can be shown that I(A, B) ≥ 0 [1].
The connection between mutual information and independence be-
comes visible, if we observe the following: If A, B are independent,
then

pA,B (a, b) = pA (a)pB (b) (5.12)

5.4 Intensity-based Registration 185

and

pA,B (a, b)
log = log 1 = 0. (5.13)
pA (a)pB (b)
The next example illustrates mutual information for image pairs.

Example 5.3

Consider the two images A and B with only four ‘pixels’ each (fig-
ure 5.12). They have only two gray levels: 0, or black, and 1, or white.

A B

0 1 0 1

1 0 1 0

Fig. 5.12: Abstract example images. Four pixels, two gray values (0 and
1).

Obviously, both images are the same, so there is maximal dependency

in the image information.
The distributions for A and B are

pA (0) = 0.5 pA (1) = 0.5,

pB (0) = 0.5 pB (1) = 0.5.

The joint distribution is calculated as follows:

pA,B (0, 0) is the probability that a pixel in image A is black, while the
same pixel is also black in image B, here

pA,B (0, 0) = 0.5. (5.14)

Likewise,

pA,B (1, 1) = 0.5, (5.15)

186 5 Navigation and Registration

and of course

pA,B (0, 1) = 0, (5.16)

pA,B (1, 0) = 0. (5.17)

Thus, we can calculate the mutual information between the two im-
ages as

I(A, B) = 0.5 log 0.5/(0.5)2 + 0.5 log 0.5/(0.5)2 = 1. (5.18)

The next example shows that the two images do not have to use the
same colors.

Example 5.4

A B

0 1 1 0

1 0 0 1

Fig. 5.13: Gray level 0 is mapped to gray level 1 and vice versa. Here the
mutual information I(A, B) also equals one!

Before we look at the actual method of image registration with mutual

information, we briefly return to the definition in equation 5.11. The
entropy H(A) is a measure for the degree of ‘disorder’ in a random
variable A. It is defined as

H(A) = − ∑ pA (a) log pA (a). (5.19)

a
Notice that the sum ranges over the possible outcomes of A. An ex-
ample is a random variable with only two outcomes (namely 0 and 1).
Then H(A) is maximal if both outcomes have equal probability, i.e.
5.4 Intensity-based Registration 187

pA (0) = 0.5 pA (1) = 0.5.

Now, by contrast, assume one outcome has probability 1 and the other
0, i.e.

pA (0) = 1 pA (1) = 0.

Then the degree of ‘disorder’ is minimal, i.e. the entropy is 0 (ap-

ply the convention 0 log 0 = 0 in the above formula). Finally, if one
outcome has the probability 0.25 and the other one 0.75, the entropy
H(A) is 0.811.

(End of Example 5.4)

From the definition, mutual information is closely related to entropy.

The exercises at the end of this chapter discuss the connection between
entropy and mutual information from a more theoretical point of view.
We consider two more examples to further illustrate mutual informa-
tion in the context of images.

Example 5.5

Figure 5.14 shows a case in which the mutual information of two im-
ages is zero.
A B

0 1 1 1

1 0 1 1

Fig. 5.14: Image A gives no information on the content of image B. In

this case the mutual information I(A, B) = 0.

This can be verified in the same way as in example 5.3.

(End of Example 5.5)

188 5 Navigation and Registration

Example 5.6

One final example is shown in figure 5.15. Three pixels are the same,
but not the fourth.
A B

0 1 0 1

1 0 1 1

Fig. 5.15: In this example I(A, B) = 0.32.

(End of Example 5.6)

5.4.3 Registration method based on mutual information

Assume there are 256 gray levels in both images A and B, (0, . . . , 255).
The first step is to estimate the distribution pA . Set up a vector with
256 entries, one entry for each possible gray level. We count the num-
ber of voxels (in 2D: pixels) for each gray level value. This means:
we step through the image voxel by voxel. Whenever we meet a voxel
with gray level j, we increment the entry with index j in the vector by
1.
From this vector, we compute an approximation for pA : we normalize
the vector by dividing by the total number of voxels, so that the sum
of all entries becomes 1. We call this vector the gray level histogram
for pA . In the same way, we compute the histogram for pB .
After calculating the two histograms, the only thing missing is the
joint distribution of the two random variables. Therefore, consider a
table with 256 × 256 entries as a joint histogram with all entries ini-
tialized to 0. First, bring the two images into an approximate match-
ing position (i.e. by superimposing the images). For each voxel pair
5.4 Intensity-based Registration 189

with gray levels (a, b) increment the table entry with index (a, b) by
1. Compute the estimate of the joint distribution by normalizing the
table entries (divide by number of voxel pairs), so that the sum of all
table entries becomes 1. The computation is illustrated in figure 5.16.

a
CT
MR 0 255
0

b #(a,b)

255

Fig. 5.16: Table for computing the joint distribution pA,B . Place the num-
ber of voxels having gray level a in A and gray level b in B at the table
position with index a, b.

Notice that the joint distribution pA,B is position-dependent! This

means pA,B depends on the relative position of image B with respect
to image A. Notice also that pA , pB are independent of the relative
position.
Having computed the three histograms for pA , pB and pA,B , we have
all the tools for evaluating the defining equation for mutual informa-
tion (equation 5.11).

5.4.3.1 Registration criterion

Let image A be the fixed image, image B be the floating image, i.e. B
moves over image A (rotation and translation), until the final registra-
tion position is reached.
We move the floating image under six transformation parameters:
three angles α, β and γ and three translations tx ,ty and tz .
Criterion: For each position of B, evaluate the mutual information via
equation 5.11. The images match optimally for the position where the
mutual information is maximal.
190 5 Navigation and Registration

5.4.3.2 Interpolation in the floating image

One problem that arises is that the pixel (voxel) center in the floating
image may not always be exactly above the pixel (voxel) center of the
fixed image (Figure 5.17). Thus, in figure 5.17, we have assumed that
in one box-section of our CT image (fixed image) we have four voxels
with gray levels of 97, 98, 102 and 103, respectively. And we assume
a voxel of the MR, inside this box has gray level 13.

MR
97 98

CT 13

102 103

Fig. 5.17: Interpolation of the voxel position.

To compute the histogram-table for the joint distribution, we need to

state which of the four gray levels in the bounding box we will update.
There are three types of interpolation:
• nearest neighbour interpolation (NN)
• trilinear interpolation (TI)
• trilinear partial volume interpolation (TPV)

Nearest Neighbor Interpolation

For the example of figure 5.17, the CT-voxel with gray level 98 is the
nearest neighbour of the MR-voxel with grey level 13. We thus incre-
ment the table entry for the index pair (98, 13) in the joint distribution.
5.4 Intensity-based Registration 191

Trilinear Interpolation

Trilinear interpolation takes the weighted mean of the neighboring

gray levels. Notice that gray levels not actually occurring in the ori-
ginal image can be generated in this way. In the example of fig-
ure 5.17, we compute the distances d1 to d4 , between the MR-voxel
inside the box and the four corners of the box. Trilinear interpolation
increments the table entry with index (x, 13). Here, x is the weighted
average of the neighboring gray levels. The weight is determined by
the distance from the MR-voxel. The term trilinear stems from the fact
that the interpolation is three-dimensional.

Trilinear Partial Volume Interpolation

Trilinear partial volume interpolation avoids generating new gray

levels which do not occur in the image. We do not increment by 1,
but by the weight obtained from the interpolation. All neighbors ob-
tain increments corresponding to their distance.

Search for the registration position

The search for the registration position starts with the initialization,
i.e. images are overlaid so that centers and axes match. The optimiz-
ation is then decomposed into a separate search along several axes.
Thus, we compute the mutual information I(A, B) for the current po-
sition (start position) according to equation 5.11. We then move the
floating image by a small amount along one of the six axes (three
translational and three revolute axes). For this new position, we re-
compute the mutual information I(A, B). This process is repeated to
maximize the mutual information. Notice that the mutual information
depends on the position of the floating image relative to the fixed im-
age. As a consequence, we must recompute the joint distribution for
each new position in this process. However, the distributions pA and
pB remain unchanged when moving the floating image. The maxim-
ization process can, for example, be guided by Levenberg-Marquardt
optimization [2].
192 5 Navigation and Registration

Trilinear partial volume interpolation yields the smoothest optimiza-

tion function. It is thus the preferable interpolation method.

Binning

The histograms described above provide an estimate for the distri-

bution. The quality of this estimate depends on the number of ob-
servations. To obtain a better estimate, observations can be grouped
together into so-called bins. For example, we return to the histogram
describing pA with 256 gray level values. Then we form 64 bins each
representing four gray levels. We count the number of entries in each
bin, rather than counting the number of entries for each gray level. In
this example the width of the bins (four) has been chosen rather arbit-
rarily. Histograms depend on a good choice of the bin size, and even
bins of different width can be useful. In practice it turns out that a reas-
onable approach is the following: take several bin sizes and average
over the resulting histograms to obtain the final histogram.
Notice that intensity based registration (with mutual information) can
be applied for two-dimensional and three-dimensional images as well.
So far, we saw several basic methods for registration. In each case,
our registration was rigid. This means, potential deformations of any
structures (in images) were taken not into account. In the next section,
we will discuss image deformations.

5.5 Image Deformation

We will need methods for image deformation in two applications. In

the first application, we must correct for distortions. Thus, the physical
process of image acquistion can cause distortions of the image. The
second application is elastic registration.
Both appications mentioned (distortion correction and elastic registra-
tion) share a common root, and we will start with a simple example
illustrating this. In this case, we have two grids of points. We will call
these grids G and G0 . Let G be a grid of equidistant points. We assume
5.5 Image Deformation 193

G0 is a (slightly) deformed version of G, and we know the point-to-

point correspondences (see figure 5.18).

Fig. 5.18: Deformation of a grid of equidistant points.

5.5.1 Bilinear Interpolation

Suppose the grid points in G and G0 are landmarks in two images. Our
goal is to deform the image underlying G0 , while moving each land-
mark point in G0 to its target position in G. We find an initial regis-
tration position with landmark-based registration, as described above.
After this step, the positions of the landmarks in G and G0 may not
match exactly, and we deform G0 appropriately, to bring the points

Original values (used to compute interpolating function)

Interpolated value

du dw dv

u w v

Fig. 5.19: Distortion correction with bilinear interpolation

194 5 Navigation and Registration

into a matching position. To this end, suppose we have two neighbor-

ing landmarks u, and v, both in G0 . We treat shifts in x-and y-directions
separately. Given the point-to-point correspondence, we know that the
point u must be shifted by an amount du in x-direction, and v must be
shifted by dv. We then shift each image point w in between u and v by
an amount corresponding to the linear interpolation of its position (see
figure 5.19). The figure shows the interpolation function, from which
we read the shift for w.

Original values

du dw
dv

u v w

Fig. 5.20: Interpolation function

Interpolation is now applied in both the x-direction and the y-direction

of the image, hence the name bilinear interpolation. The advantage of
bilinear interpolation is its simplicity. The disadvantage is that our
interpolation function is not smooth (figure 5.20). We will discuss a
better method next.

5.5.2 Cubic Spline Interpolation and Gaussian Least

Squares

Given an image point with coordinates (x, y)T , we then define a poly-
nomial

a0 + a1 x + a2 y + a3 xy + a4 x2 + a5 y2 + a6 x2 y + a7 xy2 + a8 x3 + a9 y3 .
(5.20)
5.5 Image Deformation 195

This polynomial (in the two variables x and y) describes the shift of
this image point in x-direction. Likewise,

b0 + b1 x + b2 y + b3 xy + b4 x2 + b5 y2 + b6 x2 y + b7 xy2 + b8 x3 + b9 y3
(5.21)
describes the shift in y-direction.
The parameters ai and bi in this polynomial must be adjusted in such
a way that the distances between the model points (xm , ym )T (i.e. grid
points in G) and the corresponding landmarks in G0 are minimized.
The coordinates of the landmarks in G0 are given by (xd , yd )T . Shifting
under the correction polynomials will give points

(a0 + a1 xd + ... + a9 y3d , b0 + b1 xd + ... + b9 y3d )T . (5.22)

The distance for a pair of landmarks, i.e. landmark from G to land-
mark from G0 , is thus

k(xm , ym )T − (a0 + a1 xd + ... + a9 y3d , b0 + b1 xd + ... + b9 y3d )T k. (5.23)

Note that the values ai , bi in this expression are variables and xm , ym ,
xd , yd are constants! The values ai , bi appear squared in the distance
term. Now consider the sum of the squared distances for all such land-
mark pairs. The overall sum is still quadratic in the values ai , bi and the
derivative of this term with respect to all 20 parameters ai , bi yields 20
linear equations in 20 unknowns. The solution of the linear equation
system gives the optimal values for the parameters ai , bi . Transform-
ing the entire image under the function

(x, y)T → (a0 + a1 x + ... + a9 y3 , b0 + b1 x + ... + b9 y3 )T (5.24)

gives the undistorted image. This method is called Gaussian Least
Squares.

5.5.3 Thin-Plate Splines

The Gaussian least squares method in the last section provides a

smooth deformation of the image. However, this method is purely
196 5 Navigation and Registration

mathematical, and does not necessarily represent the physical condi-

tions that led to the deformation. The thin-plate spline method (which
we will look at in this section) was designed to reflect physical char-
acteristics of typical deformations.

pi

zi

Fig. 5.21: Deforming a thin metal plate. The plate is deformed by a force
pushing downwards at point pi . The magnitude of the displacement at
pi is zi .

Assume we have forces acting on a piece of sheet metal. For simpli-

city, the forces are assumed to be needles pushing in vertical direction
onto the metal plate. Assume the metal sheet is completely flat ini-
tially. To be specific, we further assume the sheet is rectangular, and
three of the four corner points are mechanically fixed. Now deform
this plate by pushing a needle in downward direction. The amount of
displacement at the needle tip p is given by a value z. The situation is
illustrated in figure 5.21.
More generally, assume we have N needles, again pushing in vertical
direction onto a metal sheet, where the position of the points pi =
(xi , yi )T on the sheet is arbitrary. We are looking for a function f (p)
where p = (x, y)T is a point on the sheet. f should assume the value zi
at point pi , i.e. it is used to model the displacement. The values zi are
real numbers.
Now set
N
f (p) = ∑ wi F(k p − pi k). (5.25)
i=1
In this function, the values wi are the parameters, i.e. the values wi
take the role of the values ai , bi in the previous section.
5.5 Image Deformation 197

The function F is called a kernel function. For now, we set F(r) = r2 .

Thus, for a point p = (x, y)T the value of F(kp − pi k) increases with
the squared distance of p from pi . But notice that the influence of the
corresponding weight value decreases with the distance from the con-
trol point. Thus, we can simply insert our control points pi = (xi , yi )T
into this function, and the desired output values zi of f are known.
In this way we obtain a linear equation system with N variables (the
weights wi ) and N equations.
The flexibility of this method stems from the fact that we have a kernel
function. To represent local inhomogeneities, physical properties etc.
we can modify the kernel function. For our application, the function
F defined here is sufficient.
To allow for translational displacement, the function f can be modi-
fied to
N
f (p) = ∑ wi F(k p − pi k) + a0 + a1 x + a2 y. (5.26)
i=1
We now have three new variables a0 , a1 , a2 . The resulting linear sys-
tem is under-determined. To obtain a unique solution, we restrict the
solution set appropriately. As an example, we can set
N N N
∑ wi = 0, ∑ wixi = 0, ∑ wiyi = 0. (5.27)
i=1 i=1 i=1
The above version of the thin-plate-splines method only works for
scalar-valued functions. To make it work for image deformation or
warping, we need functions mapping from 2D to 2D, i.e. a control
point (2D) in the image must be moved to a model point (also 2D).
Such a vector-valued function f can be generated by using two thin-
plate-splines, one for the x-coordinate and one for the y-coordinate. In
the same way this method can be extended to the 3D case.
Thin-plate-splines are rather expensive computationally. For every in-
terpolation, the distance to all control points must be calculated and
the kernel function must be evaluated.
One way to improve this is to use TPS only on the grid points of a
coarse grid (e.g. 10 pixel grid width). Within the grid, fast bilinear
interpolation can be applied.
198 5 Navigation and Registration

X-ray detector
Calibration
disk

Fig. 5.22: Calibration disk.

Example 5.7

The physical acquisition process for most image modalities causes

small deformations of the image. The deformation may resemble the
pattern shown in figure 5.18. Images produced by C-arms are de-
formed by the earth’s magnetic field. Small deformations are toler-
able as long as the images are only used for diagnosis. However, for
navigation, deformations must be corrected. An example is shown in
figure 5.22 A calibration disk is mounted in front of the detector of
the C-arm. Small metal crosses form a regular grid of radio-opaque
markers on the radio-translucent background of the disk. The mark-
ers are thus visible in the images. Notice that the distortion of images
is position-dependent. Thus, if we move the C-arm (i.e. we vary the
imaging direction), then the distortion pattern will change!

(End of Example 5.7)

The following example illustrates the difference between bilinear in-

terpolation and thin-plate splines.
5.5 Image Deformation 199

Example 5.8

Table 5.1 defines two-dimensional grid points and shift values. Fig-
ure 5.23 shows bilinear interpolation for the grid in table 5.1 and fig-
ure 5.24 shows interpolation with thin-plate splines on the same grid.

point shift value

(0, 0)T 1
(1, 1)T 1
(1, 0)T 0.5
(0, 1)T 0
(2, 1)T 0
(0, 2)T 1

Table 5.1: grid points and shift values for figure 5.23 and figure 5.24.

0.9

0.8

0.7

0.6

0.5

0.4

0.3

0.2

0.1

0
0

0.2

0.4
0
0.6 0.5

0.8 1
1.5
1 2
y x

Fig. 5.23: Bilinear interpolation.

(End of Example 5.8)

200 5 Navigation and Registration

0.9

0.8

0.7

0.6

0.5

0.4

0.3

0.2

0.1

0
0

0.2

0.4
0
0.6 0.5

0.8 1
1.5
1 2
y x

Fig. 5.24: Interpolation with Thin-Plate Splines

5.5.4 Elastic Registration

In elastic registration, we again have two images A and B, where A is

the fixed image, and B is the floating image. Here, we allow the float-
ing image B to deform during registration. Thus, assume we have two
images of a lung, for the same patient, but taken at different points in
time. The lung deforms as the patient breathes. To quantify the mag-
nitude of the deformation, or to locate targets within the lung, we will
need to deform images.

Cross correlation Thin-plate splines need landmarks and correspond-

ence pairs. Landmarks are not always available. A simple strategy to
find landmarks automatically is template matching.
Consider a small window of image B. For example, this window can
be a square region of 9 × 9 pixels.
The window will be called the template. Having a coarse alignment,
we move the template (from image B) over the corresponding region
in A, and find the best matching position. The motion of the template
is purely translational. We compute the following value for each pos-
ition, denoting translational displacements by (u, v):

∑x ∑y T (x, y)A(x + u, y + v)
C(u, v) = q q (5.28)
∑x ∑y [T (x, y)] ∑x ∑y [A(x + u, y + v)]2
2
5.6 Hand-eye Calibration 201

Here T is the template, and T (x, y) is the gray level at pixel (x, y) in
the template.
Maximizing the cross correlation by moving the floating template
gives the desired estimate for the registration position of the template.
In this way, we can find landmarks and correspondence pairs. But how
do we find good templates? Obviously, one would like to select inter-
esting windows rather than uninteresting ones. An uninteresting win-
dow has simply no contrast at all, i.e. all pixels have the same color
or gray level. Thus we choose windows having two very distinct gray
levels, assuming such windows will show delicate bony structures or
salient landmarks. To find such windows, we compute histograms for
each window in the floating image. A histogram simply plots the num-
ber of pixels having a given gray value over the x-axis with the gray
levels. The histogram is called bimodal, if we can distinguish two dis-
tinct peaks.
This matching process described here can also be extended to mutual
information matching.
Elastic registration in real-time is difficult and error-prone. In later
chapters we will describe tracking methods based on motion correla-
tion which allow for motion tracking in real-time.

5.6 Hand-eye Calibration

Accurate navigation requires a further component, which we have not

yet discussed. In practice, it is often necessary to register the robot ef-
fector to a camera image. This process is called hand-eye calibration.
In a typical navigation scenario, for example an orthopedic drilling
task, a robot program may look like this:
1. Point to the target position with the pointer of a tracking system
2. Compute the robot joint angles, given this target point
3. Move the robot to the target point/direction
4. Drill
In step one, we simply touch the target with the pointer of the tracking
system. The tracking system then computes the position and orienta-
tion of the pointer with respect to its own internal coordinate system.
202 5 Navigation and Registration

The position and orientation data will then be returned to the host
computer.
However, you may argue that we now have the position of the pointer
with respect to the internal system of the camera. How does the robot
know where the internal system of the camera is? This type of prob-
lem is a calibration problem, and this is what we will address in this
section.
To illustrate hand-eye calibration in a more specific case, assume an
LED marker is attached to a robot gripper. The tracking system reads
the 3D position of the marker. We have attached the marker on the
robot in a rather arbitrary position, and we have placed the tracking
system on a floor stand in front of the robot. The situation is illustrated
in figure 5.25.

CT
M

ET
M

RT RT
E C

Fig. 5.25: Hand-Eye-Calibration

Now four transformations describe the spatial relationships. The first

is the transformation from the robot’s base coordinate system to the
robot’s effector. Call this transformation matrix R TE . Then there is the
transformation from robot effector coordinate system to the attached
marker E TM . The tracking system has an internal coordinate system.
If the robot’s base coordinate system is the fixed world coordinate sys-
tem, then the transformation from robot base to the coordinate system
of the tracking system is needed. Call this transformation R TC . (Here
5.6 Hand-eye Calibration 203

C refers to the tracking camera). Finally, our tracking system reports

the position of the marker. This position is the transformation between
the amera coordinate system to the marker. Call this last transforma-
tion C TM .
Hence, we have the following four matrices:
RT Robot base to tool flange/effector
E
ET Effector to marker
M
RT Robot base to tracking camera
C
CT Tracking camera to marker
M

Two of the four matrices are known. Firstly, the position of the tool
flange/gripper coordinate system with respect to the robot base is
known. The robot is a kinematic chain, and matrices describe the po-
sition of the gripper or tool flange with respect to the robot’s base
coordinate system. Thus R TE is known.
Secondly, the tracking system computes the position of the marker,
so that the matrix C TM is known. (Most tracking systems output the
marker orientation as a quaternion, but we can convert this to a mat-
rix.)
However, the two other matrices are unknown. We typically do not
know where exactly the internal coordinate system of the tracking
system is located with respect to the robot’s base, since we just put
the tracking system on a floor stand, free to move in the room. Also
we attach a marker to the robot, but we are not sure how to determine
the transformation matrix from robot tool system to the marker. Now,
hand-eye-calibration is the process of computing the two unknown
matrices in this scenario.
We note that the transformation from robot base to marker via the
robot gripper must give the same result as the transformation from
robot base to marker via the tracking system. This observation can be
expressed in an equation:

R E
TE TM =R TC C
TM (5.29)
Now recall that two of the four matrices are known, namely R TE and
C T . Thus only two matrices will have to be computed from this
M
equation.
We rename the above matrices to obtain a simpler notation.
204 5 Navigation and Registration

AX = YB (5.30)
So the last two equations are the same, only we have changed the
names of the matrices. Now A and B are known, and X and Y are
unknown.
But notice that we can move the robot. After moving to a second pos-
ition, we obtain a second equation.
So assuming the first position gives

A1 · X = Y · B1 . (5.31)
The second position will give the equation

A2 · X = Y · B2 . (5.32)
We rearrange the two equations to have

A1 · X · B−1 −1
1 = A2 · X · B2 , (5.33)
or

A’ X = X B’, (5.34)
where

A’ = A−1
2 A1 , (5.35)
B’ = B−1
2 B1 . (5.36)

Hence, it remains to solve for X alone. Daniilidis [3] presents a

method for solving equation 5.36 using dual quaternions. Hence,
Daniilidis starts from equation 5.36. We will not use the method by
Daniilidis. Instead, we will start from a series of n equations of the
form in equation 5.31. If we move the robot through a series of po-
sitions, then each position will give rise to an equation of the form
equation 5.31. Thus, we will start from a series of equations of the
type

Ai X = Y Bi , i = 1, . . . , n. (5.37)
5.6 Hand-eye Calibration 205

Setting Ci = B−1
i , we have

Ai XCi = Y. (5.38)
The matrix X is a homogeneous 4x4-matrix, and hence there are
12 non-trivial entries in this matrix. Likewise, Y has 12 non-trivial
entries. Thus equation 5.38 gives rise to a linear system of equations
with 12 · n equations and 24 variables.
Since we assume n > 2, the resulting system has more equations
than variables. We have already encountered a method for solving
such over-determined systems in the context of image reconstruction.
The ART algorithm is a suitable method for this problem (see exer-
cise 1.2). After solving the linear system we obtain the values for the
variables x11 , . . . , x34 and y11 , . . . , y34 . This gives matrices describing
the transformations needed for the calibration.

RT
E

ET
Ultrasound camera M

Marker
RT
C
CT
M

Water tank

Fig. 5.26: Hand-Eye-Calibration for an ultrasound camera. A small lead

ball acts as a marker. The marker is attached to the robot. The marker
is in a water tank. The lead ball is detected in the ultrasound image.
206 5 Navigation and Registration

A further example for hand-eye calibration is shown in figure 5.26.

The same methods apply.
Note that this method will not result in full 6D data for E TM , since
C T is only 3D (position only). We can, however, fully determine
M
R T and the translational part of E T .
E M

Exercises

Exercise 5.1

Verify the values I(A, B) for the examples in figures 5.13 to 5.15.

Exercise 5.2

Show that I(A, B) = H(A) if the two images A, B are the same, i.e.
A = B. Discuss this in the light of example 5.3 above. Find the value
for H(A) in case the image A has an even distribution of gray levels,
i.e. there are 256 gray levels, and all gray levels occur equally often in
the image.

Exercise 5.3

Show that

∑ pA,B(a, b) log pA(a) = ∑ pA(a) log pA(a). (5.39)

a,b a

Exercise 5.4

The joint entropy H(A, B) is defined as

H(A, B) = − ∑ pA,B (a, b) log pA,B (a, b). (5.40)

a,b

Let A, B be binary images, (i.e. pA and pB have only two outcomes 0

and 1). Based on the result in equation 5.39, show that
5.6 Hand-eye Calibration 207

I(A, B) = H(A) + H(B) − H(A, B). (5.41)

Exercise 5.5

Prove equation 5.41 for arbitrary, not necessarily binary images.

Exercise 5.6

Show that H(A) ≥ 0 for a random variable A.

Exercise 5.7

The χ 2 -function tests the hypothesis that two random variables are
independent. It can also measure the degree of dependency between A
and B. (Pronounce χ 2 as ’ki-squared’, as in ’kite’). Thus, we measure
the ‘distance’ between the observed joint distribution pA,B (a, b) and
the hypothetic distribution which would be observed if A and B were
independent:

(pA,B (a, b) − pA (a)pB (b))2

χ 2 (A, B) = ∑ (5.42)
a,b pA (a)pB (b)
This expression is zero (minimal) if A and B are independent.
Evaluate the two criteria in equations 5.11 and 5.42 in experiments
with simple synthetic images. Find the local maxima and the global
optimum for both functions. Based on the two criteria (running in
parallel), can we develop an ad hoc test for deciding which maximum
is a global one?

Exercise 5.8

Set

p1 = 0 f (p1 ) = 0
p2 = 1 f (p2 ) = 1
p3 = 2 f (p3 ) = 0, (5.43)
208 5 Navigation and Registration

Evaluate the weights w1 , w2 , w3 , following the definition of the weights

for thin-plate splines. Visualize the effect of thin-plate spline deform-
ation for the constraints thus defined with a small program.

X-ray detector
z0
x0
S0

z1
x1
S1
X-ray source

Fig. 5.27: Projection geometry of a C-arm. The focal length f is the dis-
tance between the source and the detector.

Exercise 5.9

Place two coordinate systems S0 and S1 (the detector coordinate sys-

tem and the source coordinate system of a C-arm) according to fig-
ure 5.27.
A marker is attached to the C-arm, and we assume this marker is
already calibrated with respect to the detector coordinate system. A
second marker is used as a calibration phantom. It consists of four
(radiopaque) spheres, which mark the origin and the tips of the x-,
y- and z-vectors in the marker coordinate system. The spheres can be
distinguished in x-ray images.
We assume the pixel size of the detector is known. We also assume
the source is a point, the origin of S1 . The focal length f of a C-arm is
the distance between the center of the detector (origin of S0 ), and the
source. Derive a procedure for calibrating the focal length of a C-arm,
with this calibration phantom.
5.6 Hand-eye Calibration 209

Exercise 5.10

Let a be a point, and B be a cloud of points. The k-d-tree algorithm

computes the nearest neighbor of a in the point cloud B. A simpli-
fied version works as follows. We sort the points in B according to
ascending x-values. Find the median value of all x-axis values in B
(figure 5.28). Partition B into two subsets by the median line, i.e. a
vertical line along the median value. Then repeat this process for both
subsets in the y-direction. The result is a tree structure, shown in the
figure. To select the nearest neighbor of a, we simply insert a into the
tree, following the tree downwards from the top node. In the example,
we obtain b3 as the nearest neighbor of a.

b1 x<2
b2 y n
b3 y<3 y<2
a b4 y n y n
b3 b1 b4 b2

Fig. 5.28: k-d-tree

a) Find an example where this simplified version does not report the
correct nearest neighbor.
b) Describe and implement a way to the repair the problem from part
a).

Exercise 5.11

The matrices X and Y derived following equation 5.38 are estimates,

and were obtained by solving an over-determined system. The rota-
tional part of a 4 × 4-matrix describing a valid transformation should
be an orthogonal 3 × 3-matrix (see also exercises for chapter 2). The
result of the estimation process contains inaccuracies. We must apply
a procedure that computes an orthogonal matrix from an approxima-
tion matrix. This can be done by Gram-Schmidt orthonormalization.
210 5 Navigation and Registration

However, Gram-Schmidt orthonormalization is general, and not spe-

cifically designed for finding a valid three-dimensional orientation.
A more dedicated way is obtained as follows. We extract Euler angles
(α, β , γ) according to the convention (z, x, z) for standard Euler angles
from the given 3 × 3 rotational input matrix. Likewise, we can ex-
tract such angles according to other conventions for Euler angles (i.e.
(z, y, z)). There are 12 such conventions for Euler angles. In each case,
we reassemble the resulting matrices from the extracted angles ac-
cording to the conventions used. The resulting 12 matrices are ortho-
gonal. But typically the 12 matrices will be different. We then extract
Euler angles from each of the 12 matrices according to the same con-
vention, e.g. (z, x, z). This will give 12 triples of Euler angles, typ-
ically all different. By averaging over the 12 sets (e.g. with a median
selector) we obtain an estimate for the angles, and reassemble the mat-
rix corresponding to this average. In an implementation, compare this
strategy to the Gram-Schmidt orthonormalization.

Summary

Registration brings structures into alignment. Typically, two structures

are given, and we compute the transformation for achieving the best
match. Many navigation methods require a registration step. Digit-
ally reconstructed radiographs (DRRs) provide a basic method for
automatic registration during interventions. DRRs are synthetic x-
ray images, computed from a CT data set, via projection from dif-
ferent angles. Other typical registration methods are: landmark-based
registration, point-based registration, contour-based registration and
intensity-based registration. For landmark-based registration, a small
number of points with known correspondences is marked in both data
sets. The registration problem then reduces to matching the two point
sets, given information on which point should match which. We ob-
tain a quadratic minimization function after introducing approxima-
tions of rotation matrices. A least-squares method is used to solve the
minimization problem. Point-based registration finds the correspond-
ences by itself. Iterative closest point matching (ICP) is a standard
technique for this case. It is heuristic, and exact methods for large sets
5.6 Hand-eye Calibration 211

are not known. ICP alternates between a move step (as in landmark-
based registration) and a match step. The match step computes the
correspondences by a search for the nearest neighbor of each point.
Contour-based registration is a direct extension of ICP. We replace
the point-to-point distance in ICP by a point-to-line distance. MI re-
gistration allows for multi-modal image registration, by automatically
finding correspondences between pixel intensities (or voxel intensit-
ies). We start by placing image B on top of image A. Then move B, and
compute the value I(A, B) for each position. The position giving the
maximum is the registration position. The movement of B is guided by
Levenberg-Marquardt optimization. To compute I(A, B), we need the
histograms for pA , pB , and pA,B . Each histogram is a table. The table
entries correspond to the image intensities. For each table entry, we
count the number of pixels having the corresponding intensity. Nor-
malizing gives an approximation for pA , pB , and pA,B . Intensity-based
registration is rarely used in navigation, due to limitations in speed
and robustness. One typical application of MI-registration is CT-MRI
fusion. In radiation therapy, tumors must be delineated in the CT. If
the tumor boundary is not well visible, then the MRI can help improve
the delineation.
To apply medical imaging in navigation, distortions must be correc-
ted, and we must determine parameters describing the projection geo-
metry. For x-ray camera calibration, calibration objects with grid pat-
terns are placed into the beam path. The geometry of the grid pattern is
known from measurements. Based on this, we deform the image such
that the markers match the geometry of the grid. Thin-plate splines
and cubic splines reduce the deformation problem to a linear equation
system.
Hand-eye calibration aligns internal coordinate systems of cameras
and robots. The problem of hand-eye calibration leads to a matrix
equation of the form AX = YB, where A and B are known, and X and
Y are unknown. We can set up a number of such equations, by moving
the robot through a series of n different positions, giving n equations
Ai X = YBi , where i = 1, ..., n. The resulting system is overdetermined
for larger n. The ART algorithm provides a fast and practical solution
method.
212 5 Navigation and Registration

A simple scheme for elastic registration can be set up with thin-plate

splines. But here landmarks must be known. We move small templates
over the image, and evaluate cross-correlation, in order to find land-
marks. Here, cross-correlation can be replaced by mutual information.

Notes

L. Brown [4] gives a summary of early methods for image registra-

tion. Introductions to the problem of medical image registration are
contained in [5] and in the text book [6].
Frameless radiosurgery based on DRR registration has been in routine
clinical use since the year 2000 [7]. x-ray images of the patient’s skull
are registered to DRRs taken before treatment.
Linear approximations for rotation matrices are a standard tool in ro-
bot kinematics. Point cloud registration can also be solved without
an approximated rotation matrix [8, 10]. ICP methods were first de-
scribed in [9]. [11] gives a survey of variants for ICP. Mutual inform-
ation and joint entropy occur in many applications. I(A, B) can be
expressed as a direct function of joint entropy [1], see also the ex-
ercises for this chapter. Multi-modal image registration with mutual
information was first described in 1996 by F. Maes et. al. and P. Vi-
ola and W. Wells [12, 13]. It is now a field of its own [14]. Phase-
correlation based on Fourier transformation finds a translational dis-
placement for aligning images [15]. More recent methods, including
cross-correlation ratio, are described in [16, 17].
The thin-plate spline method is widely used for elastic registration
[18]. A different method is the demon’s algorithm [19].
The mechanical flex of a C-arm causes inaccuracies, since the source-
detector distance is subject to small changes, depending on the posi-
tion of the C-arm. Furthermore, the relative orientation of coordinate
systems in the source and the detector is subject to mechanical in-
stability. C-arm distortion correction and flex calibration is described
in [20] and [21]. R Tsai and R. Lenz [22, 23] investigate hand-eye
calibration in the scenario given by equation 5.34. K. Daniilidis [3]
extends this approach by representing rotational parameters with qua-
References 213

ternions. [24] considers hand-eye calibration for typical medical ro-

botics scenarios.
It is useful to distinguish between 2D/3D registration and 3D/2D re-
gistration. An example for the first case is the following: we are given
a 2D slice image. The goal is to find the matching placement of this
(floating) slice in a fixed CT data set. By contrast, 3D/2D registration
does the opposite: we are given fixed 2D images taken from known
positions. The goal is to register a 3D data set (see also [25]).
Soft tissue moves under respiration and pulsation. Beyond 2D-3D re-
gistration, so-called 2D-4D registration allows for navigation in soft
tissue (see e.g. [26]). We will address the problem of tissue motion in
chapter 7.
Multi-modal image fusion is one of the main applications of MI-based
registration. We have looked at CT-MRI fusion in this chapter. Beyond
this, many other pairs of modalities require registration. Dedicated
methods have been developed in [27–29]. Beyond cross-correlation
and mutual information, many other similarity measures can be ap-
plied to intensity-based registration [30]. Diffusion-tensor imaging
adds an extra data component to 3D image data. In this case, registra-
tion must include the additional information [31]. Methods for finding
a good bin width for mutual information have been described in the
literature, such as Scott’s rule [32, 33].
Automatic registration typically does not make use of the surgeon’s
experience in recognizing anatomical structures. However, such ex-
perience can help. A method to include prior information on target
shapes into the registration process via machine learning is given in
[34].
A problem related to registration is image stitching. For example, C-
arm images are arranged in a mosaic pattern to obtain a full image of
a long bone or a chest [35].

References

[1] T. Cover and J. A. Thomas, Elements of Information Theory,

2nd ed., ser. Wiley Series in Telecommunications and Signal
Processing. John Wiley & Sons, Sep. 2008.
214 5 Navigation and Registration

[2] K. Levenberg, “A method for the solution of certain problems in

least squares,” Quarterly Applied Mathematics, vol. 2, pp. 164–
168, 1944.
[3] K. Daniilidis, “Hand-eye calibration using dual quaternions,” In-
ternational Journal of Robotics Research, vol. 18, no. 3, pp. 286–
298, 1999.
[4] L. G. Brown, “A survey of image registration techniques,” ACM
Computing Surveys, vol. 24, no. 4, pp. 325–376, December 1992.
[5] J. B. Maintz and M. A. Viergever, “A survey of medical image
registration.” Medical Image Analysis, vol. 2, no. 1, pp. 1–36,
Mar 1998.
[6] J. Modersitzki, Numerical Methods for Image Registration, ser.
Numerical Mathematics and Scientific Computing. Oxford,
UK: Oxford University Press, Dec. 2003.
[7] J. R. Adler, Jr., S. D. Chang, M. J. Murphy, J. Doty, P. Geis,
and S. L. Hancock, “The CyberKnife: A frameless robotic sys-
tem for radiosurgery,” Stereotactic and Functional Neurosur-
gery, vol. 69, pp. 124–128, 1997.
[8] K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares fit-
ting of two 3-d point sets,” IEEE Transactions on Pattern Ana-
lysis and Machine Intelligence, vol. 9, no. 5, pp. 698–700, 1987.
[9] P. J. Besl and H. D. McKay, “A method for registration of 3-
D shapes,” IEEE Transactions on Pattern Analysis and Machine
Intelligence, vol. 14, no. 2, pp. 239–256, Feb. 1992.
[10] B. K. P. Horn, “Closed-form solution of absolute orientation us-
ing unit quaternions,” Journal of the Optical Society of America
A, vol. 4, no. 4, pp. 629–642, 1987.
[11] S. Rusinkiewicz and M. Levoy, “Efficient variants of the ICP al-
gorithm,” in Third International Conference on 3D Digital Ima-
ging and Modeling. Los Alamitos, CA, USA: IEEE Computer
Society, 2001, pp. 145–152.
[12] F. Maes, A. Collignon, D. Vandermeulen, G. Marchal, and P. Su-
etens, “Multimodality image registration by maximization of
mutual information,” IEEE Transactions on Medical Imaging,
vol. 16, no. 2, pp. 187–198, April 1997.
[13] W. M. Wells, III, P. A. Viola, H. Atsumi, S. Nakajima, and
R. Kikinis, “Multi-modal volume registration by maximization
References 215

of mutual information,” Medical Image Analysis, vol. 1, no. 1,

pp. 35–51, 1996.
[14] J. P. W. Pluim, A. Maintz, and M. A. Viergever, “Mutual-
information-based registration of medical images: A survey,”
IEEE Transactions on Medical Imaging, vol. 22, no. 8, pp. 986–
1004, August 2003.
[15] C. D. Kuglin and D. C. Hines, “The phase correlation image
alignment method,” in Proceedings of the International Confer-
ence on Cybernetics and Society, vol. 1. San Francisco, CA,
USA: IEEE Systems, Man, and Cybernetics Society, Sep. 1975,
pp. 163–165.
[16] A. Roche, G. Malandain, and N. Ayache, “The correlation ra-
tio as a new similarity measure for multimodal image registra-
tion,” in Proc. of First Int. Conf. on Medical Image Comput-
ing and Computer-Assisted Intervention (MICCAI’98), ser. Lec-
ture Notes in Computer Science, vol. 1496. Cambridge, USA:
Springer Verlag, October 1998, pp. 1115–24.
[17] B. Fischer and J. Modersitzki, “Curvature based image regis-
tration,” Journal of Mathematical Imaging and Vision, vol. 18,
no. 1, pp. 81–85, 2003.
[18] F. L. Bookstein, “Principal warps: thin-plate splines and the de-
composition of deformations,” IEEE Transactions on Pattern
Analysis and Machine Intelligence, vol. 11, no. 6, pp. 567–585,
1989.
[19] J. P. Thirion, “Image matching as a diffusion process: an analogy
with maxwell’s demons,” Medical Image Analysis, vol. 2,
no. 3, pp. 243 – 260, 1998. [Online]. Available: http://www.
sciencedirect.com/science/article/pii/S1361841598800224
[20] C. Brack, H. Götte, F. Gossé, J. Moctezuma, M. Roth, and
A. Schweikard, “Towards accurate X-ray camera calibration in
computer assisted robotic surgery,” in Proceedings of the In-
ternational Symposium on Computer Assisted Radiology (CAR),
Paris, France, June, 26th-29th, 1996, pp. 721–728.
[21] A. Gueziec, P. Kazanzides, B. Williamson, and R. H. Taylor,
“Anatomy-based registration of CT-scan and intraoperative X-
ray images for guiding a surgical robot,” IEEE Transactions on
Medical Imaging, vol. 17, no. 5, pp. 715–728, 1998.
216 5 Navigation and Registration

[22] R. Y. Tsai and R. K. Lenz, “A new technique for fully

autonomous and efficient 3D robotics hand-eye calibration,” in
Proceedings of the 4th international symposium on Robotics
Research. Cambridge, MA, USA: MIT Press, 1988, pp. 287–
297. [Online]. Available: http://portal.acm.org/citation.cfm?id=
57425.57456
[23] ——, “A new technique for fully autonomous and efficient 3D
robotics hand/eye calibration,” IEEE Transactions on Robotics
and Automation, vol. 5, no. 3, pp. 345–358, June 1989.
[24] F. Ernst, L. Richter, L. Matthäus, V. Martens, R. Bruder, A. Sch-
laefer, and A. Schweikard, “Non-orthogonal tool/flange and ro-
bot/world calibration for realistic tracking scenarios,” The In-
ternational Journal of Medical Robotics and Computer Assisted
Surgery, vol. 8, no. 4, pp. 407–420, Dec. 2012.
[25] E. B. van de Kraats, G. P. Penney, D. Tomazevic, T. van Walsum,
and W. J. Niessen, “Standardized evaluation methodology for
2-D-3-D registration,” IEEE Transactions on Medical Imaging,
vol. 24, no. 9, pp. 1177–1189, Sep. 2005.
[26] A. Schweikard, H. Shiomi, J. Fisseler, M. Dötter, K. Ber-
linger, H. B. Gehl, and J. R. Adler, Jr., “Fiducial-less respira-
tion tracking in radiosurgery,” in Medical Image Computing and
Computer-Assisted Intervention – MICCAI, ser. Lecture Notes in
Computer Science, vol. 3217. Springer, 2004, pp. 992–9.
[27] Y. Hu, H. U. Ahmed, C. Allen, D. Pendsé, M. Sahu, M. Em-
berton, D. J. Hawkes, and D. Barratt, “MR to ultrasound im-
age registration for guiding prostate biopsy and interventions,”
in Medical Image Computing and Computer-Assisted Interven-
tion - MICCAI 2009, ser. Lecture Notes in Computer Science,
G.-Z. Yang, D. J. Hawkes, D. Rueckert, A. Noble, and C. Taylor,
Eds. Springer Berlin Heidelberg, 2009, vol. 5761, pp. 787–794.
[28] C. Grova, A. Biraben, J.-M. Scarabin, P. Jannin, I. Buvat, H. Ben-
ali, and B. Gibaud, “A methodology to validate MRI/SPECT
registration methods using realistic simulated SPECT data,” in
Medical Image Computing and Computer-Assisted Intervention
- MICCAI 2001, ser. Lecture Notes in Computer Science, W. J.
Niessen and M. A. Viergever, Eds. Springer Berlin Heidelberg,
2001, vol. 2208, pp. 275–282.
References 217

[29] W. Wein, S. Brunke, A. Khamene, M. R. Callstrom, and

N. Navab, “Automatic ct-ultrasound registration for diagnostic
imaging and image-guided intervention,” Medical Image Ana-
lysis, vol. 12, p. 577–585, 2008, elsevier B.V.
[30] G. P. Penney, J. Weese, J. A. Little, P. Desmedt, D. L. G. Hill,
and D. J. Hawkes, “A comparison of similarity measures for use
in 2-D-3-D medical image registration,” Medical Imaging, IEEE
Transactions on, vol. 17, no. 4, pp. 586–595, Aug 1998.
[31] B. T. T. Yeo, T. Vercauteren, P. Fillard, X. Pennec, P. Golland,
N. Ayache, and O. Clatz, “DTI registration with exact finite-
strain differential,” in 5th IEEE International Symposium on Bio-
medical Imaging: From Nano to Macro (ISBI ’08), May 2008,
pp. 700–703.
[32] A. Kraskov, H. Stögbauer, and P. Grassberger, “Estimating
mutual information,” Phys. Rev. E, vol. 69, p. 066138, Jun 2004.
[Online]. Available: http://link.aps.org/doi/10.1103/PhysRevE.
69.066138
[33] D. W. Scott, Multivariate density estimation: theory, practice,
and visualization, ser. Wiley Series in Probability and Statistics.
New York, NY: John Wiley, 1992, vol. 275.
[34] C. Guetter, C. Xu, F. Sauer, and J. Hornegger, “Learning based
non-rigid multi-modal image registration using Kullback-Leibler
divergence,” in Medical Image Computing and Computer-
Assisted Intervention - MICCAI 2005, ser. Lecture Notes in
Computer Science, J. S. Duncan and G. Gerig, Eds. Springer
Berlin Heidelberg, 2005, vol. 3750, pp. 255–262.
[35] L. Wang, J. Traub, S. Weidert, S. M. Heining, E. Euler, and
N. Navab, “Parallax-free long bone x-ray image stitching,” in
Medical Image Computing and Computer-Assisted Intervention
- MICCAI 2009, ser. Lecture Notes in Computer Science, G.-Z.
Yang, D. J. Hawkes, D. Rueckert, A. Noble, and C. Taylor, Eds.
Springer Berlin Heidelberg, 2009, vol. 5761, pp. 173–180.
Chapter 6
Treatment Planning

Most surgical interventions rely on a planning step. For example, in

orthopedic surgery, angles or cutting planes must be defined before the
intervention. This step requires image data. Given the image data, we
could plan the intervention with paper and pencil. However, it seems
reasonable to combine planning with navigation, since we must re-
trieve the geometric results of the planning steps during the interven-
tion.
In radiosurgery, an optimal set of directions from which to irradiate
the tumor must be defined. This gives rise to complex patterns of inter-
sections between beam directions. For each beam direction, we must
assign a dose weight. Then a motion path for the beam source must
be computed with mathematical methods. This path must be collision
free and dose constraints must be satisfied.

6.1 Planning for Orthopedic Surgery

In intertrochanteric osteotomy, a small bone wedge is removed to im-

prove the posture of the femur head. Figure 6.1 shows the two cutting
planes for removing the wedge. A metal implant (figure 6.1, left) joins
the two bone fragments (femur head / femur shaft).
During the procedure, the surgeon drills a channel through the femur
neck from the lateral direction figure 6.2. The channel will later hold
the top part of the implant. It is necessary to drill the channel before
cutting the femur. Otherwise the femur head would be loose, making

219
220 6 Treatment Planning

implant tip neck

head

isthmus of the
femur neck

wedge angle

cutting plane
shaft

lateral corticalis

Fig. 6.1: Implant for intertrochanteric femur osteotomy. To adjust the

position of the femur head, a small bone wedge is removed. The size,
position and angle of the wedge are computed in the planning phase.
During the intervention, a channel is drilled into the femur neck. The
top part of the implant is then placed into this channel. The shaft of the
implant is attached to the lateral femur shaft with screws.

it hard to drill the channel. But now it is difficult to find the correct
angle for the drill, since the implant shaft must touch the lateral femur
shaft after removing the wedge.

Cut

Fig. 6.2: The implant must be placed before the cut is made.
6.1 Planning for Orthopedic Surgery 221

To plan the intervention, a desired new position of the femur head with
respect to the bone must be determined. Hence, three parameters are
specified in the planning phase: channel direction, cutting plane and
wedge plane.
The planning procedure consists of the following steps:
1. Take two x-ray images (anterior/posterior and axial)
2. Construct a simplified model of the femur bone before wedge re-
moval
3. Determine the desired pose, based on this model
4. Plan the implant position
Images for step one are taken during the intervention. Planning is
based on the x-ray images, hence no registration step is needed.
For step two, we extract the following five features from the x-ray im-
ages: femur shaft axis, center of the femoral neck isthmus (see also
figure 6.1), femoral head center point, femoral shaft radius, and os-
teotomy plane. In the navigation set-up with an optical tracking sys-
tem and the C-arm, we can obtain the spatial position of these features
in the following way. In the simplest case, the feature is a point. The
point is marked in both images on the computer screen. Cast a line in
space from the x-ray source point to the marked image point on the
detector. This is done for both images. The intersection point of the
two lines gives the spatial position of the point. In a similar way, the
spatial positions of the remaining features can be extracted.
The features thus extracted define a simplified bone model, and step
three is done as follows: We define a coordinate system for the femur
shaft. This coordinate system does not move with the shaft. All mo-
tions of the femur head and neck caused by repositioning are now de-
scribed with respect to this coordinate system. The coordinate system
is defined in the following way:
• The origin is the intersection point between the cutting plane and
the shaft axis (defined in step 2).
• The z-axis is the extension of the shaft axis and points into the
proximal direction (i.e. towards the patient’s head).
• The y-axis is a vector in the AP-direction in the anatomy. Note that
the AP-direction is defined implicitly by the user, when taking the
x-ray image in AP-direction under navigation. To obtain a valid
222 6 Treatment Planning

coordinate system, we adjust this y-axis in such a way that it is

orthogonal to the z-axis defined before.
• The x-axis completes the coordinate system such that we obtain a
right-handed coordinate system.
In medical terminology, the three angles describing the orientation of
the femur head are called:
• flexion/extension (tilting of the femur head into the anterior/posterior
direction, this is the x-axis defined above)
• varus/valgus (tilting of the femur head downwards/upwards, this
is a rotation about the y-axis of the above coordinate system, see
also figure 6.3)
• rotation/derotation (rotating the femur head fragment about the
femur shaft axis, which is our z-axis; the direction of rotation is
clockwise when looking onto the x-y-plane from the proximal dir-
ection)

z
Femur
Head

Wedge
x

Fig. 6.3: Forward and inverse planning for intertrochanteric femur os-
teotomy. Frontal view onto the right femur. In this view, a valgus of 15
degrees is specified. Original posture of the femur (solid) and new pos-
ture (transparent).

With the features defined above, a model of the femur can be visual-
ized on the computer screen (figure 6.3).
Three rotational parameters and three translational parameters are set
by the surgeon. In an inverse planning step, we compute the result-
6.2 Planning for Radiosurgery 223

ing wedge to be removed. Mathematically, this inverse step is very

simple. We take the x-y-plane of the coordinate system defined above.
The wedge (see figure 6.3) is between the original x-y-plane and the
new x-y-plane. To obtain the new position of the x-y-plane, we rotate
the coordinate system defined above by the three angles (x-rotation,
followed by y-rotation, followed by z-rotation). The matrix operations
involved are straightforward (see chapter 2).
Finally, after planning the wedge, we plan the position of the implant
(step 4). The implant tip must remain at a safe distance from the bone
surface, and the shaft of the implant must be aligned with the femur
shaft. To guide the positioning of the implant, we mark the lateral
surface of the femur shaft in the images. We place and move the im-
plant manually over the femur model (figure 6.3). To obtain correct
alignment, the features extracted above are displayed in the same co-
ordinate system. The position of the implant is now given in terms
of the coordinate system specified for the femur. Thus all planning
data refer to the same coordinate system. This coordinate system is
defined directly on the images from the C-arm. Given the markers on
the C-arm, we can locate all planning data in the anatomy.

6.2 Planning for Radiosurgery

In radiosurgery, tumors are irradiated from many different directions,

to minimize dose in healthy tissue, and accumulate high dose in the
tumor.
To plan a treatment in radiosurgery, the path of the radiation source
must be computed. This path depends on the shape and size, and also
on the anatomic position of the tumor. Specifically, we must avoid
irradiating critical healthy organs close to the tumor. Such organs can
be the lung, the spine, a blood vessel or a nerve. To avoid critical
structures, the path of the robot must be computed given the individual
anatomy. Images of the anatomy are available as CT or MR stacks.
First assume the tumor has spherical shape. To irradiate a spherical
target, beams cross-fire from multiple directions at the target. This is
shown in figure 6.4. The left part of the figure shows a treatment with
one single beam direction. Such a treatment would be far from op-
224 6 Treatment Planning

Fig. 6.4: Irradiating a spherical target from multiple directions. Dose is

accumulated in the target region while healthy tissue is spared.

timal, since the healthy tissue along the beam path will receive the
same dose as the target. On the right side of the figure, two beams are
used. Generally, the right plan is preferrable over the left plan because
we can ensure that the dose in most of the healthy tissue remains be-
low a given threshold.
If we increase the number of beams in figure 6.4, we obtain a so-
called isocentric treatment (figure 6.5). All beams intersect, so that
the treated volume is a sphere.

Fig. 6.5: Isocentric treatment. Beam axes cross in a single point, the
treated volume is spherical.
6.2 Planning for Radiosurgery 225

T
.
.
.
.
.

Fig. 6.6: Non-spherical target region T and critical region B.

In practice, the target region often is not spherical. Figure 6.6 shows
an elongated tumor region T and a critical structure B.
During treatment, the beam is placed in a series of fixed positions
(called treatment nodes). At each position the beam is activated for
a certain amount of time. Thus, treatment planning consists of two
phases:

Phase 1: Find irradiation directions (also called beam directions)

Phase 2: Find a weight for each beam direction (duration of activation

of the beam at this irradiation direction)

For phase 1, we must choose the placement of the beams. The number
of beams can be large (i.e. 1000 beams). For phase 2, the duration of
activation of each beam must be computed. This is an optimization
problem. The computations for phase 1 and phase 2 interact. Thus it
is useful to iterate over both phases.
After we have computed a treatment plan, we must simulate the effect
of this plan in terms of dose delivered to the individual regions in
the target area. Such a simulation must then be visualized. Thus we
distinguish between forward and inverse planning:
226 6 Treatment Planning

Forward planning. Given: irradiation directions and the dose weights

for all beam directions. Calculate and visualize the resulting dose dis-
tribution.

Inverse planning. Given: only the regions (here T for target and C for
critical). Find appropriate beam directions and weights.

For inverse planning, the requirements are:

1. The dose in T and C should be homogeneous (uneven dose in the
target and/or surrounding tissue can cause complications).
2. Maximal dose thresholds in critical regions should not be ex-
ceeded. Such thresholds can be specified by the surgeon.
3. Sharp decline of dose in the tissue surrounding T .
4. Conformality, i.e. the region which receives a high dose (‘treated
volume’) should match T .
This leads to the following input procedure for the planning data:
1. Regions C and T are delineated by polygons in the tomography.
2. Polygons are drawn in each (positive) slice.
3. Stacking the polygons yields a 3D reconstruction of the regions.
4. Upper and lower dose thresholds for C and T are entered as inputs.
Treatment planning is typically done on CT image data, but other im-
age modalities (e.g. MRI, fMRI) can be used additionally. Given the
large number of possibilities for placing beams (and combinations of
beam placements), the goal is to find a beam placement scheme which
returns highly conformal distributions for any irregular target shape.

6.2.1 Beam Placement

When developing a scheme for beam placement, we will set up a hier-

archy of shapes, from simple to more complex shapes. While it is im-
possible to find a global optimum for arbitrary target shapes, there is a
straightforward method for special shapes: for a sphere, an isocentric
beam placement is ideal (figure 6.5).
6.2 Planning for Radiosurgery 227

For non-spherical shapes, an isocentric treatment may not be optimal.

To treat non-spherical volumes, one could pack several spheres to-
gether to cover the tumor (figure 6.7). However, this is also problem-
atic. Overlap between spheres will often be unavoidable. (figure 6.7).
Such overlap between the spheres must be minimized: overlap would
result in hot spots, i.e. undesirable overdosage of subregions within
the target.

Target Cold spot Hot spot

Fig. 6.7: Sphere packing for non-spherical target shape results in uneven
coverage (hot spots).

Consider the target volume in the figure 6.8. This shape is obtained
by sweeping a sphere. Now, instead of having a single isocenter, we
sweep the isocenter along a line segment. This gives rise to a series of
discretized isocenter points (figure 6.9b). Each of these new isocen-
ter points is then treated as if it was the center of a spherical target
volume. The result is an overlay of a series of spherical volumes. The
main difference between this scheme and sphere packing is that now
the spheres do overlap, while homogeneity is achieved by the sweep-
ing process.
Now the question remains of how to place beams for arbitrarily
shaped target volumes. A straight-forward extension of the above line-
sweeping placement scheme is illustrated in figure 6.10. Thus, the
outer boundary of the target volume is retracted, and a series of iso-
center points are placed on the retracted surface. Sweeping is then
applied to this retracted surface.
228 6 Treatment Planning

Fig. 6.8: 3D Volume obtained by sweeping a sphere

a)

b)

Fig. 6.9: Sweeping an isocenter point along a line segment.

Fig. 6.10: Placement of isocenter points for irregular shapes.

6.2.2 Beam Weights

In a second step, we compute the weights of the beams. We compute

the beam weights with linear programming. Thus, linear programming
computes durations of activation for each beam by solving a large
system of inequalities.
To set up the systems of inequalities, consider the situation in fig-
ure 6.11. The figure shows a target region (tumor T ), a critical region
C, and a polygon surrounding C and T . Furthermore, the figure shows
three beams crossing the regions delineated. The polygons shown are
bounded by line segments.
6.2 Planning for Radiosurgery 229

C
T

Fig. 6.11: Beam weighting and arrangements

We extend each line segment to a line. The lines partition the plane
into so-called cells. Here, we define a cell as a maximally connec-
ted region containing no points on the given lines. (Thus, we consider
geometric cells, not anatomic cells.) All cells thus defined are convex.
Now we take a reference point in each cell. This is an arbitrary point
inside the cell. For each such point we can compute a label. This label
simply lists all regions which contain the cell. The label also includes
the beams containing the cell. Our definition of geometric cells can
be extended in two ways: first, the target is three-dimensional, and
second, our beams are typically cylinders. For both extensions the ne-
cessary modifications are straightforward.
The partitioning of space which has been induced by the beams and
the delineation gives rise to an equation system: We take the label of a
single cell c. Assume c is contained in the target, and also in three of
the beams, called s1 , s2 and s3 . Then we wish to bound the dose in the
target from below. Thus the dose should be above a given threshold
value t.
Our goal is to compute the duration of activation of each beam. In our
example (figure 6.11), we assign variables x1 , x2 , x3 for these unknown
durations of activation for the beams s1 , s2 and s3 .
Overall we have an equation of the form

x1 + x2 + x3 ≥ t. (6.1)
230 6 Treatment Planning

Here, the value t specifies the lower threshold for the dose in the target.
In the same way we can set a bound for the dose in critical regions,
i.e. we can state that the dose should not exceed a value t 0 .
Let n be the total number of beam directions. Assume the beams are
numbered in such a way that c is contained in the beams s1 , ..., sk . By
x1 , ..., xk we denote the weights of the beams s1 , ..., sk . Let c be a cell
in a critical region. The dose in c should not exceed t 0 . We obtain the
equation

x1 + ... + xk ≤ t 0 . (6.2)
After having computed all cells, this results in a system of inequalities
of the form (for n beams)

a11 x1 + ... + a1n xn ≤ t 0 (6.3)

a21 x1 + ... + a2n xn ≤ t 0
..
.
am1 x1 + ... + amn xn ≤ t 0 ,
where the ai j are 0 or 1.
If c is in T , then c corresponds to an inequality of the form

a11 x1 + ... + a1n x1 ≥ t, (6.4)

where t is lower bound for the dose in T .
Exact methods for computing reference points for all cells are too slow
for our application. For our purposes the following approximation suf-
fices: choose a dense grid of the points in 3D, in an area containing the
target. For each grid point p, test which beams contain p. Assume p
is contained in the beams s1 , ..., sk . From this, we can readily compute
the label of p. The label of every grid point is a set. For each point we
obtain one or more inequalities.

6.2.2.1 Linear programming

In linear programming, we have a set of constraints given as linear

inequalities, and our goal is to check whether this set of linear in-
equalities admits a solution. For example, consider the inequalities:
6.2 Planning for Radiosurgery 231

x1 + x2 ≤ 10 (6.5)
x1 ≥ 5
x2 ≥ 4

Obviously, these constraints are feasible, i.e. there are values for x1
and x2 satisfying these inequalities. The constraints are visualized in
figure 6.12.

x2

10

5 10 x1

Fig. 6.12: Feasible region for the linear constraints in equation 6.5

On the other hand, the given inequalities will become infeasible if we

apply the following small modification (see figure 6.13):

x1 + x2 ≤ 10
x1 ≥ 5
x2 ≥ 6

In general, we have an inequality system of the form

(6.6)
232 6 Treatment Planning
x2

10

5 10 x1

Fig. 6.13: Infeasible constraints

a11 x1 + · · · + a1n xn ≤ b1
a21 x1 + · · · + a2n xn ≤ b2
..
.
am1 x1 + · · · + amn xn ≤ bm

where m and n can be different. Notice that the ≤-sign used here is
not a restriction. Inequalities in linear programming can also take the
form

ai1 x1 + · · · + ain xn ≥ bi .
We have used only the form with a ≤-sign, since we could always
transform a greater-than inequality into a less-than inequality by mul-
tiplying the entire inequality by -1.
The inequality system determines a polytope in n-dimensional space
(n is the number of variables, i.e. x1 , .., xn ). Notice that n is not equal
to two or three, although the system describes the constraints for the
three-dimensional case. Rather, the dimension n is the number of vari-
ables in the system. A feasibility test (F) only checks whether this
polytope is not empty, i.e. if there is a solution or not. Two problems
are distinguished in linear programming:

(LP) ‘search for a maximum under constraints’

6.2 Planning for Radiosurgery 233

and

(F) ‘feasibility test’.

Notice that the above system of inequalities in equation 6.6 is not the
standard form of linear programming in the literature. The standard
linear programming (LP) is defined in the following way: Given con-
straints in equation 6.6, maximize the expression

c1 x1 + ... + cn xn , (6.7)
where all xi ≥ 0. The expression in equation 6.7 is called the objective
function . Figure 6.14 shows the situation. Here the objective function
is 1x1 + 0x2 .

allowed

maximum

Fig. 6.14: Feasible region and objective function

The gray region is the feasible region. Notice that it does not extend
into the negative quadrants, since we require all xi to be positive. The
values ci determine a vector c. We maximize in direction of this vector
c, while remaining within the feasible region.
Thus (LP) contains a maximization, which is more than (F) alone.
However, in standard form, (LP) imposes the restriction that all xi be
positive. Convince yourself that this restriction can be removed: sub-
stitute each occurrence of the variable xi in the given LP-problem by
the expression (yi − y0i ), where yi and y0i are new variables. After sub-
stitution, the new problem has the standard LP-form with all variables
yi , y0i ≥ 0.
234 6 Treatment Planning

The matrix form of the LP problem in equations 6.6 and 6.7 is ob-
tained by setting up a coefficient matrix A from the coefficients ai j ,
and two vectors b = (b1 , . . . , bm )T , c = (c1 , . . . , cn )T . An LP problem
can then be stated in the compact form

(max cx s.t. Ax ≤ b). (6.8)

We have compared the two problem types (LP) and (F). We will now
look at the connection between (LP) and the well-known linear equa-
tion systems. n equations (with n unknowns) determine (if independ-
ent) an intersection point in n-dimensional space. But notice that, in
general, (LP) contains more than n inequalities, although we have not
imposed any restrictions on the number m of constraints! The reason
is that (LP) contains the actual inequalities with the ai j - resp. bi -
coefficients and the implicit inequalities xi ≥ 0. This means that the
number of constraints in standard (LP) is actually n + m. This value is
larger than the number n of variables. Observe that each subset of n
constraints, picked at random among the m + n inequalities, determ-
ines an intersection point.
Now we can derive a naive method for solving general (LP) prob-
lems. Pick any subset of n constraints among the n + m constraints.
For this subset, determine the intersection point, for example with a
linear equation solver. The resulting point may not be a feasible point,
since we have not observed any inequality signs in our constraints (fig-
ure 6.15). But nonetheless, we can check if our point is feasible. Now
repeat this for all n-element subsets of the n + m-element constraint
set. Having checked all such points, we can either report infeasibility,
or we can output the point which maximizes the objective function.
But notice that this naive method will not help much in practice: the
number of n-subsets of an n + m-set is exponential in general.

6.2.2.2 Finding beam weights with linear programming

As noted above, the planning process is interactive. The first step is to

delineate the critical regions and the target. This is done in the CT data
set. In each slice of the CT set, we delineate the relevant structures
(both target and critical regions). After this, we need to find the beam
6.2 Planning for Radiosurgery 235
point found by
solving system of
linear equations

feasible
polytope

Fig. 6.15: Solving a system of linear equations does not necessarily yield
a feasible point

directions. To this end, we specify the number of beams to be placed

according to the heuristic method in the previous section. To each
beam, we assign a variable xi . We then place a dense grid of points
over the region of interest and compute the labels of each grid point.
Notice that many of the grid points will have the same label. We will
return to this later.
When applying linear programming, we indeed solve the feasibility
problem (F) discussed above for the constraint inequalities derived
from the labels of the grid points. We could include an objective func-
tion where c = (−1, . . . , −1)T . This objective function minimizes the
total dose (sum of all variables xi ). In this form, we may simply obtain
the answer that the given system is infeasible.
The problem that the given constraints may be infeasible can be solved
by introducing so-called slack variables si , s0i . The slack variables are
treated in just the same way as the regular linear programming vari-
ables, i.e. we require si , s0i be positive:
236 6 Treatment Planning

a11 x1 + ... + a1n xn + s1 − s01 ≤ b1 (6.9)

..
.
am1 x1 + ... + amn xn + sm − s0m ≤ bm

After extending the input linear system in this way, the system will
always be feasible. In the optimization step, we then minimize the
sum of the slack variables, instead of the sum of the variables xi .
As noted above, many of the grid points will have the same labels.
Obviously, it is useful to have a small number of inequalities, since
the computing time for solving the resulting system is often in the
range of several hours. To this end, we remove grid points having the
same label as a grid point found earlier. This is straightforward, but
we can do better.
Some grid points not having the same labels may still be redundant,
since the restrictions they impose are already subsumed in other con-
straints. To discuss the process of removing redundant labels, we re-
turn to the definition of geometric cells in the plane. Although the
geometric structures we use are not all lines (e.g. they are cylinders in
space and line segments in CT slices) the concepts can readily be visu-
alized in the plane. Consider oriented lines as in figure 6.16. One cell
(shown shaded) is marked. It is the intersection of several half-planes.
The half-planes are bounded by lines.

Fig. 6.16: Maximal cell. Lines are oriented, as indicated by arrows. The
number of lines having the shaded cell in their interior half-space attains
a local maximum.
6.2 Planning for Radiosurgery 237

To define this orientation of lines, an arrow marks the ‘upper’ or ‘in-

ner’ half-space for the line. The respective other half-space is the outer
or lower half-space. Now we define a maximal cell: A cell c is max-
imal, if all lines directly bounding c have their inner half-space on the
same side as c. Similarly, we can define minimal cells. To remove re-
dundant constraints, we only retain constraints for maximal cells, if
the dose is bounded from above. Likewise, only constraints for min-
imal cells are retained, in cases where the dose is bounded from below.
The interactive planning process for finding the dose weights now pro-
ceeds as follows:
After beam directions have been placed computationally, the threshold
values for upper and lower dose limits in tumor and critical regions are
set. These threshold values and region/beam boundaries determine a
system of inequalities. Afterwards it is checked whether this system
admits a solution, i.e. we perform a feasibility test (F).
Notice that the constants ai j in the linear program can be set in such a
way that depth in tissue is represented. A table listing dose attenuation
with depth in tissue is the basis of this process. To adjust the values ai j ,
we examine each of the points corresponding to our LP-constraints.
The points correspond to the j-index in ai j . Given the CT of the target
region, we determine the depth in tissue of each point, depending on
the beam direction.

Fig. 6.17: CT slice for sample case with polygon delineating the target
238 6 Treatment Planning

Figure 6.17 shows a typical planning problem. The tumor is delineated

by a polygon, shown in light gray in the back part of the head. In this
case, there are no critical regions in direct vicinity of the target. The
planning goal is to achieve high conformality and homogeneity. To
specify constraints on the homogeneity of dose within the target, we
require that the tumor dose stays between 2000 cGy and 2200 cGy
(1 Gy = 1 Gray = 1 J/ kg; 1 cGy is 0.01 Gy). An example result of
inverse planning for this case is given in figure 6.18. When this plan is
executed on a phantom, the dose actually delivered can be determined
(see figure 6.19).

Fig. 6.18: Inverse planning input and output for sample case in fig-
ure 6.17. (b) Stack of tumor polygons in each positive slice. (c) 80%
isodose curve after computing the beam weights with linear program-
ming. (d) overlay of (b) and (c).

6.2.2.3 Dose Evaluation

We evaluate a dose distribution with a so-called dose volume histo-

gram (figure 6.20). The x-axis lists dose values in cGy. The y-axis
lists percentages of the total volume. Thus, the histogram gives per-
centages of volumes receiving specific amounts of dose. The histo-
6.2 Planning for Radiosurgery 239

Fig. 6.19: Phantom: photographic film is exposed as the planned treat-

ment for the case in figure 6.17 is executed. (Notice treated region closely
matches the input tumor shape).

gram provides a way to evaluate a dose distribution at the target in

several ways: how well is the target covered? How homogeneous is
the distribution within the target? Likewise, the dose distribution at
critical regions can be evaluated: How big is the fraction of the crit-
ical structure receiving a dose above a given threshold? Typically, for
the target steeper curves correspond to better distributions.

6.2.2.4 Collimators

Typically, the shape of a radiation beam is determined by a collim-

ator. Thus a cylinder beam is generated by placing a collimator with
cylindrical opening in front of the beam source (figure 6.21).
Instead of a cylinder collimator, beams can be shaped with so-called
multi-leaf collimators. In this case, a number of metal leaves determ-
ine the beam shape. The leaves are actuated, and can thus be moved
be motors. In this way the cross-section of the beam can be formed.
240 6 Treatment Planning

100

80

60
Volume [%]

40

20

2769

500 1000 1500 2000 2500

Dose [cGy]

Fig. 6.20: Dose-volume histogram for interactive evaluation of dose dis-

tributions. Tumor volume (bold curve), esophagus (thin dotted line),
spine (dashed line)

Fig. 6.21: Beam collimator. The beam is not cylindrical, but a cone. The
radiation source is at the tip of the cone.

Reachable Robot Configurations and Treatment Planning

In robotic radiosurgery, we are given a set of beam directions, from

which the target must be irradiated. Here it is necessary to find a
valid robot path connecting these beam directions. The path must be
collision-free with respect to the remaining work-space and the pa-
tient. The line-of-sight of imaging devices must not be blocked. To
move along the path, any changes of robot configurations (elbow up /
6.3 Four-Dimensional Planning 241

Target

Movable
leaves

Fig. 6.22: Multi-leaf collimator

elbow down, robot left shoulder configuration / right shoulder config-

uration) should be avoided.
However, configuration changes become necessary whenever the ro-
bot reaches joint angle limits. Thus, it is useful to find a path with
minimal number of configuration changes. Figure 6.23 shows an ex-
ample for motion planning in radiosurgery. Valid directions are on a
sphere of fixed radius, where the target is at the center of this sphere. A
subset of this sphere corresponds to robot positions where no intersec-
tions between the robot arm and lines-of-sight of cameras will occur.
This subset is marked by white points in the figure. The points in this
set (node set) are connected by a series of standard paths, defined for
individual anatomical locations.

6.3 Four-Dimensional Planning

Organs in soft tissue move during respiration. Critical regions close

to the target may move at different velocities. For example, structures
close to the spine may move less than organs further away. This is
illustrated in figure 6.24.
We can address the problem of organ motion in the planning process.
To this end, we must capture the motion of the organs with med-
ical imaging. Four-dimensional CT is suitable for this purpose. Fig-
ure 6.25 illustrates four-dimensional CT. A four-dimensional CT typ-
ically consists of 10 three-dimensional CT stacks, where each stack
corresponds to one time point.
242 6 Treatment Planning

Fig. 6.23: Motion connecting treatment beam positions

Having obtained a four-dimensional CT image set, we can incorporate

organ motion into the planning process. Again we use linear program-
ming. Recall that we computed values ai j as coefficients of our linear
program above. The computation of these values is now adapted to
reflect the motion of the organs. Let p = p j be a voxel. Assume we
have derived our j-th constraint from this voxel p j . For each beam,
we compute the time fraction (over the entire respiratory cycle) dur-
ing which p is in each beam bi . The necessary modifications to our
linear program are straightforward, and we can reflect the motion of
organs in detail.
In the following chapter (chapter 7), we will address the problem of
organ motion with a different method. We will see how to track a mov-
ing target with an instrument, thereby compensating for organ motion
with active tracking.
To this end, we will use several extensions of linear programming.
Quadratic programming is a straightforward extension of linear pro-
gramming. Furthermore, as we shall see, support vector regression is
an extension of quadratic programming, based on dual quadratic pro-
gramming. The exercises below illustrate dual linear programming,
which will be helpful for understanding many of the steps involved.
6.3 Four-Dimensional Planning 243

Fig. 6.24: Motion of organs in soft tissue. The target moves faster than a
critical structure in the vicinity of the target.

Stack 1 Stack 2 Stack 10

Fig. 6.25: Four-dimensional CT

Exercises

Exercise 6.1 Graphical solution of LP problems

Given the LP problem

244 6 Treatment Planning

Maximize

5x1 + x2
subject to

x1 + x2 ≤ 10
x1 ≥ 5
x2 ≥ 4.

Determine a solution by computing all intersection points of bounding

lines and inserting them into the objective function.

Exercise 6.2 Dual of a linear program

Suppose we are given a linear program in matrix form as in equa-

tion 6.10:

(max cx s.t. Ax ≤ b) (6.10)

The so-called dual of an LP problem is also an LP problem. Here the
roles of m and n, and the roles of the vectors b and c are exchanged.
The dual LP is obtained by introducing new variables (α1 , ..., αm )T :

(min bα s.t. AT α ≥ c) (6.11)

Here, AT denotes the transpose of the matrix A, and the vector α de-
notes the vector of the variables αi , i.e. α = (α1 , ..., αm )T . The vari-
ables (α1 , ..., αm )T are called dual variables. To differentiate between
the original version of the LP problem, i.e. as in equation 6.10, and
the dual in equation 6.11, the original version is also called the primal
linear program.
a) Set up the dual of the linear program in exercise 6.1. Find a solu-
tion vector of the dual LP by evaluating the objective function at
the intersection points defined by the (dual) constraints.
6.3 Four-Dimensional Planning 245

b) Let x0 be a solution vector of a linear program. Then a solution

vector y0 of the dual linear program can also be found by the so-
called equilibrium theorem of linear programming. This theorem
relates inequalities of the primal problem to variables yi of the dual
problem. Notice that the number of inequalities of the primal is m,
and the number of variables of the dual is also m. Specifically,
from the above graphical solutions, you will find that some of the
inequalities are the carrier lines of the solution point. I.e. the solu-
tion point x0 is the intersection point of two lines. These two lines
will be called the carrier lines of the solution point. According to
the equilibrium theorem, the carrier lines correspond exactly (in
terms of their indexes) to the dual variables having non-zero val-
ues in the solution y0 . This theorem holds both ways. From this
information alone, determine the solution of the dual for the LP
problem in exercise 6.1, given the solution x0 for the primal prob-
lem.

Summary

Treatment planning relies on geometric constraints describing spatial

relations of anatomical structures. Forward planning is the process of
determining and visualizing the effects of a planned treatment. Inverse
planning starts from a desired result, and parameters for achieving this
result are computed.
In navigated femur osteotomy, anatomical features can be marked on
x-ray image pairs, so that the spatial placement of these features can
be found. With these features, the intervention can be planned interact-
ively by a computer. Thus, for example, we assume the projection geo-
metry of the C-arm is known. Then two circles delineating the femur
head in a pair of x-ray images determine a pair of cones in space.
The intersection of the cones gives the position of the femur head, and
enables 3D navigation. In radiosurgery, the directions from which to
irradiate a tumor must be computed. By using a very large number of
beam directions, the dose distribution can often be improved. In this
case, automatic inverse planning is needed. After finding the direc-
246 6 Treatment Planning

tions for irradiation, the activation duration (called weight) for each
beam must be found. Linear programming provides an effective and
fast way for computing such weights.
In linear programming, a set of linear constraints is checked with re-
spect to feasibility. After computing a feasible point (i.e. a point sat-
isfying the constraints), we can move this point in such a way that an
objective function is maximized or minimized. This objective func-
tion is also a linear function. Assuming the dose from different beams
add up to a total dose, we obtain a linear system of inequalities. In this
system, the dose from each beam is represented as one variable, and
points covering the target region give rise to linear constraints in these
variables. We saw that linear programming directly extends methods
for solving linear equation systems, which we have frequently applied
in the previous chapters.
To reflect organ motion, we modify the linear program accordingly.
Based on four-dimensional imaging, the organ motion can be quanti-
fied. We can thus protect critical structures which would receive too
much dose if organ motion would bring this structure closer to the
target during respiration or pulsation. In the exercises, at the end of
this chapter, we look at duals of linear programs. Variables in the dual
linear program correspond to inequalities in the primal and vice versa.
In the next chapter, we will extend the mathematical tools used so far
to address the problem of tracking soft tissue motion.

Notes

The use of linear programming in inverse radiation therapy planning

was suggested in [1]. The inverse planning methods in [2, 3] have
been clinical standard tools in robotic radiosurgery since 1997. Four-
dimensional planning based on linear programming is described in
[4].
In brachytherapy, small radioactive seeds are implanted into tumors.
The position and weight for the seeds must be planned before treat-
ment. Thus, as in radiation therapy and radiosurgery, dose constraints
for critical regions and dose homogeneity constraints must be ob-
served. The intervention can be guided by robots [5].
References 247

Further examples for inverse treatment planning in orthopedic surgery

can be found in [6].
Treatment planning methods for vascular surgery based on simulation
of vascular flow are described in [7, 8].
As an alternative to ionizing radiation, focused ultrasound can be used
as an ablative surgical instrument [9]. As in radiosurgery and brachy-
therapy, the procedure is based on forward and inverse treatment plan-
ning. Further applications are hyperthermia [10], thermo-ablation [11]
and transcranial direct current stimulation [12].
Several authors have suggested to apply machine learning to treatment
planning (see e.g. [13]). Thus, we can characterize recurring situations
and geometric constraints to simplify and shorten the time-consuming
interactive processes. For the case of radiosurgery, treatment planning
can take several hours in each case. The quality of the plan depends to
a large extent on the availability of resources and the expertise of the
user.

References

[1] Y. Censor, M. D. Altschuler, and W. D. Powlis, “On the use

of Cimmino’s simultaneous projections method for computing
a solution of the inverse problem in radiation therapy treatment
planning,” Inverse Problems, vol. 4, no. 3, p. 607, 1988.
[2] A. Schweikard, R. Tombropoulos, L. Kavraki, J. R. Adler, Jr.,
and J.-C. Latombe, “Treatment planning for a radiosurgical sys-
tem with general kinematics,” in IEEE International Conference
on Robotics and Automation (ICRA 1994), 8-13 May 1994, pp.
1720–1727.
[3] A. Schweikard, M. Bodduluri, R. Tombropoulos, and J. R.
Adler, Jr., “Planning, calibration and collision-avoidance for
image-guided radiosurgery,” in Proceedings of the IEEE/RSJ/GI
International Conference on Intelligent Robots and Systems
(IROS’94), vol. 2, 12-16 Sept. 1994, pp. 854–861.
[4] A. Schlaefer, J. Fisseler, S. Dieterich, H. Shiomi, K. Cleary, and
A. Schweikard, “Feasibility of four-dimensional conformal plan-
248 6 Treatment Planning

ning for robotic radiosurgery.” Medical Physics, vol. 32, no. 12,
pp. 3786–3792, Dec 2005.
[5] G. Fichtinger, J. P. Fiene, C. W. Kennedy, G. Kronreif, I. Ior-
dachita, D. Y. Song, E. C. Burdette, and P. Kazanzides, “Robotic
assistance for ultrasound-guided prostate brachytherapy,” Med-
ical Image Analysis, vol. 12, no. 5, pp. 535 – 545, 2008, special
issue on the 10th international conference on medical imaging
and computer assisted intervention - {MICCAI} 2007.
[6] H. Gottschling, M. Roth, A. Schweikard, and R. Burgkart, “In-
traoperative, fluoroscopy-based planning for complex osteotom-
ies of the proximal femur,” International Journal of Medical Ro-
botics and Computer Assisted Surgery, vol. 1, no. 3, pp. 67–73,
2005.
[7] S. Nakajima, H. Atsumi, A. H. Bhalerao, F. A. Jolesz, R. Kikinis,
T. Yoshimine, T. M. Moriarty, and P. E. Stieg, “Computer-
assisted surgical planning for cerebrovascular neurosurgery,”
Neurosurgery, vol. 2 41, pp. 403–410, 1997.
[8] N. Wilson, K. Wang, R. W. Dutton, and C. Taylor, “A soft-
ware framework for creating patient specific geometric models
from medical imaging data for simulation based medical plan-
ning of vascular surgery,” in Medical Image Computing and
Computer-Assisted Intervention - MICCAI 2001, ser. Lecture
Notes in Computer Science, W. J. Niessen and M. A. Viergever,
Eds. Springer Berlin Heidelberg, 2001, vol. 2208, pp. 449–456.
[9] C. M. C. Tempany, E. A. Stewart, N. McDannold, B. J. Quade,
F. A. Jolesz, and K. Hynynen, “MR imaging–guided focused ul-
trasound surgery of uterine leiomyomas: A feasibility study,” Ra-
diology, vol. 226, no. 3, pp. 897–905, 2003, pMID: 12616023.
[10] A. Jordan, R. Scholz, P. Wust, H. Fähling, and R. Felix, “Mag-
netic fluid hyperthermia (MFH): Cancer treatment with AC mag-
netic field induced excitation of biocompatible superparamag-
netic nanoparticles,” Journal of Magnetism and Magnetic Ma-
terials, vol. 201, no. 1-3, pp. 413 – 419, 1999.
[11] S. Rossi, M. Di Stasi, E. Buscarini, P. Quaretti, F. Garbagnati,
L. Squassante, C. T. Paties, D. E. Silverman, and L. Buscarini,
“Percutaneous rf interstitial thermal ablation in the treatment of
References 249

hepatic cancer.” American Journal of Roentgenology, vol. 167,

no. 3, pp. 759–768, Mar. 2014.
[12] J. P. Dmochowski, A. Datta, M. Bikson, Y. Su, and L. C. Parra,
“Optimized multi-electrode stimulation increases focality and
intensity at target,” Journal of Neural Engineering, vol. 8, no. 4,
p. 046011, 2011.
[13] M. Hauskrecht and H. Fraser, “Planning treatment of ischemic
heart disease with partially observable Markov decision pro-
cesses,” Artificial Intelligence in Medicine, vol. 18, no. 3, pp.
221 – 244, 2000.
Chapter 7
Motion Correlation and Tracking

To track the motion of an anatomic structure in real-time, we need

very fast and robust imaging methods. However, this is often insuffi-
cient. A correlation-based approach can help here. Suppose we have
an external structure (e.g. the patient’s skin) which can be tracked
easily, with optical methods. Then suppose we have an internal target,
which is often very difficult to track.
Now we record the motion of the external structure (via highspeed
imaging) and infer the position of the internal target via motion cor-
relation. This can only work if the two motions (external and internal)
are indeed correlated. Thus, we must establish that there is such a cor-
relation, from physiological data. Clearly, such a correlation could be
a complex mathematical dependency, subject to damping and hyster-
esis. Thus, it would seem likely that we can apply machine learning
to the problem of finding the exact correlation pattern.

Fig. 7.1: Tracking a moving target in external beam radiation therapy

251
252 7 Motion Correlation and Tracking

If we succeed in finding the correlation pattern, and if this pattern

remains suffciently stable throughout treatment, then we can track in-
ternal structures with high precision. The problem of learning such
correlation patterns is one instance of motion learning. It seems reas-
onable to study the more general problem of motion learning in robot-
ics, since many aspects of what we regard as dexterity rely on learning.
As discussed in chapter 6, in external beam radiation therapy, tumors
are irradiated with a moving beam of radiation. Tumors in the chest
and abdomen move due to respiration. Our goal is to track this motion,
see figure 7.1.

7.1 Motion Correlation

Respiratory motion is periodic. Furthermore, we observe that the mo-

tion of the target tumor and the motion of the patients’s skin are cor-
related.

Fig. 7.2: Motion correlation

Figure 7.2 shows an example. In the figure, the motion of the skin
surface is illustrated by the two vertical arrows. Thus, the skin surface
(arc line in the figure) moves up and down during the respiratory cycle.
The target (gray circle) moves in left-right direction. At full exhalation
(left image), the target is on the left side. At full inhalation, the target
is on the right side.
Our main observation is the following: tracking the skin surface is
easier than tracking a tumor inside the body.
7.1 Motion Correlation 253

To track the skin surface, all we need is an optical or magnetic tracking

marker placed on the skin. Optical tracking gives us exact position
information in real-time, and no image analysis is needed.
Hence, we combine two sensors (x-ray and optical) in the following
way: We take a stereo x-ray image pair and add a time-stamp to this
image pair. We read the optical tracking system (or infrared tracking
system) for the same point in time. This is done for several points in
time. We then establish a correlation model between the motion of
external markers visible with optical tracking and the internal motion
of the target. The position of the internal target for images taken in the
past is computed from x-ray images. From this correlation model, we
infer the exact real-time position of the target tumor.
Intermediate positions of the target can now be computed by interpol-
ation.
Figure 7.3 illustrates the process of correlation-based tracking.

infrared data
(continuous)

X-ray data points

Fig. 7.3: Correlation-based tracking: the bold curve stems from the real-
time sensor (here optical tracking). A series of points on the dashed
curve are known (here: x-ray images), but these points are in the past,
since we need segmentation time. The bold curve and the dashed curve
(here: ground truth and target position) are in correlation. Our goal is
to estimate the unknown dashed curve from this data.

By taking more than two positions of the target, we can refine this
correlation model. Thus curvilinear motion of the target can be ap-
proximated by a series of line segments, giving a model for this mo-
tion. The correlation model is continuously updated during treatment
to reflect changes in breathing pattern.
As in the case of registration, we can replace specific image mod-
alities but maintain the general method. In our description, optical
254 7 Motion Correlation and Tracking

tracking takes the role of a real-time sensor, and x-ray imaging com-
putes ground truth intermittently. Motion correlation then determines
the ground truth in real-time.

Example 7.1

Several scenarios for correlation-based tracking are possible. As dis-

cussed above, the first scenario links optical tracking and x-ray ima-
ging. Tumors are not always visible in x-ray imaging. Then we can
employ the following pseudo-stereotaxic scheme for tracking. Small
gold landmarks are placed in the vicinity of the target before treat-
ment. The relative position of the target with respect to the gold land-
marks is known from a planning CT, also taken before the interven-
tion.
During treatment, we take stereo x-ray images intermittently, and the
position of the gold landmarks is determined in the images.
Thus, motion tracking consists of the following steps.
• Before treatment (patient in treatment position on treatment couch)
1. Take x-ray image pairs with time stamps
2. Take optical skin marker readings with time stamps
3. Match data sets with corresponding time stamps
4. Compute correlation model
• During treatment
1. Take sensor reading
2. Infer corresponding internal target position from optical data
and correlation model
3. Periodically (e.g. once every five seconds) take new x-ray im-
age pair
4. Compute an updated correlation model

(End of Example 7.1)

7.2 Regression and normal equations 255

7.2 Regression and normal equations

To apply motion correlation, we must compute the correlation model.

We are given data points with respiration data, taken before treatment.
An exceedingly simple way to compute such a model is the following.
We determine the coordinate axis with the largest excursion in the
data points. This is done for both the x-ray image data and for the
optical marker data. Both data consist of three-dimensional points.
But we ignore the other two coordinate directions in favor of the one
with largest excursion. Assume the coordinate with largest excursion
is the z-coordinate for the optical data and the x-coordinate for the
x-ray data. We take the two points with smallest and largest z-axis
value (resp. x-axis value). Linear interpolation is then used to infer
the internal target position from the external optical emitter position
figure 7.4.

x-ray data points optical data

points

Fig. 7.4: Data points from optical tracking and x-ray imaging. Both
types of sensor data points have time stamps. This allows for establishing
the correspondences between the data points.

Obviously this very simple technique has several drawbacks. It simply

connects two data points by a line, and ignores all the other points. A
more suitable method for fitting a line is described next. This tech-
nique takes all points into consideration, since they are valid data
points. In figure 7.5 we select and visualize a single coordinate (here
the x-coordinate) for both types of data. We do this for all three co-
ordinates, and obtain a visulization for the correlation model.
Suppose now we are given data points as in the following table:
256 7 Motion Correlation and Tracking

(x-coordinate)
X-ray
optical
(x-coordinate)

Fig. 7.5: Line Regression

(1, 9)
(2, 21)
(3, 33)
(4, 39)
(5, 50)

If we take a closer look, we see that the data points are roughly on a
line. We wish to find the line that best fits the data. This line does not
necessarily contain all the points, but it should match the data. Our
goal is thus to fit the line y = mx + b to the data, i.e. solve the system

b + 1m = 9
b + 2m = 21
b + 3m = 33
(7.1)
b + 4m = 39
b + 5m = 50.

The system is over-determined.

To find the solution for an overdetermined linear system, we could ap-
ply a method called the ART method. ART solves an overdetermined
system by approximating the intersection of n planes chosen at ran-
7.2 Regression and normal equations 257

dom (n is the number of variables). This is based on the assumption

that the intersection points are very close together. Simple examples
show that this approach may not find very good regression lines. The
ART method is discussed in detail in exercise 1.2.
Instead, a better approach is to look for values (b0 , m0 )T which min-
imize the expression:

(b + 1m − 9)2 + ... + (b + 5m − 50)2 (7.2)

This is equivalent to finding a line that minimizes the sum of squared
distances to the data points. We write the system in equation 7.1 in
matrix form:
   
11 9
1 2   21
   
B= 1 3  , w = b , y = 33 (7.3)
  m  
1 4 39
15 50
Then our system is:

Bw = y (7.4)
Since B is a 5x2-matrix, BT will be a 2x5-matrix, and we can rewrite
our system to:

BT Bw = BT y (7.5)
This equation is called the normal equation for the system in equa-
tion 7.4. BT B is a 2x2-matrix. Now this system can be solved by
Gaussian elimination. Notice that the transition from Bw = y to
BT Bw = BT y substantially reduces the matrix size. It can be shown
that solutions of the least-squares problem (equation 7.2) are solu-
tions of equation 7.5 and vice versa (see exercise 7.1 at the end of this
chapter).
258 7 Motion Correlation and Tracking

7.3 Support Vectors

The method described in the last section has a remaining drawback. It

does fit a line to a cloud of points, and does take into account all of the
points equally. However, in practice it turns out that respiration data is
subject to hysteresis. This means that the inhale curve of a point may
be different from the exhale curve, as shown in figure 7.6.

inhale

exhale

Fig. 7.6: Hystersis: inhale path of target differs from exhale path.

The difference can be quite large. A simple way to address this prob-
lem is to fit two lines to the data instead of just one. Here, we would
use the optical tracking data to determine a gradient in the data, i.e.
decide whether a given data point is an inhale or an exhale point. We
would then map to one of the two regression lines. It turns out that res-
piratory traces are indeed curves and not just lines, so obviously we
should attempt to fit curves to the data points. Polynomials of degree
two or higher can be fit to the points by least squares methods. How-
ever, it is useful to include more sensor information. Beyond gradient
information on the motion, speed and acceleration can be measured. A
breath temperature sensor distinguishes between respiratory phases. A
spirometer measures respiratory air flow. Similarly, data for pulsation
can be acquired in a variety of ways.
Support vector methods collect heterogeneous data to set up training
samples. Support vector regression computes a functional representa-
tion of correlations in the data. In the most simple form, the process
resembles line regression. However, very complex correlations can be
captured.
7.3 Support Vectors 259

output
input

Fig. 7.7: Regression function for a cloud of points

Furthermore, having data from heterogeneuos sources, it would seem

necessary to remove data with weaker correlation to the target mo-
tion. Thus we need mathematical methods for analyzing correlation
patterns in time series with data from heterogeneous sources.
Above we saw that the problem of computing a correlation model can
be regarded as a line-fitting problem (figure 7.5). With support vector
regression, we can fit a line to a cloud of points. Support vector regres-
sion can also fit a curve to such a cloud (figure 7.7). Of course there are
many mathematical methods to do just this: fit a curve to a point cloud.
The advantage of support vector regression is the enormous flexibility.
But more than that, support vector regression relies on a globally con-
vergent optimization engine. This engine (called quadratic program-
ming) performs the basic computation. You may not think much of
this latter advantage. However, it is a big advantage in practice. No
divergence will occur, and the optimization will not get stuck in local
minima.
In the previous chapter, we saw applications of linear programming
in treatment planning. Support vector methods emerged from linear
programming and can be regarded as a direct extension.
To emphasize the fact that support vectors work in higher dimensions
as well, we write the input values as vectors xi .
Hence, we have assumed a function with vector input xi has a scalar
function output yi . Since we wish to work in high-dimensional space,
we must think of our regression function to be a plane in space rather
than a line in the plane. More generally, the regression function will
260 7 Motion Correlation and Tracking

be a hyperplane in a space of dimension n. Or even more generally, it

will be a curved surface in high-dimensional space.
Before we look at regression with support vectors, we will look at the
problem of separating two point clouds by a line. We will address this
problem by support vector theory. It will turn out that the two prob-
lems (separating and regression) are related, but the basic concepts can
be illustrated more easily if we look at the problem of separation first.
Assume we have a planar example with only four points x1 , x2 , x3 ,
and x4 . We seek a line such that x1 and x2 are above and x3 and x4 are
below this line. The situation is illustrated in figure 7.8.

w
x1 x4

x2 x3

Fig. 7.8: Separating two clouds of points

7.3.1 Representation of lines

Typically, lines are represented by equations of the form y = mx +

b. For our application, we will not use this representation of lines.
Instead, we will represent lines (in the plane) in the form

w1 x1 + w2 x2 + d = 0. (7.6)
To visualize this representation of lines, notice that w = (w1 , w2 )T is
the normal vector of the line. But furthermore, the direction of this
normal vector w indicates an orientation of the line. Thus a point p =
7.3 Support Vectors 261

(p1 , p2 )T is above the line (in the direction into which w is pointing),
if

w1 p1 + w2 p2 + d > 0. (7.7)
And p will be below the line if w1 p1 + w2 p2 + d < 0. The representa-
tion of lines discussed here also has a close relationship to distances.
Namely, if w is a unit vector, then we know the distance of p from the
line. It is simply the magnitude of the value w1 p1 + w2 p2 + d.
Notice also that this representation of lines is not unique. If wx+d = 0
is a line, then (λ wx) + λ d = 0 is the same line. Thus, we can scale
our line equations. The scaling that turns the normal vector into a
unit vector (i.e. λ = 1/kwk) is often useful. One advantage of this
representation of lines is that it works in any dimension. Hence,

w1 x1 + w2 x2 + w3 x3 + d = 0 (7.8)
is a plane in space, with much the same distance properties we just
discussed. And the same holds for higher dimensions.

7.3.2 Linear programming for separating point clouds

Assume as above we have a planar example with only four points

x1 , x2 , x3 , and x4 . We seek a line such that x1 and x2 are above and x3
and x4 are below this line. This is the situation from figure 7.8.
Consider the linear program

x11 w1 + x12 w2 + d ≥ 0
x21 w1 + x22 w2 + d ≥ 0
x31 w1 + x32 w2 + d ≤ 0
x41 w1 + x42 w2 + d ≤ 0.
(7.9)

This is indeed a linear program, but here the objective function is

empty, i.e. (c1 , c2 )T = (0, 0)T . Notice that the values wi and d are the
variables of this program! And the xi j are the constant coefficients.
262 7 Motion Correlation and Tracking

A solution of this linear program will assign values to w = (w1 , w2 )T

and d. Then the separating line is given by the normal vector w and
the value d.
We will use values yi = ±1 to indicate whether our data points xi
should be above the line or below. We rewrite the above linear pro-
gram to include the values yi :

y1 (wx1 + d) ≥ 0
y2 (wx2 + d) ≥ 0
y3 (wx3 + d) ≥ 0
y4 (wx4 + d) ≥ 0
(7.10)

The system in equation 7.10 is the same as the system in equation 7.9.
We have turned all inequalities into ≥-inequalities.
Now the linear program above will return a line such as the line shown
in the next figure (figure 7.9). Notice that two of the points in figure 7.9
are actually on the separating line, hence not strictly below or above
it. These are the points x2 and x4 . Indeed linear programming will
typically return this type of separating line. One can argue that this
type of separating line does not represent the data points very well.

x1
x4

x2
x3

Fig. 7.9: Separator line for the classification problem in equation 7.10

Assume the points x1 , x2 , x3 , and x4 are samples from a larger data set
with more (unknown) sample points. We would again like to classify
7.3 Support Vectors 263

the data into two subsets. It would then seem more reasonable to use
the dashed line in figure 7.10 as a separator line.

x1
x4

x2
x3

Fig. 7.10: Maximum margin separator line for the classification problem
in equation 7.10

The advantage of this dashed line is that the margin between the sep-
arating line and the data points is maximal. In the picture, the data
points can be thought of as repelling points which push the separating
line away by the maximum amount. Likewise, we may say the data
points ‘support’ the dashed separator line.
The basis of support vector theory is the observation that one can find
such a maximum margin separator plane with quadratic programming.
Specifically, we replace the above linear program for finding the sep-
arator plane by the program:
Minimize

w21 + w22
subject to

y1 (wx1 + d) ≥ 0
y2 (wx2 + d) ≥ 0
y3 (wx3 + d) ≥ 0
y4 (wx4 + d) ≥ 0.
(7.11)
264 7 Motion Correlation and Tracking

Recall that linear programs typically have an objective function as

well. However, the program in equation 7.11 is not a linear program,
since here the objective function is quadratic.
One would certainly want to know why the quadratic program in equa-
tion 7.11 does indeed compute a maximum margin line. For those who
want to know, we give an example at the end of this chapter (see ex-
ercise 7.2).

7.3.3 Regression

Having looked at maximum margin separator lines, we return to the

problem of finding a regression line. We saw that quadratic program-
ming does give a maximum margin separating line. Likewise it can
be shown that quadratic programming can provide a regression line
amidst a point cloud such that certain optimality criteria (resembling
the optimal margin) are satisfied.
The regression line (or plane) can be found with quadratic program-
ming:
Minimize
wwT (7.12)
subject to

yi − (wxi + d) ≤ ε
(wxi + d) − yi ≤ ε.

The constraints mean that the distance between the value (wxi + d)
and the value yi should not be bigger than ε. Recall that the values yi
in the case of regression are the function values at the points xi . Thus
we no longer have yi = ±1. Instead the values yi are real numbers.
Notice also that there are two constraints per sample point xi . The
two constraints bound the distance from below and above. This means
that the regression line must not have a distance more than ε from all
samples (xi , yi ). This may not always be feasible, i.e. if ε is too small.
To allow for finding a regression line, even if the points are far apart,
we introduce slack variables. Slack variables allow for a violation of
7.3 Support Vectors 265

the constraints. (We already saw a very simple application of slack

variables in chapter chapter 6).
Thus, instead of requiring

yi − (wxi + d) ≤ ε, (7.13)
we ask for a little less, namely

yi − (wxi + d) ≤ ε + ξ , (7.14)
where ξ is a variable (ε is a constant)!
ξ is treated just like any other variable in quadratic programming (or
linear programming), i.e. ξ ≥ 0. You should take a minute to see why
equation 7.14 is indeed a weaker constraint than equation 7.14.
We would like ξ to be as small as possible, since ξ is the amount by
which the original constraint is violated.
Thus we include the minimization of the ξ -values into the above quad-
ratic program:
Minimize
ww + ξ1 + ξ10 + ξ2 + ξ20 . . .
subject to

y1 − (wx1 + d) ≤ ε + ξ1
(wx1 + d) − y1 ≤ ε + ξ10
y2 − (wx2 + d) ≤ ε + ξ2
(wx2 + d) − y2 ≤ ε + ξ20
...
ξi , ξi ≥ 0.
0

(7.15)

We introduce a constant C, which represents a trade-off between the

weight of the quadratic part of the objective function (ww) and the
linear part. Thus, we modify the objective function in equation 7.15
to:
Minimize
ww +C(ξ1 + ξ10 + ξ2 + ξ20 . . . ).
266 7 Motion Correlation and Tracking

7.3.3.1 Dualization

Support vector machines do not employ quadratic programming in the

form in equation 7.15. Rather, they rely on dualization. Dualization is
a standard technique in linear programming (see also the exercises for
chapter 6 for a graphical visualization of this technique). Similar to
linear programs, quadratic programs can be written in dual form.
As we shall see, the dual form has several advantages.
The dual QP for regression is:
Maximize

1 m m m
− ∑ (αi − αi )(α j − α j )xi x j − ε ∑ (αi + αi ) + ∑ yi (αi − αi0 )
0 0 0
2 i, j=1 i=1 i=1

subject to
m
∑ (αi − αi0) = 0
i=1
and

αi , αi0 ∈ [0,C]. (7.16)

Unfortunately, the dual does not look much like the primal in equa-
tion 7.15 from which we started. However, we can recognize the con-
stant C from the primal. Also, we still find ε.

Remark 7.1

The process of computing the dual from a given primal is based on

Lagrange multipliers: We start from the so-called Lagrange function
of the primal input system, take derivatives with respect to the (primal)
variables w, d, and ξi , ξi0 , set the resulting expressions to zero and sub-
stitute them back into the Lagrange function. Lagrange functions are
illustrated in the exercises below. Given a small amount of theory on
Lagrange multipliers, the derivation of the dual quadratic program
from the primal quadratic program is not difficult. One of the exer-
cises at the end of this chapter shows that the dual in equation 7.16 is
7.3 Support Vectors 267

indeed a quadratic program in the first place (exercise 7.3). As a fur-

ther exercise (see end of this chapter), we show how to derive a very
simple dual linear program from a primal linear program, all based on
Lagrange theory.
The exercise 7.3 at the end of this chapter not only illustrates the the-
ory behind support vector machines in much more detail, but it also al-
lows you to implement an exceedingly simple support vector machine.
To be able to apply support vector machines in other contexts, this
will turn out to be most helpful. Likewise, exercises 7.4 and 7.5 will
help to get a sense how the dual quadratic program in equation 7.16
is derived from the primal. Finally, you may again consider solving
exercise 7.2, since it further illustrates why we have set up the primal
in equation 7.11 the way we did.

(End of Remark 7.1)

We mentioned the fact that the dual has several most surprising ad-
vantages over the primal. We will now set out to include kernel func-
tions into the dual system in equation 7.16. This will allow us to find
non-linear regression functions, which we need in our application.
But before we do that, one more thing remains:
Having solved the quadratic program in equation 7.16, e.g. with the
Matlab routine quadprog, we get values for the variables αi , αi0 . How
can we get back to the values for w and d from the values for αi , αi0 ?
The answer is: As part of the dualization process, we obtain equa-
tions (setting to zero derivatives with respect to dual variables). These
equations state that
m
w = ∑ (αi − αi0 )xi = 0. (7.17)
i=1
The value d can be computed in the following way:
m
d = y j − ∑ (αi − αi0 )xi x j − ε, (7.18)
i=1
for a j with α j 6= 0, or
268 7 Motion Correlation and Tracking
m
d = y j − ∑ (αi − αi0 )xi x j + ε, (7.19)
i=1

for a j with α 0j 6= 0.
Finally, we must evaluate the function value y for an unknown sample
x.
Here, of course, we have

f (x) = wx + d, (7.20)
but it is preferable to write
m
f (x) = ∑ (αi − αi0 )xi x + d, (7.21)
i=1
where we have simply inserted our formula for w derived above. The
reason for this rewriting will become clear after we have discussed
kernel functions. We will do this next.

7.3.4 Kernels

Consider the function F which maps points x = (x1 , x2 )T in two di-

mensions to
 
  x1 x1
x x1 x2 
F : 1 → 
x1 x2  . (7.22)
x2
x2 x2
Hence, F maps from the input space (2D) to four-dimensional space.
Here, 4-space is called the feature space of the mapping.
The reason why we would map samples from 2D space into 4D space
is again best illustrated for the context of separation: In higher dimen-
sions, it may be easier to separate the sample points by a plane. This is
illustrated by the following example. Three points in 2D (two positive
/ one negative) can always be separated by a line. However, for four
such points (two positive / two negative) this may not be the case. (Try
to find a 2D example for this case.) But in 3D, four points can always
7.3 Support Vectors 269

be separated by a plane. This generalizes to higher dimensions. In n-

space, n + 1 points can always be separated. Thus there is heuristic
motivation to map the input sample points to a space of higher di-
mension, in order to simplify the separation. An example below will
further illustrate this observation.
We again consider the above map F. Then for two points x, y we have

F(x)F(y) = (xy)2 . (7.23)

To see this, simply evaluate the left hand side:

  
x1 x1 y1 y1
x1 x2  y1 y2  2 2 2 2
F(x)F(y) =   
x2 x1  y2 y1  = x1 y1 + 2x1 x2 y1 y2 + x2 y2 (7.24)
x2 x2 y2 y2

Likewise, the right hand side evaluates to:

(xy)2 = x12 y21 + 2x1 x2 y1 y2 + x22 y22 (7.25)

The effect of the mapping is shown in figures 7.11 and 7.12. For the
first figure, random data points uniformly distributed in the rectangu-
lar region [-2, 2] by [-2, 2] were generated. We mark all data points
within the unit circle (i.e. points (x1 , x2 )T in the plane with x12 +x22 < 1)
as positive, and we mark the points outside the circle as negative.
In the second picture, the same points were mapped under F. No-
tice that the original (unmapped) points cannot be separated by a line.
However, after the mapping under F, the points can be separated by
a plane! Notice that the figure shows a 3D subspace of the 4D output
space of F.
Hence, after mapping to higher dimensions under F, the data become
linearly separable. This is in accordance with the above observation
that separating points by hyperplanes is easier in higher dimensions.

Kernels and Quadratic Programming

We return to the problem of finding a regression plane. Instead of com-

puting the regression plane in the original data space, we will compute
270 7 Motion Correlation and Tracking
Positive: x21 + x22 < 1, Negative: Else
2

1.5

0.5

−0.5

−1

−1.5

−2
−2 −1.5 −1 −0.5 0 0.5 1 1.5 2

Fig. 7.11: Random sample points in interval [-2,2] by [-2,2]. Positive

samples marked as ’+’, negative samples marked as dots.
4

0
4
2 4
0 3
2
−2 1
−4 0

Fig. 7.12: Sample points from figure 7.11 mapped under

F : (x1 , x2 )T → (x̃1 , x̃2 , x̃3 , x̃4 )T = (x1 x1 , x1 x2 , x2 x1 , x2 x2 )T . Cross-sectional
(x̃1 , x̃2 , x̃4 )-subspace.
7.3 Support Vectors 271

the regression plane in high-dimensional feature space. To this end, it

suffices to map the input data points xi under a mapping F. To find
a regression plane, one could solve the following dual quadratic pro-
gram (version I):
Maximize
m m m
−1/2 ∑ (αi −αi0 )(α j −α 0j )F(xi )F(x j )−ε ∑ (αi +αi0 )+ ∑ yi (αi −αi0 )
i, j=1 i=1 i=1

subject to
m
∑ (αi − αi0) = 0
i=1
and
αi , αi0 ∈ [0,C]. (7.26)
However: We can also solve the following dual QP (version II):
Maximize
m m m
−1/2 ∑ (αi − αi0)(α j − α 0j )(xix j )2 − ε ∑ (αi + αi0) + ∑ yi(αi − αi0)
i, j=1 i=1 i=1

subject to
m
∑ (αi − αi0) = 0
i=1
and
αi , αi0 ε[0,C]. (7.27)
Why is it better to solve the dual QP in version II, rather than solving
it in version I above? The reason is: We have

F(x)F(y) = (xy)2 . (7.28)

Thus, the two programs are the same, and give the same solution. But
it can be hard to compute F(x) and F(y) explicitly, while it is often
much easier to compute (xy)2 . Specifically, the above function F is
only one example for such a function. For other functions, F(x) is
much harder to compute.

Example 7.2
272 7 Motion Correlation and Tracking

It can be shown that there is a function F, for which

F(x)F(y) = (1 + xy)k . (7.29)

But computing F(x) explicitly in this case would require evaluating
an exponential number of terms (exponential in k), and this function
F maps vectors to a space of dimension exponential in k.
The advantage of solving the dual QP in version II is that F is never
computed explicitly. Instead, one only evaluates terms of the form
(1 + xy)k .
This is much more efficient.

(End of Example 7.2)

Remark 7.2

An important point to note is that the last observation also holds for
the step of computing w and d, from the values αi and αi0 . You may
wish to check this in the formulas defining the values for w and d
(equation 7.17 and equation 7.19). (End of Remark 7.2)

Hence, equation 7.27 gives the final method for finding a regres-
sion function with support vector machines. Notice that the use of
kernels allows for finding non-linear regression functions as in fig-
ure 7.7. Equation 7.27 could be implemented in Matlab, e.g. by calling
the Matlab-function quadprog. However, some minor rearrangement
would be necessary to do this. Furthermore, the quadprog() module in
Matlab does not give very good results in this case. Only very small
data sets can be handled. A much better way to implement support
vector methods is to use specialized algorithms for solving the optim-
ization problem in equation 7.27. While this problem is indeed a quad-
ratic program, more efficient algorithms for solving this special type
of problem have been developed [1]. Such algorithms are provided in
dedicated libraries for support vector methods [2].

(End of Remark 7.2)

7.3 Support Vectors 273

The following example illustrates the difference between simple line

regression and support vector regression.

Example 7.3

Support vector regression does not compute an explicit curve repres-

enting the correlation, such as a linear or polynomial curve, but rather
a high-dimensional regression hyperplane which is then mapped back
to the low-dimensional data space. The hyperplane is linear, but the
mapping can be non-linear if we use a kernel function.
To illustrate the difference between explicit regression and support
vector regression and evaluate practical benefits, we need respiratory
data, ideally without relative latency between different image sources.
Fluoroscopy is an x-ray image modality, and aquires images in real-
time. Both LED skin markers and implanted gold markers are visible
in fluoroscopic images. Hence, we do not need to calibrate relative
acquisition latencies.
For explicit regression (i.e. line regression with normal equations, or
polynomial curve fitting), we can visualize the dependency between
motions of LED markers and fiducial markers as a line or a curve.
Figure 7.13 shows the result of a measurement with two-plane flu-
oroscopy. Due to hysteresis, the point cloud is partitioned into two
subregions. The figure also shows a regression line.
Figure 7.14 shows the results for support vector regression. We see
that support vector methods can represent hysteresis directly.
Instead of fitting a single line to the data, we can also fit polynomi-
als to the data points. Figure 7.15 shows two polynomial curves. The
curves are blended at both ends to obtain a better match to the data
and to avoid extrapolation errors.
Figure 7.16 compares polynomial fitting (degree less than 6) to sup-
port vector regression. The improvement obtained by support vector
regression is clearly visible.

It is often difficult or undesired to record fluoroscopic data. Thus it

is useful to look for other ways to validate and compare correlation
274 7 Motion Correlation and Tracking
290

288

286

284

282

280

278
141 142 143 144 145 146 147 148 149

Fig. 7.13: Motion correlation measured with two-plane fluoroscopy. The

x-axis shows the motion of an optical marker along a single coordinate
in space. The y-axis shows the motion of an internal marker, again along
a single coordinate. We fit a line to the training data points (large dots).
The small dots represent the data points from fluoroscopy. The effect of
hysteresis is clearly visible.

methods. Ultrasound is non-invasive. Figure 7.17 shows a set-up for

recording correlation data via ultrasound imaging.
A robotic arm holds the ultrasound camera. Optical markers (LEDs)
are placed in a belt on the patient’s chest. The ultrasound images do
not show the positions of both types of markers. Thus, the latency of
ultrasound image acquisition must be determined.
In figure 7.18, a robot arm moves a phantom object in a water tank.
The phantom is a small lead ball attached to a fixture, moved by the
robot. The ultrasound camera tracks the motion of the phantom. This
process generates two curves. The delay between the two curves rep-
resents the relative latency of ultrasound acquisition. Here, relative
latency means the latency between ultrasound imaging and robot mo-
tion. By bringing the two curves into a matching position, we can
calibrate the relative latency time.
7.3 Support Vectors 275
290

288

286

284

282

280

278
141 142 143 144 145 146 147 148 149

Fig. 7.14: Support vector regression for motion correlation. Large gray
dots: training data. Small black dots: fluoroscopy data. Small gray dots:
points computed by support vector regression. Same data as in fig-
ure 7.13.
290

288

286

284

282

280

278
141 142 143 144 145 146 147 148 149

Fig. 7.15: Representing hysteresis by a correlation model consisting of

two polynomial segments. Data as in figure 7.13.
276 7 Motion Correlation and Tracking
288

amplitude [mm]
286
284
282
280
278
0 20 40 60 80 100 120
time [s]

288
amplitude [mm]

286
284
282
280
278
0 20 40 60 80 100 120
time [s]
fiducial correlation output model points error

Fig. 7.16: Motion traces for the data in figure 7.13. Top row: polynomial
fitting. Bottom row: support vector regression.

Ultrasound probe
with pressure sensor

Belt with LED

markers

Fig. 7.17: Validating motion correlation with ultrasound. A pressure

sensor measures the force applied to the skin surface of the patient. The
robot adjusts the position of the camera accordingly.

However, now we only have the relative latency. To compute absolute

latencies, we can proceed as follows. We first calibrate the latency
of a fast optical tracking system: By means of an oscilloscope and
additional electronics, this can be done within an accuracy of below
0.5 ms. We then determine robot latency with a set-up of two robots
of the same type. A motion command for the first robot is issued at
time point t0 . This motion is detected by an optical marker attached to
the first robot.
7.4 Double Correlation 277

Ultrasound
probe

Phantom

Fig. 7.18: Phantom set-up for latency calibration.

280
left robot
x−axis position [mm]

270 right robot

260

250

240

8 8.5 9 9.5 10 10.5 11 11.5 12 12.5 13

time [s]

Fig. 7.19: Latency measurement with two robots

At this point in time, we trigger a motion command for a second robot.

The second robot also carries an optical marker. The two markers are
visible for the same tracking camera. We determine the graph for both
marker motions, and compute the delay from the graph (Figure 7.19).

(End of Example 7.3)

7.4 Double Correlation

Implanted markers have several disadvantages. Firstly, implanting

is invasive, and requires an additional procedure before treatment.
Secondly, the markers can move with respect to the target. This can
278 7 Motion Correlation and Tracking

cause inaccuracies. We see the rib cage in x-ray images, but the tu-
mor is not always visible. The rib cage moves during respiration. To
obviate the need for implanted markers, we can use two correlation
models, both of which are established before treatment:
• model 1: external marker motion to rib cage motion
• model 2: rib cage motion to target motion
We combine the two correlation models in a chain to infer the target
position in real-time (figure 7.20).

skin markers rib cage rib cage target

DRRs 4D CT
double correlation

Fig. 7.20: Double correlation. The motion of the rib cage is detected by
digitally reconstructed radiographs (DRRs). Before treatment the target
is marked in a series of CT scans, showing distinct respiratory states (4D
CT). DRRs are computed from a 4D CT data set. Live shots are matched
to the DRRs to detect the current respiratory state, and compute ground
truth location of the target.

To establish the correlation between external marker motion and bone

motion, we need a substep, involving registration. We can use the
DRR-technique from chapter 5 (see also figure 5.3), but now we ap-
ply it to 4D CT images. As discussed in chapter 5, a DRR (digitally
reconstructed radiograph) is a projection image of a given CT image.
During treatment, we take stereo x-ray images, also called live images.
When a new live image is taken, we compare it to each of the DRRs.
(Recall that the DRRs now stem from 4D CT.) The comparison selects
the best matching DRR. We identify the CT scan from which this
DRR was generated. This allows for determining a time step in the
3D CT series. The given DRR also corresponds to a projection angle
and a translational shift. Thus, after the matching step, we know the
position, orientation and respiratory phase of the patient in the live
image.
7.4 Double Correlation 279

Exercises

Exercise 7.1

For the example in equation 7.1, show that the least squares solution
obtained by direct differentiation of equation 7.2 with respect to b and
m is the same as the solution obtained from the normal equation.

Exercise 7.2

Looking at only two sample points x1 and x2 , explain why the quad-
ratic program

Minimize
w21 + w22
subject to

y1 (wx1 + d) ≥ 0
y2 (wx2 + d) ≥ 0,
(7.30)

where y1 = 1 and y2 = −1, computes a maximum margin separator

line.

Hints:
Represent the line by its parameters (w, d), where w is the normal
vector of the line. Then this representation of a line can be multiplied
by a factor λ , without changing the line. Thus (λ w, λ d) for non-zero
λ is the same line as (w, d). First show that it suffices to consider only
those lines having

y1 (wx1 + d) = 1,
y2 (wx2 + d) = 1.
(7.31)
280 7 Motion Correlation and Tracking

Now recall that for any line, we obtain the point-line distance by scal-
ing the normal vector w to unit length, so that the margin is given
by:
w d
x1 + (7.32)
||w|| ||w||
1
Hence the margin is simply ||w|| . To maximize the margin, we must
minimize the length of w.

Exercise 7.3

The Matlab routine quadprog implements quadratic programming.

Hence we can write our own support vector machine by calling the
routine quadprog simply for the system in equation 7.11. This will ac-
tually work! (But do not be disappointed to see that this program will
be very slow, and will not be able to handle more than 20 data points.
There are other methods to implement support vector machines much
more efficiently). Similar to the Matlab routine for linear program-
ming, the quadprog-routine expects input parameters in matrix and
vector form. The parameters are: H is a symmetric matrix, A is a mat-
rix, and b and f are vectors. Then u = quad prog(H, f, A, b) minimizes
1 T T
2 u Hu + f u subject to Au ≤ b.

a) Write a program for the system in equation 7.11 with input points
as in figure 7.5, and visualize the resulting regression line. This
is an exceedingly simple support vector machine. Again, it will
not be a very effective support vector machine, but will serve to
illustrate the basics.
b) Show that the system in equation 7.16 is actually a quadratic pro-
gram. Thus, show that input parameters in the form H, f, A, b can
be set up for the system in equation 7.16. One seemingly simple
way to do this would be to set θi = αi − αi0 , and use the θi as vari-
ables, i.e. minimze 12 θ T Hθ + fT θ subject to Aθ ≤ b However, this
will fail, since we have the subexpression αi + αi0 in the objective
function. Hint: Use the 2m variables θ1 , . . . , θ2m by setting:
θi = αi − αi0 for i = 1, . . . , m
and
θi+m = αi0 for i = 1, . . . , m.
7.4 Double Correlation 281

Then αi = θi + αi0 , and H is a 2m by 2m matrix.

Run this as a program for the same points as in part a), to compute
and visualize the regression line with dual quadratic programming.
c) Run your program (now for more than just two points) with the
kernel function in equation 7.22, and show that non-linear regres-
sion functions can be found.

Exercise 7.4 Lagrange multipliers

Consider the optimization problem

Max f (x, y)

Subject to g(x, y) = c

Here, f and g are functions of two variables x and y. For example, f

and g could be the functions

f (x, y) = −x2 − y2 , (7.33)

g(x, y) = x + 2y. (7.34)

The function f can be visualized via its level curves (figure 7.21).
The level curves are obtained as curves where f (x, y) = c for various
constants c. Likewise, we can visualize the constraint g(x, y) = c as
a curve. Looking again at the above optimization problem, two cases
are possible: the curves f (x, y) = c and g(x, y) = c can cross, or they
can meet tangentially. The key observation for Lagrange multipliers is
the following: f can attain a maximum under the constraint g(x, y) = c
only if the level curves meet as tangents.
The gradient vector
 
∂f ∂f T
∇f = , (7.35)
∂x ∂y
is orthogonal to the level curve. Thus, if f and g meet tangentially,
their gradient vectors must be an α-multiple of each other for a non-
zero α. Therefore, we have
282 7 Motion Correlation and Tracking

Fig. 7.21: Level curves of the function f (x, y) = −x2 − y2

∇ f = α∇g. (7.36)
α is called Lagrange-multiplier. To summarize, we can state the so-
called Lagrange system for the above optimization problem in the fol-
lowing form:

∂f ∂g
=α
∂x ∂x
∂f ∂g
=α
∂y ∂y
g(x, y) = c

A more compact form is obtained in the following way. Here, the

whole system (now with three equations) can be stated in a single
equation.
Set
Λ (x, y, α) = f (x, y) − α(g(x, y) − c). (7.37)
Then all three above equations are obtained by setting derivatives of
Λ to 0. Specifically, the system
7.4 Double Correlation 283

∇(x,y,α)Λ (x, y, α) = 0 (7.38)

condenses the three equations above into a single one. Note that dif-
ferentiating Λ with respect to α, we obtain the original constraint
g(x, y) = 0. Hence,

∇α Λ (x, y, α) = 0 (7.39)
is equivalent to g(x, y) = 0.
(Here we have included the constant c into the definition of the func-
tion g, so that we have replaced the original constraint g(x, y) = c
by the constraint g(x, y) = 0). The compact version of this system,
namely the function Λ has a name. It is called the Lagrangian of the
given optimization problem, or also the Lagrange function.
Now consider the optimization problem:
Minimize

f (x, y) = x2 + y2
subject to

g(x, y) = x + y = 2.
a) Find the Lagrange function for this system
b) Find the optimum with the help of the Lagrange function
Notice that the theory of Lagrange multipliers extends to the case of
inequality constraints (see e.g. [3, 4]).

Exercise 7.5

The optimization problem

Max c1 x + c2 y

subject to

a11 x + a12 y = b1
a21 x + a22 y = b2 (7.40)
284 7 Motion Correlation and Tracking

is a linear program as defined in equations 6.6 and 6.7, since each of

the two equality constraints can be resolved to two inequality con-
straints.
The Lagrange technique for solving optimization problems can be ex-
tended to the case of several constraint functions gi in the following
way.
Given m constraints, in a problem of the form:

Maximize

f (x1 , . . . , xn )
subject to

g1 (x1 , . . . , xn ) = 0
..
.
gm (x1 , . . . , xn ) = 0, (7.41)
we must use m Lagrange multipliers α1 , . . . , αm .
Here, the tangent condition used above (gradient vectors of f and g
aligned) is not as obvious. In fact, for multiple constraints, the tangent
condition requires that the gradient vector of f is a linear combination
of the gradient vectors of the g-functions.
Specifically, in this case, the Lagrange function is

m
Λ (x1 , . . . , xn , α1 , . . . , αm ) = f (x1 , . . . , xn ) − ∑ αk gk (x1 , . . . , xn ).
k=1
(7.42)
At the optimum, the partial derivatives of the Lagrange function with
respect to the variables xi and αi must vanish.
a) Set up the Lagrange function Λ for the optimization problem in
equation 7.40. (You will need two Lagrange multipliers α1 , α2 ,
for the two constraints.) Take derivatives of Λ with respect to the
Lagrange multipliers αi and the primal variables x, y, and set the
resulting expressions to zero. Inserting these equations back into
Λ , show that Λ = bα, and AT α = c. Compare to the dual of a
linear program defined in exercise 6.2.
7.4 Double Correlation 285

b) Apply the strategy from part a) to the optimization problem:

Minimize
ww + ξ1 + ξ10 + ξ2 + ξ20 . . .
subject to
y1 − (wx1 + d) = ε + ξ1
(wx1 + d) − y1 = ε + ξ10
y2 − (wx2 + d) = ε + ξ2
(wx2 + d) − y2 = ε + ξ20
...,
taking derivatives with respect to the slack variables ξi , ξi0 as well.
This is a slight modification of the primal quadratic program for
regression in equation 7.15. Compare your result to the dual quad-
ratic program for regression in equation 7.16.

Exercise 7.6

Having acquired some background on mutual information and support

vector machines, we can now combine the two techniques. Hence,
for heterogeneous data streams (i.e. multiple infrared-markers, breath
temperature sensors, strain gauges, acceleration sensors in the in-
frared markers), discuss the process of searching for tight correlations
between heterogeneous data streams in the training phase, and then
improving reliability and accuracy by merging several tightly coupled
sensor data streams.

Summary

Targets in soft tissue move. Tracking such targets with registration

alone is difficult. By contrast, the skin surface can be tracked with real-
time imaging. Motion correlation can then provide an alternative to
registration. Typically, the correlation between the motion of the skin
surface and the motion of the target is computed prior to treatment.
Here, so-called surrogates can take the role of the skin surface.
286 7 Motion Correlation and Tracking

Hysteresis is a problem in this context. Inhale curves differ from ex-

hale curves. Thus we have two curves in our correlation, and this situ-
ation cannot be represented by a single function. We saw that support
vector methods can address this problem. Typically, support vector
regression does not compute an explicit regression function. Instead,
a dual quadratic program is solved to obtain parameters specifying
a plane in high-dimensional space. This plane determines a (not ne-
cessarily linear) mapping in low-dimensional space. Support vector
methods are a direct extension of linear programming. The first step
in this extension was the introduction of separating planes for clas-
sification. Such planes can also be found with linear programming
alone. But support vector methods allow for finding separating planes
with maximum margins. This leads to quadratic programming, and
with the dual of a quadratic program we can include kernel functions
to represent non-linear correlations.
The rib cage moves with respiration as well. Thus we can track
without implanted markers using DRRs and four-dimensional CT
scans.
We must validate the hypothesis that motions of organs are indeed cor-
related. Furthermore, we must measure the strength of the correlation.
To this end, it would be ideal to have all structures visible in a single
stream of images, so that we can ignore latencies in image acquisition.
Two-plane fluoroscopy is a useful tool in this case, but typically too
invasive for routine use. We can replace fluoroscopy by ultrasound,
where the ultrasound is calibrated with respect to acquisition latency.

Notes

Motion correlation for tumor tracking was introduced in [5], and is

now in routine clinical use in radiosurgery [6]. A variety of further
applications for motion correlation has since been investigated. Sur-
veys are given in [7] and [8], see also [9–15]. Validation methods for
motion correlation, i.e. based on ultrasound imaging are given in [16].
Organs do not only move, but also deform during respiration. The
methods in this chapter avoid direct image registration. However, sev-
eral authors describe methods for addressing the problem of respirat-
References 287

ory motion tracking with registration (see e.g. [17, 18]), or by com-
bining motion correlation with direct registration.
A library for support vectors is available from [2], see also [1, 19, 20].
Additional background on normal equations and least squares prob-
lems is given in [21]. The text book by C. Bishop [22] discusses learn-
ing methods and regression.

References

[1] J. C. Platt, Advances in kernel methods: Support Vector Learn-

ing. Cambridge, MA, USA: MIT Press, 1999, ch. Fast training
of support vector machines using sequential minimal optimiza-
tion, pp. 185–208.
[2] C.-C. Chang and C.-J. Lin, LIBSVM: a library for support vector
machines, 2001, software available at http://www.csie.ntu.edu.
tw/∼cjlin/libsvm.
[3] W. Karush, “Minima of functions of several variables with in-
equalities as side conditions,” Master’s thesis, Department of
Mathematics, University of Chicago, Chicago, IL, 1939.
[4] H. W. Kuhn and A. W. Tucker, “Nonlinear programming,” in
Proceedings of the Second Berkeley Symposium on Mathemat-
ical Statistics and Probability, J. Neyman, Ed., University of
California. University of California Press, Aug. 1951, pp. 481–
492.
[5] A. Schweikard, G. Glosser, M. Bodduluri, M. J. Murphy, and
J. R. Adler, Jr., “Robotic Motion Compensation for Respiratory
Movement during Radiosurgery,” Journal of Computer-Aided
Surgery, vol. 5, no. 4, pp. 263–277, 2000.
[6] È. Coste-Manière, D. Olender, W. Kilby, and R. A. Schulz,
“Robotic whole body stereotactic radiosurgery: clinical advant-
ages of the Cyberknife(R) integrated system,” The International
Journal of Medical Robotics and Computer Assisted Surgery,
vol. 1, no. 2, pp. 28–39, 2005.
[7] C. N. Riviere, J. Gangloff, and M. de Mathelin, “Robotic com-
pensation of biological motion to enhance surgical accuracy,”
288 7 Motion Correlation and Tracking

Proceedings of the IEEE, vol. 94, no. 9, pp. 1705–1716, Sept

2006.
[8] D. Verellen, M. de Ridder, N. Linthout, K. Tournel, G. So-
ete, and G. Storme, “Innovations in image-guided radiother-
apy,” Nature Reviews Cancer, vol. 7, no. 12, pp. 949–960, 2007,
10.1038/nrc2288.
[9] M. Baumhauer, M. Feuerstein, H.-P. Meinzer, and J. Rassweiler,
“Navigation in endoscopic soft tissue surgery: Perspectives and
limitations,” Journal of Endourology, vol. 22, no. 4, pp. 751–766,
2008.
[10] T. Depuydt, D. Verellen, O. C. L. Haas, T. Gevaert, N. Linthout,
M. Duchateau, K. Tournel, T. Reynders, K. Leysen, M. Hooge-
man, G. Storme, and M. de Ridder, “Geometric accuracy of a
novel gimbals based radiation therapy tumor tracking system,”
Radiotherapy and Oncology, vol. 98, no. 3, pp. 365 – 372, 2011.
[11] R. Ginhoux, J. Gangloff, M. de Mathelin, L. Soler, M. M. A.
Sanchez, and J. Marescaux, “Active filtering of physiological
motion in robotized surgery using predictive control,” IEEE
Transactions on Robotics, vol. 21, no. 1, pp. 67–79, 2005.
[12] P. J. Keall, G. S. Mageras, J. M. Balter, R. S. Emery, K. M. For-
ster, S. B. Jiang, J. M. Kapatoes, D. A. Low, M. J. Murphy, B. R.
Murray, C. R. Ramsey, M. B. V. Herk, S. S. Vedam, J. W. Wong,
and E. Yorke, “The management of respiratory motion in radi-
ation oncology. Report of AAPM task group 76,” Medical Phys-
ics, vol. 33, no. 10, pp. 3874–3900, 2006.
[13] A. Khamene, J. K. Warzelhan, S. Vogt, D. Elgort,
C. Chefd’Hotel, J. L. Duerk, J. Lewin, F. K. Wacker, and
F. Sauer, “Characterization of internal organ motion using
skin marker positions,” in MICCAI 2004, Part II, ser. LNCS,
C. Barillot, D. R. Haynor, and P. Hellier, Eds., vol. 3217,
MICCAI. St. Malo, France: Springer, Sep. 2004, pp. 526–533.
[14] T. Neicu, H. Shirato, Y. Seppenwoolde, and S. B. Jiang, “Syn-
chronized moving aperture radiation therapy (SMART): average
tumour trajectory for lung patients,” Physics in Medicine and
Biology, vol. 48, no. 5, pp. 587–598, 2003.
[15] J. Wilbert, M. Guckenberger, B. Polat, O. Sauer, M. Vogele,
M. Flentje, and R. A. Sweeney, “Semi-robotic 6 degree of free-
References 289

dom positioning for intracranial high precision radiotherapy; first

phantom and clinical results,” Radiation Oncology, vol. 5, p. 42,
2010.
[16] F. Ernst, R. Bruder, A. Schlaefer, and A. Schweikard, “Correl-
ation between external and internal respiratory motion: a valid-
ation study,” International Journal of Computer Assisted Radi-
ology and Surgery, vol. 7, no. 3, pp. 483–492, 2012.
[17] G. P. Penney, J. M. Blackall, M. S. Hamady, T. Sabharwal,
A. Adam, and D. J. Hawkes, “Registration of freehand 3D ul-
trasound and magnetic resonance liver images,” Medical Image
Analysis, vol. 8, no. 1, pp. 81 – 91, 2004.
[18] T. Rohlfing, C. R. Maurer, W. G. O’Dell, and J. Zhong, “Mod-
eling liver motion and deformation during the respiratory cycle
using intensity-based nonrigid registration of gated MR images,”
Medical Physics, vol. 31, no. 3, pp. 427–432, 2004.
[19] A. J. Smola and B. Schölkopf, “A tutorial on support vector re-
gression,” Statistics and Computing, vol. 14, pp. 199–222, 2004.
[20] B. E. Boser, I. M. Guyon, and V. N. Vapnik, “A training al-
gorithm for optimal margin classifiers,” in Proceedings of the
Fifth Annual Workshop on Computational Learning Theory, ser.
COLT ’92. New York, NY, USA: ACM, 1992, pp. 144–152.
[21] C. L. Lawson and R. J. Hanson, Solving Least Squares Prob-
lems, ser. Classics in Applied Mathematics. Englewood Cliffs,
NJ: Society for Industrial and Applied Mathematics, Jan. 1987,
vol. 15.
[22] C. M. Bishop, Pattern Recognition and Machine Learning, ser.
Information Science and Statistics. New York, Berlin, Heidel-
berg: Springer, 2006.
Chapter 8
Motion Prediction

In the previous chapter we discussed motion correlation. With motion

correlation, we compute the current position of a moving target. Then
we must move the robot so that it follows the target. The robot motion
will thus always lag behind the moving target. This time lag is illus-
trated in figure 8.1. In the figure, the x-axis represents time, while the
y-axis shows target position.

1 patient
robot

0.5
position

−0.5

−1
0 1 2 3 4 5 6
t
time [sec]

Fig. 8.1: Signal Prediction

The figure shows an ideal situation, with a perfect sine wave. At time
point t shown in the figure, the position of the target and the robot
position are known. Time is discretized, i.e. at fixed time intervals (i.e.
every 50 milliseconds) we must send a motion command to the robot.
By the time we have computed the target position and the robot has
reached its new position, the target will have moved. This time lag

291
292 8 Motion Prediction

(computing time, robot motion time, etc.) is typically constant (e.g.

100 ms).
The idea of motion prediction is the following. We send a motion
command to the robot. The command tells the robot not to move to
the current position of the target, but to the position where the target
will be in 100 ms. Based on the past history of the motion curve, we
can determine this future position. Thus it is our goal to extrapolate the
patient’s motion curve by a fixed amount δt . δt is called the prediction
horizon (figure 8.2).

1 + patient
o
robot

0.5
position

−0.5

−1
0 1 2 3 4 5 6
t + δt
time [sec]

Fig. 8.2: Prediction horizon

With optical tracking, we can record a single coordinate (i.e. the z-

coordinate) of a respiratory motion curve, and obtain a graphical rep-
resentation as in figure 8.3. The x-axis shows the time in seconds, the
y-axis represents target motion along the z-coordinate in space. Ob-
serve that the signal is not strictly periodic.
Figure 8.4 shows a motion trace of the ostium of a pulmonary vein
close to the human heart. Both cardiac motion and respiratory motion
are clearly visible. The motion trace was obtained with a volumetric
ultrasound setup.
In principle, we could apply regression methods to the problem of
prediction. Thus, for example support vector regression computes a
value y for an unknown sample point x, given training data pairs
(x1 , y1 ), ..., (xm , ym ). However, we will regard motion correlation and
motion prediction as separate problems. There are several reasons for
this: in prediction, we have time points with equidistant spacing, and
8 Motion Prediction 293

124

122

120

position
118

116

114

0 20 40 60 80 100
time [sec]

Fig. 8.3: Respiratory signal (measured with infrared tracking)

120

110

100

90
position

80

70

60

50
40 45 50 55 60 65
time [sec]

Fig. 8.4: Cardiac motion and respiratory motion, measured with ultra-
sound. Typically, the frequency of cardiac motion is four times larger
than the frequency of the respiratory signal alone. The amplitude of res-
piratory motion is larger than the amplitude of cardiac motion at this
target location.

we have a fixed prediction horizon. Furthermore, in prediction, the

last known time point is more relevant for the computation than points
further in the past. Finally, we need a prediction result within a robot
command cycle, thus within a few milliseconds. Standard methods
for correlation often require substantial computing time. While sup-
port vectors provide a valid method for prediction, we will need so-
called warm-start methods, capable of both learning and un-learning
in real-time. We will find that other, more dedicated methods are more
suitable.
294 8 Motion Prediction

8.1 The MULIN Algorithm

MULIN is an algorithm for predicting respiratory motion. The basic

idea is very simple. Breathing is reasonably regular in typical cases,
so that we use the following method. Simply add the (old) difference
between the patient curve and robot curve to the patient curve, to get
the next value (see also figure 8.2). We now record the error, and try to
predict it. We can then obtain a second-order strategy, in the same way.
Stated in simpler terms we do the following: Time steps are discrete,
so we will call the individual steps of the patient curve patient(n), or,
for short p(n) (see figure 8.2). Then p is a function of the time points
0, . . . , m. To be more specific, let p(n) denote one coordinate of the
tumor position, i.e. the z-coordinate.
Likewise, we denote by r(n) the robot position at time step n. We have
the past history until time point n. Now we must predict the value
p(n + 1).
We can simply set

p(n + 1) = p(n) + [p(n) − r(n)], (8.1)

and obtain what we call the MULIN algorithm of order 0 (which is
equivalent to linear extrapolation). Equation 8.1 amounts to adding
the error made in the last step, just assuming the error will remain
constant. But of course it will not. We now plot the error curve with the
other curves. The dash-dotted curve shows the error, i.e. the difference
between the patient curve and the robot curve (see figure 8.5).
Thus the error curve e(n) (e for error) is given by

e(n) = p(n) − r(n). (8.2)

The key to the MULIN algorithm is now the following: The error
curve is also periodic, but has a smaller amplitude. So it should be
easier to predict! Thus we delay the error curve by one time step,
calculate the difference between the error curve and the delayed error
curve, and add that to the last known value of the error curve. The
result will change the above error term [p(n) − r(n)]. To delay the
error curve, we simply use older values. I.e. if e(n) is the value of the
error curve at time point n, then e(n − 1) is the value of the delayed
curve.
8.1 The MULIN Algorithm 295
1 patient
robot
error
0.5
position

−0.5

−1
0 1 2 3 4 5 6
time [sec]

Fig. 8.5: Prediction error (dotted curve)

To predict the error curve, we must compute a value for e(n + 1) =

p(n + 1) − r(n + 1) from the past history, i.e. from p(n), r(n) etc.
In the same way as above, we can estimate

e(n + 1) = e(n) + [e(n) − e(n − 1)]. (8.3)

This is the same as

e(n + 1) = p(n) − r(n) + [p(n) − r(n) − (p(n − 1) − r(n − 1))]. (8.4)

The right hand side can be rewritten to

2[p(n) − r(n)] − [p(n − 1) − r(n − 1)]. (8.5)

Overall, we obtain a MULIN-algorithm of order 1 by setting

p(n + 1) = p(n) + 2[p(n) − r(n)] − [p(n − 1) − r(n − 1)]. (8.6)

We can now repeat the same construction to obtain a MULIN al-

gorithm of order 2:
Define a function e∗ by setting

e∗ (n) = 2[p(n) − r(n)] − [p(n − 1) − r(n − 1)] = 2e(n) − e(n − 1).

(8.7)
296 8 Motion Prediction

To predict the function e∗ , we set as above

e∗ (n + 1) = e∗ (n) + e∗ (n) − e∗ (n − 1). (8.8)

We expand this, and obtain

e∗ (n + 1) = 2e(n) − e(n − 1) +
[2e(n) − e(n − 1) − 2e(n − 1) + e(n − 2)],

thus
e∗ (n + 1) = 4e(n) − 4e(n − 1) + e(n − 2). (8.9)
Overall, the MULIN algorithm of order 2 is now given by

p(n + 1) =
p(n) + 4[p(n) − r(n)] −
4[p(n − 1) − r(n − 1)] +
[p(n − 2) − r(n − 2)]. (8.10)

We see that MULIN of higher order looks more deeply into the past
of the signal, as it uses the older values p(n − 1), p(n − 2).
MULIN can be generalized to order k (see exercise 8.1 at the end of
this chapter).
When deriving MULIN, we have always assumed that the value of p
for the immediate next time step p(n + 1) must be predicted. Thus our
prediction horizon was a single time step. However, it may also be
necessary to predict several time steps into the future.
To look further into the signal’s future, we would predict values

p(n + s), p(n + s − 1), . . . , p(n + 1) (8.11)

from

p(n), p(n − 1), p(n − 2), p(n − 3), . . . . (8.12)

Likewise, we could look further into the past by using the values

p(n), p(n − s), p(n − 2s), p(n − 3s), . . . (8.13)

8.1 The MULIN Algorithm 297

instead of p(n), p(n − 1), p(n − 2), p(n − 3), . . . .

In this case, the parameter s controls how far we look into the past.
Finally, to avoid sudden changes in the values computed by MULIN,
we introduce a smoothing parameter µ. Thus, to compute

p(n + s) (8.14)
we use the same method as above, but we also compute p∗ (n + s − 1)
and then set

p∗ (n + s) = µ p(n + s) + (1 − µ)p∗ (n + s − 1). (8.15)

Fig. 8.6: MULIN predictor for a motion signal. Thick solid line: patient
position. Dashed line: robot position, dotted line: predictor output for
MULIN order two.

Figure 8.6 shows an example for MULIN of order two. The dotted
line shows the predictor output. Notice that overshooting occurs at the
maxima and minima.
298 8 Motion Prediction

8.2 Least Means Square Prediction

Classical methods from signal processing can also be applied to the

problem of signal prediction. A number of methods has been de-
veloped since the 1960s for this problem. Amongst those methods are
LMS-prediction [1], as well as Fourier decomposition, Kalman filter-
ing and newer versions of the LMS prediction, such as nLMS and RLS
(see [2] for a survey). Experimentally, none of these methods gives
good results for the problem of anatomical motion prediction, since
respiration is far more irregular than typical electronic signals. How-
ever, such methods provide a basis for the development of more ded-
icated methods. We will next describe the LMS method, and extend
this method to a wavelet-method which we will call wLMS. Wavelet
decomposition can be regarded as an extension and generalization of
Fourier decomposition, so that the wLMS method combines several
of the classical approaches. Finally, we will again return to support
vectors, and apply them to the wavelet methods.
In its most simple form, LMS prediction is defined as follows:

en = yn − yLMS
n (8.16)
yLMS
n+1 = w y
n n (8.17)
wn+1 = wn + µen yn (8.18)

Here we used the following naming conventions: en is the error in the

last known time point, i.e. the difference between the observed res-
piration curve yn and predicted value yLMS
n , which had been predicted
for the last step. The main idea of LMS is to use a weight value wn
which is computed at each step. To get to a working method, the val-
ues yLMS
0 and w0 must be initialized. We set yLMS
0 = y0 and w0 = 1.
The parameter µ controls the step-length.
To understand the definition of the LMS predictor we consider the
square of the error en .
Thus, set
 2
En+1 = yn+1 − yLMS
n+1 . (8.19)
But by definition,
8.2 Least Means Square Prediction 299

En+1 = (yn+1 − wn yn )2 . (8.20)

We take the derivative of the squared error function E thus defined,
with respect to w, and obtain
dE
= 2 (yn+1 − wn yn ) (−yn ) (8.21)
dw
thus
dE  
= 2 yn+1 − yLMS
n+1 (−yn ) = −2en+1 yn . (8.22)
dw
dE
Now, to decrease the error, we subtract dw from wn to obtain the new
estimate for wn+1 . This gives

wn+1 = wn + µen+1 yn . (8.23)

Since we do not yet have a value for en+1 , we will simply use en in-
stead, which gives the formula in the definition of the LMS predictor.

error

w1 w2 w
Fig. 8.7: Gradient descent

Notice that LMS performs a gradient descent by taking the derivative

of the squared error function. Figure 8.7 illustrates the computation.
We compute the local value of the derivative of the (squared) error
function. If this gradient is positive (value w1 in the figure), then we
know the error will increase if we increase the weight value w, i.e.
step to the right of w1 . Likewise, if the gradient is negative (value w2 ),
then our error will decrease, if we increase the value for w. Thus, in
both cases, we know in which direction to change our value of w. But
we do not know by which amount to change this value. Assume, for
300 8 Motion Prediction

example, we have the value w2 (again compare the figure), so we must

step to the right. If the step-length is too big, we will over-step, and
actually increase the error. As noted above, the parameter µ controls
the step-length of the gradient descent. In practice, it often turns out to
be difficult to find a good value for µ. The necessity to choose such a
parameter is one of the disadvantages of the LMS algorithm. The fig-
ure also shows that the gradient descent performed by LMS will only
compute a local minimum for the error function. Notice the difference
to the least squares methods used in previous chapters, which actually
compute global minima.
One further disadvantage of LMS is the necessity to estimate en+1
(we did this by replacing en+1 by en ). This resembles the assumption
underlying MULIN, namely the assumption that the error will remain
constant.
A vector-based version of LMS allows for increasing the depth of the
past history M we look at.
We set

un = (un,1 , . . . , un,M )T = (yn−M+1 , . . . , yn )T , (8.24)

and

yLMS
1 = · · · = yLMS
M = y0 (8.25)
with

w0 = (0, . . . , 0, 1)T . (8.26)

Similar to the one-dimensional LMS value, set
 
wn+1 = wn + µ yn − yLMS
n un , n ≥ M, (8.27)
and
yLMS T
n+1 = wn un . (8.28)
A further variant of LMS is nLMS, or the normalized LMS method.
It is defined as follows:
8.3 Wavelet-based LMS Prediction 301

en = yn − ynLMS
n (8.29)
nLMS
yn+1 = wn yn (8.30)
en
wn+1 = wn + µ (8.31)
yn
If you compare the definitions in equation 8.18 and equation 8.31, you
will notice that a multiplication is simply replaced by a division. How
can this be right? It may seem astonishing that we can simply replace
this, and still obtain a valid definition. And how can this division be a
normalization?
The riddle is solved if we look again at the multi-dimensional version
of LMS: Here, a multi-dimensional version is obtained by replacing
the definition in equation 8.27 by


µ yn − ynLMS
n un
wn+1 = wn + 2
, n ≥ M. (8.32)
kun k2
Thus, we simply divided by the square of the magnitude of our in-
put signal, and this was done to normalize! After normalization, it
becomes easier to choose the value µ, especially in cases where
the magnitude of our input changes rapidly. Of course, in the one-
dimensional-case, the division will cancel out, so that we obtain a
derivation for the one-dimensional nLMS method.
Note that it is necessary to add a constant α (typically 10−4 or smaller)
to the denominators of equations 8.31 and 8.32 to avoid division by
zero.
The next section will deal with a principle similar to LMS, applied to
the bands of a wavelet decomposition.

8.3 Wavelet-based LMS Prediction

Wavelet decomposition resembles Fourier transformation, where a

given periodic function is written as a sum of sine and cosine waves.
However, Fourier transformation requires a periodic input function.
Our respiratory functions are not strictly periodic. It is true that the
shape of one respiratory cycle resembles the shape of the other cycles.
302 8 Motion Prediction

But the resemblance of the cycles can be quite faint, even if the patient
breathes normally.
To motivate the use of wavelets in our application, we again look at
the example in figure 8.6. Here we applied the MULIN predictor to
an aperiodic, but nonetheless very smooth signal. MULIN gives good
results in this case. However, if the signal is not smooth, problems will
arise. Often, anatomical motion data are far from being smooth (fig-
ure 8.8). If a sudden change in direction occurs near a peak point along
the curve (see arrow in figure 8.8), then the predictor will assume that
the peak has been reached, and initiate a downwards motion, thereby
causing errors.

626

625.5
position

625

624.5

20 25 30 35 40 45 50 55 60 65 70
time [sec]

Fig. 8.8: Respiratory motion curve recorded with an infrared tracking

system. Maxima and minima along the curve are difficult to predict, due
to noise and irregular breathing pattern.

Thus, it is reasonable to apply averaging or smoothing before predic-

tion. However, averaging is difficult in our case. To understand the dif-
ficulty here, assume we average over the past k time points during pre-
diction. This would give equal weight to the past k data points includ-
ing our current point. But the most recent data point is the most crit-
ical one, i.e. it carries the most information. Thus we lose a substantial
amount of information by averaging. Wavelet methods provide a more
systematic way for smoothing data. As an important feature, smooth-
ing can be applied without loss of information. Specifically, we need a
method that filters the data into distinct frequency bands, such that the
sum of these bands exactly represents the original signal. Instead of
filtering out high-frequency noise altogether, we apply prediction to
8.3 Wavelet-based LMS Prediction 303

each frequency band separately. Bands which become known to have

carried noise in the past can then receive lower weights in the course
of prediction. We can then apply machine learning to the individual
bands in the wavelet decomposition.
Consider the definitions

c0,n = yn (8.33)

1

c j+1,n = c j,n−2 j + c j,n , j≥0 (8.34)
2

W j+1,n = c j,n − c j+1,n , j ≥ 0. (8.35)

Here yn is again our signal. To better understand this definition, first
notice that equation 8.34 simply computes an average between two
consecutive values of our original signal.
We now select a positive integer J which will determine the resolution
depth up to which we compute the decomposition, i.e. j = 0, . . . , J.
We next illustrate the computation of the c-values. These values are
written into a table.
As described in the definition above, we begin by setting j = 0, and

c0,n = yn , n = 0, . . . , m. (8.36)
This will give the first line of the c-table. I.e. assume m = 8, so we
have computed the first line, giving

c0,0 c0,1 c0,2 c0,3 c0,4 c0,5 c0,6 c0,7 c0,8 .

Having done that, we can compute

1
c1,1 = (c0,0 + c0,1 ). (8.37)
2
Next we can compute c1,2 from c0,1 and c0,2 . Both latter values are
already in the first line of the table:
1
c1,2 = (c0,1 + c0,2 ) (8.38)
2
304 8 Motion Prediction

Likewise we now compute c1,3 , c1,4 , . . . c1,8 from the values in the first
line of the table. Our table will look like this:

c0,0 c0,1 c0,2 c0,3 c0,4 c0,5 c0,6 c0,7 c0,8

c1,1 c1,2 c1,3 c1,4 c1,5 c1,6 c1,7 c1,8

We now compute
1
c2,3 = (c1,1 + c1,3 ). (8.39)
2
Likewise we obtain c2,4 , . . . , c2,8 . Finally, we obtain the full c-table
(for J = 3):

c0,0 c0,1 c0,2 c0,3 c0,4 c0,5 c0,6 c0,7 c0,8

c1,1 c1,2 c1,3 c1,4 c1,5 c1,6 c1,7 c1,8
c2,3 c2,4 c2,5 c2,6 c2,7 c2,8
c3,7 c3,8

Notice that our c-table is not rectangular, i.e. table entries c1,0 , c2,0 ,
c2,1 , c2,2 , c3,0 etc. do not exist.
Remember, we set

W j+1,n = c j,n − c j+1,n (8.40)

for j = 0, . . . , J.
The computation of the W -values proceeds in much the same way.
We obtain the table

W1,1 W1,2 W1,3 W1,4 W1,5 W1,6 W1,7 W1,8

W2,3 W2,4 W2,5 W2,6 W2,7 W2,8
W3,7 W3,8 .

We remain in the above example. For n = 8 and J = 2 we consider

W1,8 and W2,8 .
Then we have
8.3 Wavelet-based LMS Prediction 305

W1,8 +W2,8 + c2,8 = c0,8 − c1,8 + c1,8 − c2,8 + c2,8 (8.41)

= c0,8 (8.42)
= y8 . (8.43)

And more generally,

yn = W1,n + · · · +WJ,n + cJ,n . (8.44)

We can now illustrate the role of the parameter J. This parameter de-
termines the depth of the decomposition of our signal y. Hence, for
J = 1, we obtain the results shown in table 8.1. In the last line of
table 8.1, we see the original signal yi . Notice that, as a result of equa-
tion 8.44, each column in tables 8.1 and 8.2 sums up to one of the
signal values yi .

Table 8.1: Wavelet decomposition with two bands

W1,1 W1,2 W1,3 . . . W1,n

c1,1 c1,2 c1,3 . . . c1,n
y1 y2 y3 . . . yn

Each line in the table is called a band. For J = 2, we obtain three bands
(table 8.2).

Table 8.2: Wavelet decomposition with three bands

W1,3 W1,4 W1,5 . . . W1,n

W2,3 W2,4 W2,5 . . . W2,n
c2,3 c2,4 c2,5 . . . c2,n
y3 y4 y5 . . . yn

Overall, vertical summation of the bands gives the original signal.

Thus, the tables indeed yield a decomposition of the original signal
yi .
The decomposition is graphically illustrated in figure 8.9. An in-
put signal (shown in the top row) is decomposed into four bands
306 8 Motion Prediction
original signal
6

0
2.5 3 3.5 4 4.5 5
Decomposition (W1, W2, W3, c)

6
W1
3

0
2.5 3 3.5 4 4.5

6
W2
position

0
2.5 3 3.5 4 4.5

6
W3
3

0
2.5 3 3.5 4 4.5

6
c
3

0
2.5 3 3.5 4 4.5
time [sec]

Fig. 8.9: Wavelet decomposition for an input signal (with J = 3). Top
row: input signal. Bottom rows: Wavelet bands W1 , W2 , W3 and c .

W1 ,W2 ,W3 and c (four bottom rows). Notice that the input signal is
noisy when compared to the c-band in the decomposition. Thus, a
standard application of wavelet decomposition is noise removal in sig-
nal processing. Definitions 8.33 to 8.35 are just one way of defining
a wavelet decomposition. There are many other ways. The specific
decomposition introduced above is called the à trous wavelet decom-
position. Other types of wavelets are discussed in the exercises at the
end of this chapter, and this will also explain why we chose the à trous
wavelets for our application.
Equation 8.45 is the key to the predictor, to be defined next. The signal
is first decomposed into several bands. Then we predict the next values
for the individual bands separately.
We define a predictor in the following way:
8.3 Wavelet-based LMS Prediction 307
J
yn+1 = wJ+1 cJ,n + vJ+1 cJ,n−2J + ∑ w jW j,n + v jW j,n−2 j (8.45)
j=1

Here the values w1 , . . . , wJ , wJ+1 and v1 , . . . , vJ , vJ+1 are variables!

Hence, these values are unknown, and we must estimate them in a
learning step. To this end we will simplify the notation.
We set
 
wj
= w̃ j , j = 1, . . . , J,
vj
 
wJ+1
= w̃J+1 ,
vJ+1

(8.46)
and

(W j,n ,W j,n−2 j ) = W̃ j,n , (8.47)

for j = 1, ..., J and

(cJ,n , cJ,n−2J ) = c̃J,n . (8.48)

Then we rewrite our above predictor in the following way:
J
yn+1 = c̃J,n w̃J+1 + ∑ W̃ j,n w̃ j (8.49)
j=1
To estimate the values for our variables w1 , . . . , wJ , wJ+1 and v1 ,...,vJ ,
vJ+1 we set up a matrix B, by defining:

 
W̃1,n−1 W̃2,n−1 . . . W̃J−1,n−1 W̃J,n−1 c̃J,n−1
 .. .. .. .. .. .. 
B= . . . . . .  (8.50)
W̃1,n−M W̃2,n−M . . . W̃J−1,n−M W̃J,n−M c̃J,n−M

Note that the entries of the matrix B are themselves vectors, namely
1-by-2 vectors. But this does not change the fact that the following
equation is simply a linear system of equations!
308 8 Motion Prediction

   
w̃1 yn
B  ...  =  ... 
   
(8.51)
w̃J+1 yn−M+1
This system has the 2J + 2 variables w1 , . . . , wJ+1 and v1 , . . . , vJ+1 and
a number M of equations. If M is larger than 2J + 2, then the system
is over-determined.
Equation 8.51 is the training equation for w1 , . . . , wJ+1 and v1 , . . . , vJ+1 .
Each matrix line in B corresponds to one training sample, and we thus
have M training samples. The training samples stem from the signal
history y0 , y1 , . . . , yn . As we go down the matrix lines, we step back-
wards in the signal history by one sample for each matrix line.
It remains to discuss how the system in equation 8.51 should be
solved. Naturally, one could use Gaussian elimination. However, over-
determined systems of this type can be solved with advantage by nor-
mal equations, so here again the method from the previous chapter can
be used. This will fit a regression line in the least-mean-square sense
to the data points represented by the matrix lines. Obviously, we can
also use support vectors for this regression. There are several ways to
do this, and we will discuss this possibility below.
But before we do this, we will look at two issues related to the im-
plementation and practical improvements of the wLMS algorithm.
Remember that the original version of the LMS algorithm (without
wavelets) used a smoothing parameter µ. Indeed we have used such a
parameter for MULIN as well.
To emphasize the fact that we had set up the B-matrix from the de-
composition data with the indices n − 1, . . . , n − M, we will add these
indices to the notation. Hence, write

B(n−1,...,n−M) (8.52)
for the above B-matrix in equation 8.51.
Likewise we extend the notation for the weight-vector, giving
 (n−1,...,n−M)  
w̃1 yn
B(n−1,...,n−M)  ...  =  ...  .
   
(8.53)
w̃J+1 yn−M+1
8.3 Wavelet-based LMS Prediction 309

Using the same notation, we can rewrite the predictor equation:

 (n−1,...,n−M)
w̃1
B(n)  ... 
 
= yn+1 (8.54)
w̃J+1
Now to apply the smoothing parameter µ we will use the old weight
vector (computed in the preceding step) and the weight vector com-
puted from equation 8.53. Hence, we set

 (new)
w̃1
 .. 
 .  =
w̃J+1
 (n−2,...,n−M−1)  (n−1,...,n−M)
w̃1 w̃1
(1 − µ)  ...  + µ  ... 
   
.
w̃J+1 w̃J+1

And we replace equation 8.54 by

 (new)
w̃1
B(n)  ... 
 
= yn+1 . (8.55)
w̃J+1

8.3.1 Variable scale lengths

In the above description of the wavelet predictor, we have used a

minor simplification. This was done to reduce the amount of sub-
scripts in the notation. We return to equations 8.45 and 8.46. In equa-
tion 8.45, yn+1 was obtained from the decomposition data with weight
variables w1 , . . . , wJ , wJ+1 and v1 , . . . , vJ , vJ+1 by
J
yn+1 = wJ+1 cJ,n + vJ+1 cJ,n−2J + ∑ w jW j,n + v jW j,n−2 j .
j=1
310 8 Motion Prediction

Instead of this latter definition, with just two weight variables w and
v per value of j, we use a series of such variables (for each j), now
called
(0) (a )
wj ,...,wj j .
Thus we obtain

(0) (1)
yn+1 = wJ+1 cJ,n + wJ+1 cJ,n−2J ·1 + ... +
(a
J+1 −1)
wJ+1 cJ,n−2J ·(aJ+1 −1) +
J
∑ wJ+1W j,n−2 j ·0 + w j
(0) (1)
W j,n−2 j ·1 + ... +
j=1
(a j −1)
wj W j,n−2 j ·(a j −1) . (8.56)

The necessary extensions to equation 8.46 are straightforward, i.e. we

now have
 
(0)
wj
 . 
 ..  = w̃ j , j = 1, . . . , J
 
(a j −1)
wj
 (0) 
wJ+1
 .. 
 .  = w̃J+1
(aJ+1 −1)
wJ+1
instead of equation 8.46.

8.4 Support Vectors for Prediction

SVR methods, as described in the previous chapter, can also be ap-

plied to respiration prediction [3]. To obtain a model for inhale-
exhale motion, we can proceed as follows: Training samples are pairs
(t1 , y(t1 )), . . . , (tm , y(tm )), where y(ti ) denotes the position of some
target point at time ti . During production, we compute the value
8.4 Support Vectors for Prediction 311

y(tm + δt ). However, our signal is non-stationary. The depth of inhala-

tion varies over time. Types of curves thus obtained are individual, and
may vary considerably between patients. In particular, resting phases
of varying lengths between respiratory cycles occur. The slope of in-
halation and exhalation is different and time-varying in many cases.
The following approach addresses these problems.
Instead of samples of the form (ti , y(ti )) we use a series of values

y(t1 ), y(t2 ), . . . , y(ts ), y(ts+k )

as a single training sample.
Notice that the entire series y(t1 ), y(t2 ), . . . , y(ts ), y(ts+k ) is one single
sample, whereas y(t1 ), y(t2 ), . . . , y(ts ) is our production sample. Here
we require that the time points t1 ,t2 , . . . ,ts ,ts+k have equidistant spa-
cing. The parameter k controls the prediction horizon. As noted above,
there are other ways to use support vectors for our application, one of
which combines wavelets and support vectors. The next section pre-
pares this alternative.

8.4.1 Online Regression and AOSVR

Respiration prediction is time-critical, and we must keep computing

time to a minimum. Online support vector algorithms have been de-
veloped [4] to shorten computing time.
In linear programming, so-called warm-start methods have been de-
veloped. Such methods incrementally compute a solution of the input
LP problem for a given data set. That is, given a solution for the first
m inequality constraints, one can quickly update this solution when a
single new constraint is added. This can be done without starting the
entire computation from scratch. This is often of advantage since, in
many cases, the new constraint does not even restrict the feasible re-
gion any further, and this condition can be tested readily. But even if it
does further restrict the feasible region, then an updated solution can
be obtained very quickly from the solution for the original problem.
Such warm-start methods have been extended to support vector ma-
chines, called AOSVR. In this case, it is necessary to be able to learn
312 8 Motion Prediction

the training data after a new sample has been added. It may also be
necessary to “un-learn” single samples.

8.4.2 Combining Support Vectors and Wavelets

In chapter 7, we investigated correlation methods. We used a very

simple strategy based on normal equations to find a regression line
amidst data points in the plane. In this case we had input values xi and
desired output values yi . Very much the same situation occurred in
the estimation of our weight values for wavelet-based prediction (see
equation 8.51). This equation computes a linear least mean square fit
to the data. Obviously, it is possible to apply support vector methods
to the training data in equation 8.51. Each line of this matrix equa-
tion corresponds to one training sample, giving a total of M training
samples.
Specifically, a sample of the form

(W̃1,n−1 , . . . , W̃J,n−1 , c̃J,n−1 , yn ) (8.57)

is one single training sample for this SVR.
To obtain a full set of samples, we again use the matrix lines in equa-
tion 8.51, giving:

(W̃1,n−1 , . . . , W̃J,n−1 , c̃J,n−1 , yn )

..
.
(W̃1,n−M , . . . , W̃J,n−M , c̃J,n−M , yn−M+1 )

Then the production sample has the form

(W̃1,n , . . . , W̃J,n , c̃J,n ). (8.58)

We thus obtain a combination of wavelet methods and support vector
methods, which will be called wSVR.
8.5 Fast-Lane Methods and Performance Measures 313

8.5 Fast-Lane Methods and Performance Measures

Motion prediction is about forecasting the future. It is well-known that

this can be arbitrarily difficult. If the given signal data are sufficiently
evil, then prediction will fail. Luckily, respiration and cardiac motion
data are mostly regular, so that prediction algorithms provide a sub-
stantial gain of accuracy. Today, motion prediction is used in routine
clinical practice in robotic radiosurgery. However, no single one of the
above methods works best for all patients. Rather, wLMS is best in
nearly all cases. But in some cases, SVR methods provide improve-
ments which cannot be achieved with wLMS. Finally, MULIN is a
simple and robust method, and sometimes gives the best results. This
gives rise to the following idea: the patient data are measured for a
while before treatment, and data are recorded. We can run all known
algorithms in parallel, and then decide which algorithm to use for this
patient. But then we can also keep all other methods running during
treatment, and switch whenever one of the other methods, currently
not used, improves over the current method. This approach will be
called the fast-lane method [5]. To implement such an approach, we
will need methods for measuring performance.
A simple measure is the RMS error between the signal and the pre-
dicted curve:
s
1 N 2
RMS(y, yPRED ) = ∑
N i=1
yi − yPRED
i (8.59)

The RMS error thus gives an average over the squared distances
between the signal itself and the value returned by the predictor. N
is the total number of time steps.
This simple measure provides an absolute error, but does not allow for
much comparison. Thus, normalization is needed. In order to normal-
ize the RMS error, we divide by the error which would have occurred
if we had not applied any prediction at all. Stated in simpler terms,
we compute the RMS-distance between the patient curve and the pre-
dicted curve (figure 8.6), then the RMS-distance between the patient
curve and the delayed curve, then divide the two values. This yields
the relative RMS error. With yδ denoting the delayed curve:
314 8 Motion Prediction
s
1 N 2
RMS(y, yδ ) = ∑ yi − yδ ,i
N i=1
(8.60)

Now dividing gives the relative RMS error:

RMS(y, yPRED )
RMSrel = (8.61)
RMS(y, yδ )
In this way, the improvement obtained by prediction can be expressed
as a percentage of reduction in terms of RMS error. Thus, if success-
ful, prediction reduces RMS error by a certain percentage.
Relative RMS error is not the only measure for evaluating motion
prediction for robots. Maximum absolute error can be an important
measure as well. For technical reasons, it is often useful to limit the
so-called jitter of the predictor output signal. The jitter measures dis-
tances between two consecutive (discrete) points along the signal:

1 1 N PRED
J(y PRED
)= ∑ yi − yPRED
N ∆t i=0 i+1 (8.62)

1
Here, the factor ∆t normalizes with respect to the spacing between the
samples. Thus the jitter value J measures the average distance (per
second) traveled by the robot following the predictor output.
Notice that the measures just described may be in conflict with one
another. Respiration patterns vary considerably among patients. For
patients with large lung tumors, the patterns can become very irregu-
lar. Unfortunately, for lung tumors, the respiratory motion excursion
is particularly large. As noted above, no single algorithm works best
for all patients. For further empirical evaluation, and for the imple-
mentation of a fast-lane method, histograms are very useful. Given a
data base of test cases, a histogram plots the number of data base cases
(x-axis) versus the percentage gains (relative RMS) obtained with the
individual algorithm (y-axis). An example of such a histogram for a
number of predictors is shown in figure 8.10. The figure shows both,
RMS error and jitter. Here six algorithms are shown. The RLS method
is a variant of the LMS method, whereas EKF denotes the extended
Kalman filter [2] [6], applied to motion prediction.
8.5 Fast-Lane Methods and Performance Measures 315
1

0.9 0.8487

0.7768
0.8
0.7796
0.7

0.6
content

0.5789
0.5
0.4704
0.4
0.3783
0.3

0.2

0.1

0
0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 1.1 1.2 1.3 1.4 1.5 1.6 1.7 1.8
relative RMS
1
MULIN
0.9 nLMS
RLS
0.8 wLMS
EKF
0.7
SVRpred
0.6
content

0.5

0.4

0.3

0.2

0.1

0
0 0.5 1 1.5 2 2.5 3 3.5 4
relative jitter

Fig. 8.10: Histograms for evaluating predictor output

Table 8.3 states results of relative RMS errors for a large database of
respiratory traces from patient treatments, containing data from 304
full treatment sessions [7].

Table 8.3: Results of relative RMS errors obtained with different pre-
diction methods

wLMS: RMSrel 56.99 %

MULIN: RMSrel 64.36 %
SVRpred: RMSrel 68.32 %
LMS: RMSrel 76.32 %

It turns out that wLMS gives the best results in terms of RMS for over
60 % of the data base cases. MULIN gives best results on 28 % of
316 8 Motion Prediction

the cases. However, for a small number of cases, SVR is capable of

removing over 80 % of the residual error. This is not possible for the
other methods. For SVR and wSVR, the computing time is longer than
for the other methods, but jitter can be reduced with SVR and wSVR
in comparison to other methods. It turns out that wSVR can still im-
prove substantially over wLMS in many cases. However, wSVR has
the largest number of parameters. Thus it can be difficult to find the
optimal set of parameters for wSVR.
We have not discussed the choice of parameters for the algorithms. It
is clear that most algorithms will need a good choice of parameters to
be practical. But how to choose the parameters? The fast-lane scheme
sketched above provides a good background for this discussion. Be-
cause not only can we switch between algorithms, but we can also
switch between different “versions” of one single algorithm, where we
will obtain different versions by supplying the same algorithm with
different parameters.
Grid search is a very straight-forward approach for evaluating para-
meters. We place a fine grid over each axis of parameter space. We
then evaluate the algorithm for each point in parameter space thus ob-
tained. An example is shown in figure 8.11 for the case of wLMS.

Fig. 8.11: Grid search in parameter space for the wLMS algorithm
8.5 Fast-Lane Methods and Performance Measures 317

Clearly, grid search is often not feasible in practice. Grid search is

limited to situations where a large amount of data is available, the
algorithm is fast, and the number of parameters is small. Then the
parameters can be optimized for a small sub-sample and the result
can be used for the rest of the signal.

Exercises

Exercise 8.1 MULIN algorithm

Show that the general MULIN-algorithm of order n is given by

n
(−1)i 2n−i n!
p(k + 1) = p(k) + ∑ [p(k − i) − r(k − i)]. (8.63)
i=0 i!(n − i)!

Exercise 8.2 nLMS algorithm

In an implementation, compare the LMS and the nLMS algorithms

for synthetic inputs, i.e. the function y = sin(x). Check the conver-
gence radius in both cases, by a grid search with varying parameter µ.
Construct a synthetic input function with varying amplitude (such as
the one in figure 8.6). Evaluate the convergence radius for both cases,
again by grid search over the parameter µ.

Exercise 8.3 Haar Transform

Let (t1 , ...,tm ) be a list of m equidistant time points. Then a discret-

ized signal curve is given by assigning function values yi to each time
point, i.e. (y(t1 ), ..., y(tm )) =(y1 , ..., ym ). The signal y = (y1 , ..., ym ) can
be transformed in the following way [8]: (We assume m is even, oth-
erwise we add a zero element to the end of the string (y1 , ..., ym ).)
Set
y1 + y2 y +y y +y
a1 = √ m
√ , a2 = 3√ 4 , ..., am/2 = m−1 (8.64)
2 2 2
318 8 Motion Prediction

and
y1 − y2 y −y y −y
d1 = √ m.
√ , d2 = 3√ 4 , ..., dm/2 = m−1 (8.65)
2 2 2
Let T (y) = (a1 , ..., am/2 , d1 , ..., dm/2 ) denote the transformed signal.
The transformation of the signal y thus obtained is called the Haar
transform. The signal T (y) has the same length m as the input signal
y.
a) Show that the transformation thus defined is invertible, i.e. there is
a mapping I(y), such that I(T (y)) = y.
b) Show that T preserves the energy of the input signal, i.e. the scalar
product yy is equal to T (y)T (y).
c) Define and implement a ’denoising’ scheme based on the Haar
transform. Your denoiser should work as follows: Transform the
signal under T , set to zero all values in T (y) below a fixed
threshold γ, then transform the thresholded signal back under I.
d) Define a synthetic signal, add random noise, and derive a threshold
γ such that 99.9 percent of the energy of the original signal is pre-
served.

Exercise 8.4 Haar Wavelets

The Haar transform T was defined in the previous exercise. The defin-
ition of Haar wavelets is based on the Haar transform.
a) Iterate the construction leading to the Haar transform T , giving
a multi-level transform. Hint: for level 2, assume m is divisible
by 4. Construct the second level transform T (2) by transforming
only the values a1 , ..., am/2 , but leaving the values d1 , ..., dm/2 at
the end of the signal T (y) unchanged. This yields a signal T (2) y =
(a(2) , d(2) , d(1) ). Here, the lengths of the strings a(2) and d(2) are
m/4, while d(1) has length m/2 .
b) Define a component-wise multiplication for signals, i.e. yz =
(y1 z1 , ..., ym zm ). Show that the transform T can be represented by
this component-wise multiplication with so-called Haar wavelets
and Haar scaling functions. Hint: The Haar wavelets and Haar
8.5 Fast-Lane Methods and Performance Measures 319

scaling functions are signals of the same lengths as y. m wave-

lets and m scaling functions will be needed here. Multiplication of
the input signal y with the first Haar-wavelet gives the value d1 .
Multiplication with the first Haar scaling function gives the value
a1 . In the same way, multiplication with the remaining wavelets
gives the remaining values d2 , ..., dm/2 and a2 , ..., am/2 .
c) The computation in equation 8.65 uses adjacent values from the
original signal to compute new values. Thus, the difference of two
adjacent values yi and yi+1 is used to compute di . Looking at the
computation sequence of the c-values in the definition of the à
trous decomposition, discuss the meaning of the term ‘à trous’.

Exercise 8.5 Daubechies Wavelets

The Daubechies wavelets are defined in much the same way as the
Haar wavelets. However, four adjacent values from the original signal
are used to compute two values ai and di . Specifically, we have

√ √ √ √
1+ 3 3+ 3 3− 3 1− 3
a1 = y1 √ + y2 √ + y3 √ + y4 √ . (8.66)
4 2 4 2 4 2 4 2
The values defined in equations 8.33 to 8.35 resemble the definitions
in equations 8.64 and 8.65. In both cases, an average and a difference
of two values is used to define the new values. Thus we could rename
the à trous method and call it the ‘à trous Haar’ method. In fact the
so-called Haar-function is the basis of both definitions: The definition
in equations 8.33 to 8.35 and the definition in equations 8.64 and 8.65.
(The Haar function is defined as the step function with values -1 in the
interval [0, 1/2), +1 in the interval [1/2, 1), and 0 elsewhere.)
Draw a graph of a Daubechies wavelet, and discuss why Daubechies
wavelets (although highly effective in signal processing) would seem
less adequate for prediction than the à trous scheme in equations 8.33
to 8.35.
320 8 Motion Prediction

Summary

Motion prediction is used to overcome the time lag required for data
and image processing, as well as robot motion time, and collimator
motion time. Thus the command sequence sent to the robot will not
refer to the current position of the anatomical target, but to its future
position. The future position of the target can be extrapolated in time,
if the motion is stable and periodic. We obtain a straightforward, yet
very practical method simply by adding the error from previous time
steps. Classical methods from signal processing attempt to capture
unknown characteristics of an incoming signal, and can be applied to
signal prediction and feed-forward loops. In practice, the convergence
radius of the signal processing methods for our application is rather
small, and better results are obtained with more advanced methods,
such as wavelet decomposition and support vector regression. Wave-
let decomposition filters the input signal into several bands. For each
band, a least squares method estimates parameter values for describ-
ing the curve corresponding to this band. Support vector regression
can be applied in several ways to the problem of prediction. Firstly,
we can simply set up samples (t, y(t)), or samples containing sev-
eral points of the form (t, y(t)) with equidistant time spacing. But
secondly, we can apply support vector regression to the bands in a
wavelet decomposition. Finally, fast online methods are particularly
suitable for the problem, since typically only a single new sample is
added at each time step. Measurements for evaluating predictors can
themselves be used in new predictors, which gives rise to a fast-lane
scheme.

Notes

Applications of motion prediction arise in radiosurgery, where we

compensate for respiratory motion by a robotic arm [9]. As an altern-
ative, we can move the leaves of a multi-leaf collimator (figure 6.22)
[10], the treatment couch [11] or a gimbaled radiation source [12, 13].
The LMS algorithm was developed in 1960 for signal filtering [1].
nLMS is the normalized version of the LMS algorithm, see also [14].
References 321

Kalman filtering and RLS were developed in the same context. [2] is
a text book on filtering methods from signal processing. [15] gives a
survey of time series prediction.
The combination of correlation and prediction for moving anatomic
targets was suggested in 2000 [9]. It is in routine clinical use since
2002 [16]. The MULIN method was specifically developed for respir-
ation tracking [17]. Newer methods for respiration prediction include
methods based on kernel adaptive filtering [18].
The wavelet-based predictor is based on the à trous wavelet decom-
position [19]. The term à trous refers to the multi-level computation
series described in the chapter. The authors in [20] have turned this
general decomposer into a predictor, and this was applied to respira-
tion prediction in [21]. [8] is a text book with an introduction to dis-
crete wavelet decomposition.
As we have seen, support vector methods require several external
parameters to be chosen beforehand. One can estimate and modify
the values of the parameters during the application, via fast-lane meth-
ods (see also [5]). Probabilistic methods from machine learning, also
based on kernels, can also be applied to estimate the parameters
[22, 23].

References

[1] B. Widrow and M. E. Hoff, “Adaptive switching circuits,” in IRE

WESCON Convention Record, vol. 4, Aug. 1960, pp. 96–104.
[2] S. Haykin, Adaptive Filter Theory, 4th ed. Englewood Cliffs,
NJ: Prentice Hall, 2002.
[3] F. Ernst and A. Schweikard, “Forecasting respiratory motion
with accurate online support vector regression (SVRpred),” In-
ternational Journal of Computer Assisted Radiology and Sur-
gery, vol. 4, no. 5, pp. 439–447, 2009.
[4] J. Ma, J. Theiler, and S. Perkins, “Accurate on-line support vec-
tor regression,” Neural Computation, vol. 15, no. 11, pp. 2683–
2703, 2003.
[5] F. Ernst, A. Schlaefer, S. Dieterich, and A. Schweikard, “A fast
lane approach to LMS prediction of respiratory motion signals,”
322 8 Motion Prediction

Biomedical Signal Processing and Control, vol. 3, no. 4, pp.

291–299, 2008.
[6] L. Ramrath, A. Schlaefer, F. Ernst, S. Dieterich, and A. Sch-
weikard, “Prediction of respiratory motion with a multi-
frequency based Extended Kalman Filter,” in Proceedings
of the 21st International Conference and Exhibition on
Computer Assisted Radiology and Surgery (CARS’07), ser.
International Journal of CARS, vol. 2, no. supp. 1. Berlin,
Germany: CARS, June 2007, pp. 56–58. [Online]. Available:
http://www.cars-int.org/
[7] F. Ernst, Compensating for Quasi-periodic Motion in Robotic
Radiosurgery. New York: Springer, Dec. 2011.
[8] J. S. Walker, A Primer on Wavelets and Their Scientific Applic-
ations, 2nd ed., ser. Studies in Advanced Mathematics. Boca
Raton, London, New York: Chapman & Hall/CRC, 2008.
[9] A. Schweikard, G. Glosser, M. Bodduluri, M. J. Murphy, and
J. R. Adler, Jr., “Robotic Motion Compensation for Respiratory
Movement during Radiosurgery,” Journal of Computer-Aided
Surgery, vol. 5, no. 4, pp. 263–277, 2000.
[10] P. J. Keall, A. Sawant, B.-C. Cho, D. Ruan, J. Wu, P. R.
Poulsen, J. Petersen, L. J. Newell, H. Cattell, and S. Korreman,
“Electromagnetic-guided dynamic multileaf collimator tracking
enables motion management for intensity-modulated arc ther-
apy,” International Journal of Radiation Oncology, Biology,
Physics, vol. 79, no. 1, pp. 312–320, 2011.
[11] J. Wilbert, J. Meyer, K. Baier, M. Guckenberger, C. Herrmann,
R. Hess, C. Janka, L. Ma, T. Mersebach, A. Richter, M. Roth,
K. Schilling, and M. Flentje, “Tumor tracking and motion com-
pensation with an adaptive tumor tracking system (ATTS): sys-
tem description and prototype testing,” Medical Physics, vol. 35,
no. 9, pp. 3911–3921, Sep 2008.
[12] Y. Kamino, K. Takayama, M. Kokubo, Y. Narita, E. Hirai,
N. Kawada, T. Mizowaki, Y. Nagata, T. Nishidai, and
M. Hiraoka, “Development of a four-dimensional image-guided
radiotherapy system with a gimbaled X-ray head,” International
Journal of Radiation Oncology, Biology, Physics, vol. 66, no. 1,
pp. 271–278, 2006.
References 323

[13] M. Hiraoka, Y. Matsuo, A. Sawada, N. Ueki, Y. Miyaba, M. Na-

kamura, S. Yano, S. Kaneko, T. Mizowaki, and M. Kokubo,
“Realization of dynamic tumor tracking irradiation with real-
time monitoring in lung tumor patients using a gimbaled x-ray
head radiation therapy equipment,” International Journal of Ra-
diation Oncology, Biology, Physics, vol. 84, no. 3, S1, pp. S560–
S561, Nov. 2012.
[14] B. Widrow and S. D. Stearns, Adaptive Signal Processing, ser.
Prentice Hall Signal Processing Series. Upper Saddle River,
NJ: Prentice Hall, 1985.
[15] N. Wiener, Extrapolation, Interpolation, and Smoothing of Sta-
tionary Time Series, ser. M.I.T. Press Paperback Series. Cam-
bridge, MA: The MIT Press, 1964, vol. 9.
[16] A. Schweikard, H. Shiomi, and J. R. Adler, Jr., “Respiration
tracking in radiosurgery,” Medical Physics, vol. 31, no. 10, pp.
2738–2741, 2004.
[17] F. Ernst and A. Schweikard, “Predicting respiratory motion sig-
nals for image-guided radiotherapy using multi-step linear meth-
ods (MULIN),” International Journal of Computer Assisted Ra-
diology and Surgery, vol. 3, no. 1–2, pp. 85–90, June 2008.
[18] D. Ruan and P. J. Keall, “Online prediction of respiratory mo-
tion: multidimensional processing with low-dimensional feature
learning,” Physics in Medicine and Biology, vol. 55, no. 11, pp.
3011–3025, 2010.
[19] M. Holschneider, R. Kronland-Martinet, J. Morlet, and
P. Tchamitchian, “A real-time algorithm for signal analysis with
the help of the wavelet transform,” in Wavelets: Time-Frequency
Methods and Phase Space. Proceedings of the International
Conference, J.-M. Combes, A. Grossmann, and P. Tchamitchian,
Eds. Marseille, France: Springer, Dec. 1987, pp. 286–297.
[20] O. Renaud, J.-L. Starck, and F. Murtagh, “Wavelet-based com-
bined signal filtering and prediction,” IEEE Transactions on Sys-
tems, Man and Cybernetics, Part B: Cybernetics, vol. 35, no. 6,
pp. 1241–1251, 2005.
[21] F. Ernst, A. Schlaefer, and A. Schweikard, “Prediction of res-
piratory motion with wavelet-based multiscale autoregression,”
in MICCAI 2007, Part II, ser. Lecture Notes in Computer Sci-
324 8 Motion Prediction

ence, N. Ayache, S. Ourselin, and A. Maeder, Eds., vol. 4792,

MICCAI. Brisbane, Australia: Springer, November 2007, pp.
668–675.
[22] R. Dürichen, T. Wissel, and A. Schweikard, “Prediction of res-
piratory motion using wavelet-based support vector regression,”
in Proceedings of the 2012 IEEE International Workshop on Ma-
chine Learning for Signal Processing (MLSP). IEEE Signal
Processing Society, 2012, pp. 1–6.
[23] ——, “Optimized order estimation for autoregressive models to
predict respiratory motion,” International Journal of Computer
Assisted Radiology and Surgery, vol. 8, no. 6, pp. 1037–
1042, 2013. [Online]. Available: http://dx.doi.org/10.1007/
s11548-013-0900-0
Chapter 9
Motion Replication

In chapters 7 and 8 we saw two aspects of motion learning: motion

correlation and motion prediction. In both cases, the motions were
anatomical. We now look at more complex motions occuring during
surgery. Suppose we record the motion of the surgeon, i.e. with optical
tracking. We will see three types of motion in our data: intentional mo-
tion, tremor, and noise. But which is which? Clearly, this is a learning
problem. We must classify the different types of motion. As an ap-
plication, we consider robotic systems for motion replication. Such
systems replicate the motion of the surgeon, e.g. in microsurgery.
Systems for motion replication consist of two robots: the first robot
is moved by the human operator (here the surgeon), the second robot
moves the (surgical) instrument. Thus, the surgeon moves a passive
master robot, and an active replicator robot performs the same motions
at a remote site. The scenario is illustrated in figure 9.1.
It is immediately clear why such a system is of advantage in surgery.
Delicate motions can be ‘scaled’. Thus, we can command that any
motion of 1 cm, as performed by the surgeon, should be scaled to a
smaller motion, i.e. 0.1 cm. Systems for prostatectomy and cardiac
valve surgery, relying on this robotic scaling paradigm, are now in
wide-spread clinical use.
But beyond motion scaling, we can filter the natural tremor of the
surgeon. This means that we transmit any regular motion of the sur-
geon to the replicator robot, but not the tremor. Advanced classifica-
tion methods, such as support vector classification (see chapter 7) can

325
326 9 Motion Replication

Operation site
Handle

Replicating robot

Operator Master robot

Fig. 9.1: Motion Replication

be used to classify between involuntary and intended motion, so that

dexterity can be improved.
Likewise, we can detect and compensate for motion at the other end of
the system. Namely, for a very delicate operation, e.g. near the heart,
pulsatory motion can be disturbing. Thus the replicator robot can act-
ively compensate for such motion. Again, as in chapter 8, it can be
useful to predict periodic motion.
Finally, surgical training is a major issue in this context. A variety
of scenarios for rehearsing complex surgical procedures can be based
on systems for motion replication. If manual motion is replicated by
robot motion, then motion paths can be recorded and compared. Thus,
training based on animal models can be avoided.
The kinematics of the two robots in figure 9.1 are clearly not optimal.
Firstly, it is useful to replicate the motion of both hands. Thus we
will need a two-arm construction for the master robot, and for the
9.1 Tremor filtering 327

replicator as well. Secondly, the kinematics of each individual arm of

the master can be optimized to suit the application.
Replication of motion includes the replication of static forces. Thus, in
this chapter, we will look at how to compute the joint torques needed
to apply a force at the tool. Conversely, we will need to find the forces
resulting from given joint torques. For both cases, the geometric Jac-
obian matrix derived in chapter 4 will be useful.

9.1 Tremor filtering

Figure 9.2 shows the x-position of a hand-held optical marker held

by a test person. The subject was asked not to move, and to suppress
motion as much as possible. Small motions were then recorded. Three
types of displacements can be distinguished in the curve. There is a
periodic tremor, and on top of the tremor, a drift in the position is
visible. Finally, measurement noise is visible. The tremor has a fixed
frequency, which can be seen in the curve. It would seem possible for
the surgeon to suppress the drift, if there is some close visual feed-
back, e.g. an instrument is held in close vicinity to a target structure.
Then the drift can be recognized and suppressed. However, the sur-
geon cannot suppress the tremor.

Tremor data
122

120

118

116
x−position

114

112

110

108

106
−2 0 2 4 6 8 10 12 14 16 18
time [sec]

Fig. 9.2: Tremor data recorded with infrared tracking

328 9 Motion Replication

Our goal is to detect and suppress the tremor automatically. A number

of methods can be developed on the basis of the techniques described
in the preceding chapters. In particular, we could simply fit a regres-
sion curve to the motion curve shown in figure 9.2. Support vector
machines give us a very powerful tool for addressing this problem.
Thus, we would use the following three-step procedure:
1. Perform an online support vector regression on the past k position
samples.
2. Based on step one, predict the next position value.
3. Command this position value for the replicator robot, as long as
the master’s motion in the current time step does not exceed a
fixed threshold δ . If δ is exceeded, go to the current position com-
manded by the master robot.
Here, simple thresholding is used to detect the intended motion of
the surgeon. The procedure thus sketched has several drawbacks. In
step three, we must retrain the machine in case δ is exceeded. Thus
we will not have any filtering during the training time. However, we
could detect the frequency of the tremor in the data, but have not used
this possibility.
A frequency-oriented decomposition can be obtained with the à trous
wavelet decomposition (see chapter 8). The decomposition of tremor
data is shown in figure 9.3. However, amongst the individual bands in
a wavelet decomposition, we will have to decide which of the bands
is noise, which is tremor, and which is additional involuntary motion.
This can be done with a learning method, consisting of a training
phase and a production phase, such as support vector classification
(see chapter 7). In the training phase, data for individual subjects are
recorded. The training data can be classified by hand. It is useful to
record several data sets per subject, since tremor data may vary with
external parameters (e.g. daytime, stress, temperature and other ex-
ternal conditions). Given the safety requirements of this application,
small thresholds should be set in advance, such that alterations in com-
manded positions must not exceed the thresholds.
9.2 Forces 329
original signal
300

amplitude
298

296
−5 0 5 10 15 20

Decomposition
2
W1
0

−2
−5 0 5 10 15 20

2
W2
0

−2
−5 0 5 10 15 20

300
c
298

296
−5 0 5 10 15 20
time [sec]

Fig. 9.3: Wavelet decomposition of tremor data, original signal in the top
row, recorded with infrared tracking.

9.2 Forces

The simple kinematic construction in figure 9.1 has a major drawback.

The master robot is an industrial robot, designed for repeating small
motion sequences in manufacturing procedures. In order to use such
robots for motion replication, forces applied by the operating surgeon
must be measured continuously. If the operator exerts a force onto the
robot’s end effector, the robot must actively follow the force. The ro-
bot must then calculate the direction of the motion intended by the
330 9 Motion Replication

measured force, and after this comply with the acting force by mov-
ing to a new position (figure 9.4). During this process, we must com-
pensate for gravity from a payload, since gravity could be mistaken
by the robot as a force exerted by the operator, pushing downwards.
One example for the need for force feedback is suturing in cardiac
surgery. If the robot pulls too hard while tying a knot, tissue can be
damaged. On the other hand, the knots should be tightened as much
as possible. It is then difficult to measure excessive forces.

Force sensor

Effector

Gravity

Fig. 9.4: Force sensing at the end effector

However, force sensing and gravity compensation cause a time lag.

Motions following the force applied by the surgeon are not carried
out immediately, but after a time delay. This delay is noticeable for
the user, and will act as a barrier. To overcome the temporal barrier,
an inexperienced user will apply more forces than necessary. This will
often result in an undesirably large robot reaction. An alternative to
force sensors is to use light-weight cable-driven passive arms for the
master robot, specifically designed for the application. Such arms can
be moved with negligible forces, and friction of the cable construction
acts as a natural barrier towards drift caused by gravity.
Figure 9.5 shows a five-joint kinematic construction for a master robot
arm. The shoulder of the master arm is positioned close to the shoulder
of the operator. The total arm length of this master arm roughly cor-
responds to the length of an adult human arm, such that small motions
can be replicated in a natural way.
9.2 Forces 331

shoulder

Fig. 9.5: Five-joint kinematics for a cable-driven master robot arm.

Suppose our goal is to apply a specific force at the tool tip of the
replicator robot. What are the torques needed a the individual joints
of the robot?
Clearly, in practice, we would need sensors in this application. Non-
etheless, it is useful to understand the connection between forces ap-
plied at the tip of the robot, and torques acting on the joints. We again
consider the planar two-link manipulator (with revolute joints), in fig-
ure 4.1 and we again assume equal link lengths, i.e. l1 = l2 = 1. Before
we begin, we must define the term joint torque.

f
y l

θ1 x

Fig. 9.6: Definition of joint torque. A stick is attached to a base with a

revolute joint (joint angle θ1 ). A force vector f acts at the tip of a stick.
The force applied pushes the stick towards the y-axis.

We thus assume we have a stick, attached to a base (figure 9.6). The

stick can rotate about the joint, but will remain in the drawing plane.
We denote by l the length of the stick. A force, applied to the stick,
is simply a vector. The vector f in figure 9.6 illustrates this. The mag-
332 9 Motion Replication

nitude of the force is given by the length of f, and the direction of the
force is the direction of f.
Here, f is a 3D vector with coordinates
 
fx
f = fy  .
 (9.1)
fz
Since our stick rotates in the drawing plane we have fz = 0.
Let r be a vector of length l, pointing along our stick. Then applying
a force f to the tip of the stick will result in a torque at the joint. We
define the joint torque t by

t = r × f. (9.2)
Hence, t is simply defined as the cross-product of r and f. Since r and
f are in the drawing plane, t will be orthogonal to the drawing plane.
Hence, t coincides with the axis of rotation. The length τ of t is the
magnitude of our torque, i.e.

τ = ||t||. (9.3)
Our next goal is to analyze the two-link manipulator with respect to
forces and joint torques.

y y r1 r2

x p x p

Fig. 9.7: Joint torques for a two-link manipulator

Place vectors r1 and r2 along the two links, as shown in figure 9.7.
Then,
 
cos(θ1 )
r1 = , (9.4)
sin(θ1 )
9.2 Forces 333

and
 
cos(θ1 + θ2 )
r2 = . (9.5)
sin(θ1 + θ2 )
We apply a force to the tip (point p). Following our definition of joint
torque, the torque at joint 2 is then simply given by

t2 = r2 × f. (9.6)
The tip p of our robot is obtained simply by adding the two vectors r1
and r2 .
We now think of the vector r1 + r2 as a single stick (in the sense of
our above definition of torque). This stick connects the origin and the
point p. Thus the joint torque at joint 1, caused by f, is simply

t1 = (r1 + r2 ) × f. (9.7)
This last equation again follows directly from our definition of joint
torques.
Now both vectors t1 and t2 are orthogonal to our robot plane (i.e. the
x-y-plane), hence they are of the form
 
0
t = 0 . (9.8)
τ
Again τ denotes the magnitude of our torque. We consider the mag-
nitudes of the two torques (acting at joints 1 and 2) separately, and we
will call these magnitudes τ1 and τ2 . Inserting our definitions for r1
and r2 , we see that

τ1 = (c1 + c12 ) fy − (s1 + s12 ) fx ,

τ2 = c12 fy − s12 fx . (9.9)

We put the equations for τ1 and τ2 into a matrix:

    
τ1 −s1 − s12 c1 + c12 fx
τ= = (9.10)
τ2 −s12 c12 fy
334 9 Motion Replication

We had assumed that l1 = l2 = 1. This is not a necessary assumption,

but it simplifies our notation. If we repeat the same calculations for
arbitrary values of l1 and l2 , then our result extends to:
    
τ1 −l1 s1 − l2 s12 l1 c1 + l2 c12 fx
τ= = (9.11)
τ2 −l2 s12 l2 c12 fy
Notice that we have expressed f in terms of the base coordinate system
B. To reflect this fact, we write equation 9.11 as

τ = A · B f, (9.12)
where
 
−l1 s1 − l2 s12 l1 c1 + l2 c12
A= . (9.13)
−l2 s12 l2 c12
We see that the joint torques are directly proportional to the arm
lengths. Thus, typically, for a six-joint robot, the torques for joints
2 and 3 are the dominant torques.

Remark 9.1

The matrix A in equation 9.13 looks familiar. It is the transpose of

the Jacobi-matrix in equation 4.27, see chapter 4. We will discuss this
observation in the next section.

(End of Remark 9.1)

Remark 9.2

In the above discussion, we had assumed that the force vector f is

given in terms of the base coordinate system. In an application, we of-
ten have the forces given at the tool. We must thus transform f, in order
to specify forces in the tool coordinate system. To this end, we simply
multiply by the forward kinematic matrix for the two-link manipu-
lator. But notice, since f is a vector, we do not need the translational
part of our matrices. We can thus simply multiply with the orientation
part of the matrix alone, and obtain
9.3 Joint Torques and Jacobi-Matrices 335

 
B c12 −s12 E
f= · f. (9.14)
s12 c12
Inserting this into equation 9.12, we see that
 
c12 −s12 E
τ = A· · f. (9.15)
s12 c12
We now evaluate the product
 
c12 −s12
A· (9.16)
s12 c12
and obtain
   
c12 −s12 l1 s2 l2 + l1 c2
A· = . (9.17)
s12 c12 0 l2

Overall, given a force vector f (with respect to the effector coordinate

system), we have joint torques of
 
l1 s2 l2 + l1 c2 E
τ= · f. (9.18)
0 l2

(End of Remark 9.2)

We have derived a matrix relating forces at the end effector to joint

torques, for the two-link manipulator. The general case will be dis-
cussed in the next section.

9.3 Joint Torques and Jacobi-Matrices

In our definition of the joint torque, we set t = r × f, where r is a

vector along the link, and f is a planar force vector. The torque t is
thus a vector. Here, t is aligned with z = z0 , the axis of the joint.
In equation 9.8 we computed the magnitude τ of the torque t, simply
by taking the scalar product, i.e. τ = tz. This was possible, since z =
z0 = (0, 0, 1)T in this case, and f was a planar force (figure 9.8).
336 9 Motion Replication

z0

r
f
x0 -y0 -plane

Fig. 9.8: Joint torque

What if f is not planar? In this case, t is also aligned with the joint axis
z = z0 , and its length τ is the length of the projection of r × f onto the
joint axis.

τ = (r × f)z
f
z0

Fig. 9.9: Non-planar force

Thus,

τ = (r × f)z (9.19)
and

t = ((r × f)z)z. (9.20)

In equation 9.10 we set up a matrix relating the force f to the joint
torques τ1 and τ2 , for the planar two-link manipulator.
9.3 Joint Torques and Jacobi-Matrices 337

In the following theorem, we will do the same for the general case of
n revolute joints and three-dimensional forces.

Theorem 9.1

We consider a robot with n revolute joints. Let f be a force vector,

expressed in base coordinates. Let pi−1 be the origin of the coordinate
system for joint i. Likewise, zi−1 is the z-vector of this coordinate
system. Then the torques τ1 , . . . , τn induced by f at the n joints are
given by
   
τ1 z0 × (pn − p0 )
 ..   .. 
 . = . ·f (9.21)
τn zn−1 × (pn − pn−1 )

Proof:

pi-1 pn
zi-1
r = (pn - pi-1)
f

y0

z0 p0 x0

Fig. 9.10: Proof of Theorem 9.1

We must show that

τi = [zi−1 × (pn − pi−1 )]f. (9.22)

338 9 Motion Replication

According to our definition in equation 9.19, we have τ = (r × f)z.

Then (pn − pi−1 ) takes the role of r, and zi−1 takes the role of z.
Thus

τi = [(pn − pi−1 ) × f]zi−1 . (9.23)

But now,
τi = zi−1 [(pn − pi−1 ) × f]. (9.24)
For arbitrary 3-vectors a, b, c we have

a(b × c) = (a × b)c. (9.25)

We apply this to equation 9.24 and obtain:

τi = [zi−1 × (pn − pi−1 )]f (9.26)

(End of proof)

Remark 9.3

In chapter 4 we saw that the Jacobi-matrix relates effector velocity to

joint velocity.
We will now show that the transpose of the Jacobi-matrix relates
forces and torques at the effector to joint torques (see also remark
9.1).
We return to the geometric Jacobi-matrix defined in chapter 4.
Our goal is to show that the transpose JT of the Jacobian J relates
forces/torques at the effector to joint torques.
In equation 4.53 we expressed the Jacobi-matrix as
 
Jv
J= , (9.27)
Jω
where

Jv = z0 × (pn − p0 ) z1 × (pn − p1 ) . . . zn−1 × (pn − pn−1 ) , (9.28)

and
9.3 Joint Torques and Jacobi-Matrices 339

Jω = z0 z1 . . . zn−1 . (9.29)
Thus
 
z0 × (pn − p0 )
 z1 × (pn − p1 ) 
T  
Jv =  .. . (9.30)
 . 
zn−1 × (pn − pn−1 )
Comparing this to equation 9.21 we see that indeed
   
τ1 z0 × (pn − p0 )
 ..   ..  T
 . = .  · f = Jv f. (9.31)
τn zn−1 × (pn − pn−1 )

(End of Remark 9.3)

We can thus express the relationship between forces at the effector and
torques at the joints by the transpose of the upper part of the geometric
Jacobian. The following remark shows that the lower part Jω of the
geometric Jacobian allows for computing joint torques from torques
acting upon the effector.

Remark 9.4

We can not only apply a force to the effector, but we can also apply
torque to the effector. Thus, we can induce joint torque in two different
ways: (1) A force is applied to the effector. (2) A torque acts on the
effector.
In case (1) we push the effector with a finger tip. In case (2) we grasp
and twist the effector. Both cases result in joint torque.
Let m = (mx , my , mz )T be a torque, applied to the effector. Then the
induced joint torque is

t = (mz)z. (9.32)
The length of t is again the magnitude τ of the torque
340 9 Motion Replication

τ = mz. (9.33)
As a result of this definition, we obtain t as an orthogonal projection
of the effector torque vector m onto z = z0 (see figure 9.11).

m = (mx , my , mz )T
z = z0

y0
x0

Fig. 9.11: Joint torque T resulting from an effector torque m

From equation 9.27 we see that

JT = (JTv , JTω ). (9.34)

According to equation 9.29 we have
 
z0
JTω =  ...  .
 
(9.35)
zn−1
Now equation 9.33 shows that
 
τ1
 ..  T
 .  = Jω m. (9.36)
τn
We add the two types of joint torques according to equations 9.21 and
9.36, and obtain
 
τ1  
 ..  T f
 . =J . (9.37)
m
τn
9.3 Joint Torques and Jacobi-Matrices 341

(End of Remark 9.4)

The geometric Jacobian J for arbitrary robots was defined in chapter 4.

With the result in equation 9.37, we can now compute the joint torques
necessary for applying a given tool force. We also see how to find
thresholds for joint torques, given limits for applicable forces and
torques.

Example 9.1

Suppose we have a grid of unit vectors. We call these vectors f1 , ..., fn .

The vectors represent directions of forces, which we apply to the end
effectors of a six-joint robot and a seven-joint robot (chapter 3). We
will apply the force vectors sequentially, and determine the maximum
joint torques resulting from the forces fi .
Now assume we scale each vector fi to a magnitude of 50 N. We saw
that geometric Jacobians relate forces to joint torques. To compute the
joint torques resulting from fi , we must set up the geometric Jacobi-
ans for both robots. appendix C does just this, nameley deriving the
geometric Jacobians for the six- and seven-joint robots from chapter 3.
Joint torques are position-dependent. This means that they will vary
with the joint settings of the robot. In the comparison, we can select
corresponding positions for the two robots, or positions from a grid.

joint six-joint case seven-joint case

J1 38.42 31.33
J2 39.07 32.29
J3 22.45 19.35
J4 3.70 19.49
J5 3.70 7.99
J6 0 8.00
J7 0

Table 9.1: Maximum joint torques (in Nm) for a force vector f of 50 N
acting at the effector. Positions of the effector ranging over a point grid
in the work space, and directions of f defined as grid points on a hemi-
sphere.
342 9 Motion Replication

Table 9.1 lists the maximum joint torques at each joint. We see that the
torques at the last joint (joint six for the six-joint robot and joint seven
for the seven-joint robot) are always zero. This is due to the orientation
of the last joint, and to the fact that we do not apply torque to the
effector. To allow for comparision, we adjusted the DH-parameters
in such a way that the total arm length is the same for both robots
(1251mm). The DH-parameters are the same as in table 3.3.
In practice, gear ratios should be as small as possible. For typical
small-size and light-weight motors, the maximum motor torques range
between 0.5 Nm and 1.2 Nm. Thus, from table 9.1 we see that a gear
ratio of 1:60 suffices for forces of up to 50 N, depending on the max-
imum motor torque.

(End of Example 9.1)

Exercises

Exercise 9.1

Extend the static force equation in equation 9.10 for the planar two-
link manipulator to a planar three-link manipulator. Compare to the
force equation obtained by transposing the Jacobian.

Exercise 9.2

Extend the result in Theorem 1 above to the case of prismatic joints,

following the extended definition of the Jacobian in chapter 4. Con-
firm your result in a small example.

Summary

Systems for motion replication allow for down-scaling motion in com-

plex microsurgical applications. Four-arm systems (two master arms,
9.3 Joint Torques and Jacobi-Matrices 343

and two replicator arms) passively replicate motion of the operat-

ing surgeon. The master arms are often light-weight arms driven by
cables.
In addition to motion-scaling, tremor compensation is possible with
motion replicators. To detect tremor, motions are recorded and ana-
lyzed by signal processing methods. Both wavelet decomposition and
support vector methods can be applied in this context.
Time is a critical resource for most surgical interventions (due to the
increase of hemorrhage, and the increase of infection risk with oper-
ating time). Thus the motion replication process should not increase
operating time. Repetitive tasks can often be automatized. Thus, to
save operating time, small sutures can be tied autonomously by the
robot.
Realistic haptic and tactile feedback, as well as autonomous meth-
ods for repetitive tasks (such as suturing) are research topics in the
context of motion replication systems. If the transformation between
torques at joints and forces at the effector is known, we can give force
feedback to the surgeon. The geometric Jacobian provides this force
transformation through the equation:
 
τ1  
 ..  T f
 . =J .
m
τn
where τ1 , ..., τn denote the joint torques, and f and m are the forces
and moments acting at the effector.

Notes

Equation 9.37 is the main equation of the chapter. We saw that it

follows directly from the scalar triple product equation a(b × c) =
(a × b)c. In [1], equation 9.37 is proven via the concept of virtual
work (see also [2]).

Robotic motion replicator systems for surgery are in routine clinical

use for prostate surgery and cardiac surgery since 2002 [3]. During the
344 9 Motion Replication

first year following regulatory clearance, more than 5,000 operations

were performed [4]. The number and type of surgical applications are
expanding very rapidly. Early methods for motion scaling were de-
scribed in [5]. While the original system (da Vinci system, Intuitive
Surgical, Sunnyvale, USA) uses no force feedback (tools are dispos-
able, which presents a difficulty towards integrating complex force
sensing), several methods for including such feedback have been de-
scribed in the literature (see [4] for a survey).
Active tremor compensation for microsurgery has been addressed in
a number of contributions (see [6] for a survey). Beyond the context
of motion replication, tremor compensation can be applied to typical
hand-held surgical instruments as well [7].
To reduce operating time, new generations of instruments, specifically
designed for motion replicator systems, were introduced [8].

References

[1] J. J. Craig, Introduction to Robotics: Mechanics and Control,

3rd ed. Prentice Hall, 2005.
[2] M. W. Spong, S. Hutchinson, and M. Vidyasagar, Robot Modeling
and Control, 1st ed. New York: John Wiley & Sons, Inc., 2005.
[3] R. J. Cerfolio, A. S. Bryant, L. Skylizard, and D. J. Minnich, “Ini-
tial consecutive experience of completely portal robotic pulmon-
ary resection with 4 arms,” The Journal of Thoracic and Cardi-
ovascular Surgery, vol. 142, no. 4, pp. 740 – 746, 2011.
[4] A. M. Okamura, “Methods for haptic feedback in teleoper-
ated robot-assisted surgery,” Industrial Robot: An International
Journal, vol. 31, no. 6, pp. 499–508, 2004.
[5] J. Yan and S. E. Salcudean, “Teleoperation controller design us-
ing h∞ -optimization with application to motion-scaling,” Control
Systems Technology, IEEE Transactions on, vol. 4, no. 3, pp. 244–
258, 1996.
[6] C. N. Riviere, W.-T. Ang, and P. K. Khosla, “Toward active tremor
canceling in handheld microsurgical instruments,” IEEE Transac-
tions on Robotics and Automation, vol. 19, no. 5, pp. 793–800,
2003.
References 345

[7] K. C. Veluvolu and W. T. Ang, “Estimation and filtering of

physiological tremor for real-time compensation in surgical robot-
ics applications,” The International Journal of Medical Robotics
and Computer Assisted Surgery, vol. 6, no. 3, pp. 334–342, 2010.
[8] G.-P. Haber, M. A. White, R. Autorino, P. F. Escobar, M. D. Kroh,
S. Chalikonda, R. Khanna, S. Forest, B. Yang, F. Altunrende, R. J.
Stein, and J. H. Kaouk, “Novel robotic da Vinci instruments for
laparoendoscopic single-site surgery,” Urology, vol. 76, no. 6, pp.
1279–1282, 2010.
Chapter 10
Applications of Surgical Robotics

In chapter 1, we described several applications for surgical robotics.

Having discussed the building blocks for such systems in the preced-
ing chapters, we will now return to the applications. In many cases,
robotic systems were developed to improve existing surgical proced-
ures, and not for new types of interventions. Thus, we must adapt
robotic systems to the needs of the application, to bring out the be-
nefits. For the sample applications considered here (radiosurgery, or-
thopedic surgery, cardiac surgery, urologic surgery, neurosurgery and
neurology) we give some additional background.

10.1 Radiosurgery

In 1949, Lars Leksell constructed a metal frame to rigidly fix a pa-

tient’s head in space [1]. The frame is directly bolted to the skull bone
under local anesthesia. With this frame, Leksell was able to irradi-
ate targets in the head with very high precision. The term stereotaxic
radiosurgery was then introduced to describe this new method.
With the advent of tomography, a first full method for navigation was
developed. Similar to stereotaxic navigation in neurosurgery, it con-
sists of the following steps (see figure 1.14):
Step 1. The frame is attached to the patient’s skull.

Step 2. Tomographic images are taken (CT or MR image data with 30

to 50 slices).

347
348 10 Applications of Surgical Robotics

Step 3. The relative position of the tumor with respect to the frame
coordinates is calculated from the tomography.

Step 4. The frame base remains attached to the patient’s head, and is
additionally attached to the irradiation device.

Step 5. With the double attachment from step 4, we find the exact
spatial reference. The patient couch is repositioned such that
the target is centered at the isocenter of the treatment beams
(typically beams of photon radiation).

Step 6. Treatment
There are two types of radiosurgical systems:
• static beam systems (GammaKnife)
• moving beam systems (LINAC systems, figure 10.1)

gantry rotation

gantry

patient table

table rotation

Fig. 10.1: Five-joint kinematics for a LINAC radiosurgery system. The

dashed arrow shows the axis of the gantry rotation. The patient couch
has three built-in translation axes.

For the first type (Gamma Knife), beams are placed in a helmet, and
do not move during treatment. Thus the frame is attached to the pa-
10.1 Radiosurgery 349

tient’s head, and also to the treament couch. We thus have a static spa-
tial relationship between target and the radiation source(s). By con-
trast, LINAC systems move the radiation beam along circular arcs in
space, while the target is at the center of rotation. By moving the pa-
tient couch, different angles can be reached. Initially, radiosurgery was
limited to the head, due to organ motion in the chest or abdomen.
Robotic radiosurgery with a six-joint robot was introduced in 1995 to
obviate the need for attaching a head frame, and to obtain more kin-
ematic flexibility for beam motion paths [2]. Inverse treatment plan-
ning with linear programming (see chapter 6) computes beam direc-
tions and beam weights [3]. Planning for multi-leaf collimators is dif-
ferent when compared to planning for cylinder collimators, but the
general principles are similar: beam directions for the multi-leaf col-
limator are chosen in the same way as for cylindrical collimators. It is
then necessary to partition the cross-section of each beam into smal-
ler subsections to protect critical tissue in the vicinity of the target. As
for the case of beam selection, this is done by a randomized search.
Random geometric sub-patterns for beam cross-sections are gener-
ated, and a large set of ‘beam-lets’ thus chosen is submitted to linear
programming for weight computation. In the weighting step, a large
number of sub-patterns will receive weight zero. Such patterns are
removed, so that a re-sampling scheme is obtained for inverse plan-
ning [4]. The registration procedure relies on stereo x-ray imaging
and digitally reconstructed radiographs (DRRs) (see chapter 5). Thus,
no frame is needed. An on-line position correction is performed after
periodic x-ray imaging.
Respiratory motion compensation (based on motion correlation) was
then added to exisiting robotic radiosurgery systems [5]. This proced-
ure is called stereotaxic body radiosurgery (SBRS). Fast-lane methods
for motion prediction improve the accuracy of motion tracking in the
chest and abdomen. For correlation-based tracking, 1-5 gold markers
are implanted near the target. Marker-less treatment for lung tumors
uses the following procedure for image registration: Digitally recon-
structed radiographs are matched to live images (stereo x-ray image
pairs). The x-ray images of the target region are partitioned into small
patches. Typically, a 5x5 array of patches is used. Registration match
coefficients based on cross-correlation are evaluated for each pair of
350 10 Applications of Surgical Robotics

patches. Pairs of patches giving poor matches are discarded, while

small adjustments of relative patch positions are permitted. We can
thus address small deformations between CT image acquisition and
treatment.
Before the introduction of SBRS, there was the implicit assump-
tion that respiratory motion in the pancreas is negligible. However,
it turned out that there is substantial respiratory motion in this case
as well. This observation led to new clinical developments, amongst
them stereotaxic procedures for renal tumors, and modifications to
standard protocols for liver tumors, lung tumors and prostate tumors.
Since then, real-time imaging and tracking have much improved, and
are now in routine clinical use.

10.2 Orthopedic Surgery

An early robotic system is the Robodoc system described in [6].

The system burs the implant cavity for hip replacement surgery (fig-
ure 10.2). In this case, the registration procedure is stereotaxic. Thus,
the bone must be attached rigidly to the patient couch, while the pos-
ition of the couch with respect to the robot remains fixed. The cavity
is burred by the robot without user interaction.
Knee replacement is now the most common robotic intervention in or-
thopedic surgery. In conventional knee replacement surgery, the sur-
face of the knee joint is removed with a surgical saw (figure 10.3).
Typically, three to five cuts are made along different angles for the
distal femur. Likewise, a single cut is made on the proximal tibia head.
A recent development obviates the need for rigid bone fixation during
robotic knee surgery, and allows for improved user interaction [7, 8]. It
is also possible to replace only parts of the knee surface (figure 10.4).
The distal end of the femur bone consists of two condyles. In conven-
tional knee replacement, the entire end of the femur is replaced (see
figure 10.3). However, if only a single condyle is damaged, then it
is of advantage to replace only this condyle.This unicondylar implant
should be manufactured to fit the individual anatomy (figure 10.4).
Placing such free-form implants requires improved navigation and
tools for planning the implant shape and position [8]. Thus, an im-
10.2 Orthopedic Surgery 351

Cutting
plane

Implant Implant

cavity

Fig. 10.2: Total hip replacement surgery

distal femur

cutting
planes

bone to
remove for implant
placing
implant

Fig. 10.3: Cutting planes for knee replacement surgery

femur

free-form
unicondylar
implant

Fig. 10.4: Femur condyles and free-from implant

352 10 Applications of Surgical Robotics

plant shape is planned before treatment, taking into account biomech-

anical constraints, while allowing for small posture corrections. The
intervention now proceeds as follows: a surgical bur ablates bony tis-
sue from the surface. The navigation is semi-active, i.e. the surgeon
performs the actual burring. With the shape of the implant matching
allowed motions for the bur, the robot creates a so-called virtual wall
for guiding the surgery. The registration is based on optical track-
ing. A fixture for holding an optical marker is attached to the bone
in a separate procedure before tomographic imaging. The position of
the bone relative to this attached marker is known and will remain
fixed throughout the entire treatment, although the bone itself is free
to move. After extracting the bone surface from a CT image, the inter-
vention and the shape of the implant are planned. The hand-held bur
is equipped with an optical marker. Thus the relative position (bone to
instrument) is obtained without x-ray imaging. The drawback of this
registration scheme is that the additional fixture must be placed be-
fore imaging. A simple extension of this scheme uses x-ray imaging
to extract bone contours, such that the bone fixture can be placed after
imaging and planning.

10.3 Urologic Surgery and Robotic Imaging

Laparoscopic and minimally invasive surgery are techniques for re-

ducing recovery time and surgical trauma. Both techniques were de-
veloped in the 1980s. Soon after the introduction of laparoscopic
surgery, researchers attempted to replace instrument holders, laparo-
scopes and camera fixtures by actuated manipulators [9].
The da Vinci system [10] was designed for robotic telesurgery, partly
to address shortcomings of laparoscopic surgery. The laparoscope is a
long instrument inserted into a small skin incision. The incision point
constrains the possible motion of the laparoscope. In addition to con-
straining the motion, user interaction and visualization is not intuit-
ive. Thus, the incision point is a pivot point, and a leftward motion of
the upper end of the laparoscope results in a rightward motion of the
lower end (figure 10.5). By contrast, a robotic system can produce a
10.3 Urologic Surgery and Robotic Imaging 353

Laparoscope Incision point

Skin surface

Fig. 10.5: Laparoscopy: reversal of motion direction at the skin incision

point.

reversal-free mapping of motion directions and a more intuitive user

interface.
The da Vinci system consists of two units: a control unit (console),
operated by the surgeon, and a patient side cart. The console unit con-
sists of a screen display, two passive serial link arms, the surgeon user
interface and the controller system. The patient side cart comprises a
manipulator arm for moving the endoscope, two robotic motion rep-
licator arms (tool manipulators), as well as an additional user interface
for a surgical assistant.
The passive arms are equipped with grippers and can thus transmit
grip commands to the replicator arms. The screen display projects a
virtual image plane over the motion range of the passive arms. In this
display, the current position of the two replicator arms is visible, so
that both arms can be moved under direct control of the surgeon. Ad-
ditional means are provided to move the endoscope from the control
unit. Several different instruments (scalpels, grippers) can be attached
to the replicator arms, and act as end effectors. The range of opera-
tions now performed with the da Vinci system is not limited to urolo-
gic surgery, but also includes a wide variety of other applications, e.g.
colorectal surgery, hysterectomy, appendectomy and hernia repair.
Robotic C-arms are now in clinical use. Three different concepts have
been proposed: (1) The joints of a conventional C-arm are actuated.
Here the four-joint version from chapter 3 is used. (2) The source and
the detector are mounted to two separate six-joint robots. (3) The C-
shaped source-detector assembly is mounted to a single large robot
[11].
354 10 Applications of Surgical Robotics

10.4 Cardiac Surgery

Atrial fibrillation is a condition characterized by irregular conduction

of electrical impulses in the heart. A surgical treatment of this condi-
tion is the so-called Maze procedure [12]. Cuts in the heart wall and
the resulting scars can block the irregular flow of current (figure 10.6).
The cuts are produced with a catheter, guided by external imaging.
Catheter steering can be improved by an external robotic arm, sim-
ilar to the da Vinci procedure [13]. An alternative to this procedure
is proposed in [14]. Here, external robotic radiosurgery produces the
scars, along the paths indicated by the dashed lines in figure 10.6. Act-
ive motion compensation is needed to address cardiac and respiratory
motion. The advantage is that no catheter is required, which shortens
treatment time.

Superior
Vena Cava
Sup. Pulmonary
Veins

Cut lines

Inf. Pulmonary
Veins
Inferior Vena Cava

Fig. 10.6: Maze procedure for treating atrial fibrillation.

10.6 Control Modes 355

10.5 Neurosurgery

We saw that precise localization of the head is a main element of

neurosurgical procedures. Two methods for localization were de-
scribed above: stereotaxic fixation and x-ray image guidance. Both
methods are in routine clinical use.
X-ray guidance directly tracks the bony skull. However, taking con-
tinuous x-ray images for frame-less treatment is not always possible.
To address this problem, near-infrared methods have been developed
[15]. We project a thin laser beam onto the skin surface of the fore-
head. By analyzing the laser spot on the skin with high resolution
cameras, we determine the skin thickness at this point. Repeating this
for a large number of points, we obtain an estimate for the skull posi-
tion via ICP registration (see chapter 5).
Furthermore, robotic methods can improve the localization in the fol-
lowing ways [16]: The needle is carried by a jointed mechanism, and
the joints can be equipped with motors and position sensors. Force
sensing can detect forces acting on the needle. In this way it becomes
possible to control needle insertion with higher precision and repeat-
ability. Force measurements and tomographic data provide position
information for the needle tip.

10.6 Control Modes

Above we have listed exemplary fields of application. We can derive

five motion control modes from these examples.
1. Stereotaxic: The anatomy is rigidly fixed, and the robot moves
along a pre-planned trajectory during the intervention. Stereo-
taxis is mostly used in radiosurgery for the head and neck and
in neurosurgery.

2. Image-guided with intermittent imaging: An example for this con-

trol mode is frame-less radiosurgery. The patient’s head is free to
move, or only lightly immobilized with a head rest or a head mask.
Imaging is used to check for correct alignment at fixed time inter-
356 10 Applications of Surgical Robotics

vals. The robot adjusts the position of the instrument in case of

misalignment beyond a fixed threshold.

3. Motion compensation/predictive motion compensation: Respirat-

ory and/or cardiac motion is determined by appropriate sensors. A
robot moving a surgical instrument autonomously tracks this mo-
tion. To overcome latencies, motion prediction for fixed time lags
is used. Predictive motion compensation is used for radiosurgical
targets in the chest and abdomen.

4. Virtual wall: In a planning phase, the shape of tissue to be re-

moved is determined. During surgery, the surgeon moves a pass-
ive arm holding the instrument. The arm restricts the motion of
the instrument such that the shape of the tissue area to be removed
matches the planned shape. Markers attached to the target allow
for tracking any relative motion of the target. This method is most
frequently used in orthopedic surgery.

5. Motion replication: As in mode four, the surgeon moves a passive

robot arm. The motion is replicated by a second (often smaller)
arm. This allows for motion scaling and high precision surgery.
Two-arm or multiple-arm kinematics are used. Typical areas of
application are urology and cardiac surgery.
The control modes can be combined with one another to provide ad-
vantages for specific applications.
There is one more control mode, not yet in clinical use, but desirable in
some applications. In this control mode, the robot acts autonomously,
based on real-time imaging. We sketch a potential application for this
autonomous control mode in neurosurgery. Neurosurgeons describe
one of the difficulties in surgery as follows: ‘Protect a wet noodle em-
bedded in concrete, while removing the concrete around it’. Here the
wet noodle is a healthy structure (e.g. the optic nerve), whereas the
concrete is the tumor. Clearly, it would be helpful to be able to see
into the tissue by some small distance (1-2 mm) during surgery. Sev-
eral new image modalities can address this problem, such that we can
obtain an in situ endoscopic image [17]. If it was possible to classify
healthy cells versus tumor cells in such modalities, and if an appropri-
10.6 Control Modes 357

ate robotic instrument was available, the following method for robotic
tumor removal with minimal user interaction would become realistic:
Via a small skin incision or opening, we approach the target. The in-
strument and the endoscopic camera are mounted to a snake-robot.
Having reached the target, we step through the cell array in the vi-
cinity of the target, where small regions are classified by imaging.
Tumor cells are removed step by step, while the instrument plans a
path around healthy cells. The control mode for this scheme is more
autonomous than the above scheme and is based on real-time endo-
scopic imaging.

Summary

Five motion control paradigms for surgical robots were introduced in

the past two decades. The control paradigms are: stereotaxic, image-
guided, motion compensated, virtual-wall guidance and motion rep-
lication. The control paradigms were designed to improve the ac-
curacy of the surgical intervention, and to facilitate navigation. Each
paradigm has a specific area of application. The main areas are: ortho-
pedic surgery, radiosurgery, urologic surgery and minimally invasive
surgery, cardiovascular surgery and neurosurgery. However, some of
the paradigms have recently been applied outside their original area of
application, and it is likely that other, new motion control paradigms
and new areas of application will arise.

Notes

A survey of early methods in robotic surgery is given in [18]. The

first robotic systems for neurosurgery were introduced in 1985 [9, 19].
Here, a biopsy needle for brain tumors was inserted under robot guid-
ance. The Probot system was designed for transurethral resection of
the prostate [20], and introduced in 1997. Positioning an endoscope
during surgery with a robotic arm was described in [21], and the un-
derlying system (AESOP) can be considered a predecessor of the da
358 10 Applications of Surgical Robotics

Vinci system. This positioning system was later used in the develop-
ment of the Zeus system [22], in the context of a telesurgery paradigm
similar to the da Vinci system. The CyberKnife system was originally
developed to provide frameless radiosurgery in the head [23]. It in-
cluded an inverse planning system based on linear programming, and
first patients were treated with the system in 1994. It was later ex-
tended to incorporate full body radiosurgery and robotic motion com-
pensation [24]. Recently, other radiosurgical systems were equipped
with respiration tracking based on correlation and prediction, so that
robotic methods are now available for conventional radiation therapy
as well [25].

References

[1] L. Leksell, “A stereotaxic apparatus for intracerebral surgery,”

Acta Chirurgica Scandinavica, vol. 99, pp. 229–233, 1949.
[2] J. R. Adler, Jr., A. Schweikard, R. Tombropoulos, and J.-C.
Latombe, Modelling and Planning for Sensor-based Intelligent
Robot Systems, ser. Series in Machine Perception and Artificial
Intelligence. World Scientific Publishers Co., 1995, vol. 21, ch.
Image-Guided Robotic Radiosurgery, pp. 460–470.
[3] A. Schweikard, M. Bodduluri, R. Tombropoulos, and J. R.
Adler, Jr., “Planning, calibration and collision-avoidance for
image-guided radiosurgery,” in Proceedings of the IEEE/RSJ/GI
International Conference on Intelligent Robots and Systems
(IROS’94), vol. 2, 12-16 Sept. 1994, pp. 854–861.
[4] A. Schweikard, A. Schlaefer, and J. R. Adler, Jr., “Resampling:
An optimization method for inverse planning in robotic radiosur-
gery,” Medical Physics, vol. 33, no. 11, pp. 4005–4011, Nov.
2006.
[5] A. Schweikard, G. Glosser, M. Bodduluri, M. J. Murphy, and
J. R. Adler, Jr., “Robotic Motion Compensation for Respiratory
Movement during Radiosurgery,” Journal of Computer-Aided
Surgery, vol. 5, no. 4, pp. 263–277, 2000, motion Compensation
in Radiosurgery.
References 359

[6] R. H. Taylor, L. Joskowicz, B. Williamson, A. Guéziec,

A. Kalvin, P. Kazanzides, R. V. Vorhis, J. Yao, R. Kumar,
A. Bzostek, A. Sahay, M. Börner, and A. Lahmer, “Computer-
integrated revision total hip replacement surgery: concept and
preliminary results,” Medical Image Analysis, vol. 3, no. 3, pp.
301 – 319, 1999.
[7] G. Brisson, T. Kanade, A. DiGioia, and B. Jaramaz, “Precision
freehand sculpting of bone,” in MICCAI 2004, Part II, ser. Lec-
ture Notes in Computer Science, C. Barillot, D. R. Haynor, and
P. Hellier, Eds., vol. 3217, MICCAI. Rennes & Saint-Malo,
France: Springer, Sep. 2004, pp. 105–112.
[8] B. Jaramaz and C. Nikou, “Precision freehand sculpting for uni-
condylar knee replacement: design and experimental validation.”
Biomedizinische Technik (Biomedical Engineering), vol. 57,
no. 4, pp. 293–299, Aug. 2012.
[9] Y. S. Kwoh, J. Hou, E. A. Jonckheere, and S. Hayati, “A robot
with improved absolute positioning accuracy for CT guided ste-
reotactic brain surgery,” IEEE Transactions on Biomedical En-
gineering, vol. 35, no. 2, pp. 153–160, Feb. 1988.
[10] G. S. Guthart and J. Salisbury, J., “The IntuitiveTM telesurgery
system: overview and application,” in Proceedings of the IEEE
International Conference on Robotics and Automation, 2000.
(ICRA ’00), vol. 1, 2000, pp. 618–621.
[11] N. Strobel, O. Meissner, J. Boese, T. Brunner, B. Heigl, M. Ho-
heisel, G. Lauritsch, M. Nagel, M. Pfister, E.-P. Röhrnschopf,
B. Scholz, B. Schreiber, M. Spahn, M. Zellerhoff, and
K. Klingenbeck-Regn, “3D imaging with flat-detector C-arm
systems,” in Multislice CT, ser. Medical Radiology, M. F. Re-
iser, C. R. Becker, K. Nikolaou, and G. Glazer, Eds. Springer
Berlin Heidelberg, 2009, pp. 33–51.
[12] J. L. Cox, R. B. Schuessler, H. D’Agostino, Jr, C. M. Stone,
B. C. Chang, M. E. Cain, P. B. Corr, and J. P. Boineau, “The sur-
gical treatment of atrial fibrillation. III. development of a definit-
ive surgical procedure.” Journal of Thoracic and Cardiovascular
Surgery, vol. 101, no. 4, pp. 569–583, Apr. 1991.
[13] C. V. Riga, A. Rolls, R. Rippel, C. Shah, M. Hamady, C. Bick-
nell, and N. Cheshire, “Advantages and limitations of robotic en-
360 10 Applications of Surgical Robotics

dovacular catheters for carotid artery stenting.” Journal of Car-

diovascular Surgery, vol. 53, no. 6, pp. 747–753, Dec. 2012.
[14] A. Sharma, D. Wong, G. Weidlich, T. Fogarty, A. Jack,
T. Sumanaweera, and P. Maguire, “Noninvasive stereotactic ra-
diosurgery (CyberHeart) for creation of ablation lesions in the
atrium.” Heart Rhythm, vol. 7, no. 6, pp. 802–810, Jun. 2010.
[15] F. Ernst, R. Bruder, T. Wissel, P. Stüber, B. Wagner, and A. Sch-
weikard, “Real time contact-free and non-invasive tracking of the
human skull—first light and initial validation,” in Applications of
Digital Image Processing XXXVI, ser. Proceedings of SPIE, vol.
8856, Aug. 2013, pp. 88 561G–1 – 88 561G–8, high-Accuracy
Head Tracking.
[16] M. Heinig, M. F. Govela, F. Gasca, C. Dold, U. G. Hofmann,
V. Tronnier, A. Schlaefer, and A. Schweikard, “Mars - motor
assisted robotic stereotaxy system,” in Proceedings of the 5th
International IEEE EMBS Conference on Neural Engineering,
May 2011, pp. 334 – 337, stereotactic Micronavigation.
[17] C. Otte, G. Hüttmann, G. Kovács, and A. Schlaefer, “Phantom
validation of optical soft tissue navigation for brachytherapy,” in
MICCAI Workshop on Image-Guidance and Multimodal Dose
Planning in Radiation Therapy, W. Birkfellner, J. R. McClel-
land, S. Rit, and A. Schlaefer, Eds. MICCAI, Oct. 2012, pp.
96–100.
[18] M. J. H. Lum, D. C. W. Friedman, G. Sankaranarayanan,
H. King, K. Fodero, R. Leuschke, B. Hannaford, J. Rosen, and
M. N. Sinanan, “The RAVEN: Design and validation of a telesur-
gery system,” The International Journal of Robotics Research,
vol. 28, no. 9, pp. 1183–1197, 2009.
[19] S. Lavallee, J. Troccaz, L. Gaborit, P. Cinquin, A. L. Bena-
bid, and D. Hoffmann, “Image guided operating robot: a clin-
ical application in stereotactic neurosurgery,” in Proceedings of
the IEEE International Conference on Robotics and Automation,
1992. (ICRA’92), vol. 1, 1992, pp. 618–624.
[20] S. J. Harris, F. Arambula-Cosio, Q. Mei, R. D. Hibberd, B. L.
Davies, J. E. Wickham, M. S. Nathan, and B. Kundu, “The
Probot–an active robot for prostate resection,” Proceedings of
References 361

the Institution of Mechanical Engineers, Part H: Journal of En-

gineering in Medicine, vol. 211, no. 4, pp. 317–325, 1997.
[21] L. K. Jacobs, V. Shayani, and J. M. Sackier, “Determination of
the learning curve of the AESOP robot,” Surgical Endoscopy,
vol. 11, no. 1, pp. 54–55, 1997.
[22] M. Ghodoussi, S. E. Butner, and Y. Wang, “Robotic surgery
- the transatlantic case,” in Proceedings of the IEEE Interna-
tional Conference on Robotics and Automation, 2002 (ICRA
’02), vol. 2, 2002, pp. 1882–1888.
[23] J. R. J. Adler, M. J. Murphy, S. D. Chang, and S. L. Hancock,
“Image-guided robotic radiosurgery,” Neurosurgery, vol. 6,
no. 44, pp. 1299–1306, 1999.
[24] A. Schweikard, H. Shiomi, and J. R. Adler, Jr., “Respiration
tracking in radiosurgery,” Medical Physics, vol. 31, no. 10, pp.
2738–2741, 2004, motion Compensation in Radiosurgery.
[25] T. Depuydt, K. Poels, D. Verellen, B. Engels, C. Collen,
C. Haverbeke, T. Gevaert, N. Buls, G. van Gompel, T. Reyn-
ders, M. Duchateau, K. Tournel, M. Boussaer, F. Steenbeke,
F. Vandenbroucke, and M. de Ridder, “Initial assessment of tu-
mor tracking with a gimbaled linac system in clinical circum-
stances: a patient simulation study.” Radiotherapy and Onco-
logy, vol. 106, no. 2, pp. 236–240, Feb. 2013.
Chapter 11
Rehabilitation, Neuroprosthetics and
Brain-Machine Interfaces

Most stroke patients need rehabilitation training and physiotherapy.

For example, it is possible that a stroke patient can move the left, but
not the right hand. Then a very simple training device can help. This
device is a tendon-driven hand support, controlled via two commands:
grasp and release. The patient can switch between the two states with
the healthy hand, and then learn simple tasks requiring both hands.
Frequent use of the device in daily routine can improve the results of
physiotherapy, e.g. strengthen the right arm, and even help the right
hand. An improved version of this device offers more commands, i.e.
moving the index finger, so that the patient can type on a keyboard or
dial phone numbers.
But there is an even simpler application in this context: passive finger
motion can help in the rehabilitation process. Thus, actuators move
the fingers of the patient in pre-defined patterns.

11.1 Rehabilitation for Limbs

As an example for robotics in rehabilitation, the end-effector of a

small robot arm (typically a three-joint robot) is connected to the fore-
arm of the patient. The robot guides the motion of the patient’s arm,
and force/torque sensors record data. During training, the robot first
moves to a pre-defined position. At this position, the patient is asked
to apply a force or torque. The process is then repeated for a series of

363
364 11 Rehabilitation, Neuroprosthetics and Brain-Machine Interfaces

Belt

Exosceleton

Cart

Fig. 11.1: Exoskeleton and mobile cart for gait training

different arm positions. We can add a computer screen to this set-up.

On the screen, the patient’s arm motions are replicated by motions of a
cursor. In this case, the robot arm is passive. The patient can now steer
a point on the screen while solving small animated puzzles generated
by the interface [1, 2].
Likewise, an exoskeleton can be attached at several points along the
arm. Both principles (end-effector or exoskeleton) have their advant-
ages with respect to cost and flexibility [3, 4].
A device for leg rehabilitation is shown in figure 11.1. The patient’s
upper body is held by a mobile platform with wheels. The platform
can follow the patient. An exoskeleton generates motion patterns for
the legs. The device shown in the figure is limited to walking patterns
on even floors. For uneven terrains and stairs, the platforms must be
equipped with additional sensors and actuators [5, 6].

11.2 Brain-Machine Interfaces

A cochlea implant consists of a small microphone with a signal pro-

cessing unit. The signal processing unit collects external auditory sig-
nals, and stimulates the patient’s auditory nerve. Auditory brainstem
11.2 Brain-Machine Interfaces 365

implants use the same technology as cochlea implants, but directly

stimulate the brain stem of the patient. Cochlea implants are in routine
use, and demonstrate the potential of brain-machine interfaces.
In general, a brain-machine interface (BMI) records signals from the
brain to control external actuators. However, for most applications,
the control of external devices alone is not sufficient, and ‘upstream’
sensory feedback is needed. Four types of BMIs can be distinguished:
• electrodes
• electromyography (EMG)
• spectroscopy
• imaging
For electrodes, several invasive and non-invasive methods have been
investigated. The earliest (and one of the least invasive) of such meth-
ods is EEG (electro-encephalography). The EEG signal arises from
the joint activity of millions of neurons close to the brain surface. It
is acquired on the scalp surface by macroscopic electrodes. EEG sig-
nals are in the frequency range of DC to 200 Hz with an amplitude
of some 10 µV pp. EEG measurements from up to 64 points on the
scalp are the standard in neurological diagnostics. The main tool for
EEG analysis is visual inspection by the neurologist. To the untrained
eye, the signal looks like noise. Nonetheless, some of the typical sig-
nals can be used in brain-machine interfaces. One of the most robust
signal stems from the so-called ‘readiness-potential’: some hundred
milliseconds before the onset of a typical bodily action, the poten-
tial of both brain hemispheres differs by a measureable amount. Like-
wise, the content within certain frequency bands changes in response
to internal or external events. This so called ‘Event Related Desyn-
chronization/Synchronization’ (ERDS) or micro-Rhythm Modulation
can often be recognized by signal processing and machine learning
methods. Another recognizable signal stems from the motor cortex as
a result of imaginary movements (‘Motor Imaginary Paradigm’). It is
possible to replace the electrical recordings by near-infrared monit-
oring of the surface brain tissue, and thereby measure hemodynamic
processes. Although EEG-based brain-machine interfaces suffer from
a number of shortcomings (low signal bandwidth, low information
transfer rates, high response times and low specificity), they have an
366 11 Rehabilitation, Neuroprosthetics and Brain-Machine Interfaces

established place as a communication channel for patients with the

locked-in syndrome (patients unable to even move their eyes).

A B C D E F
G H I J K L
M N O P Q R
S T U V W X
Y Z 1 2 3 4
5 6 7 8 9

Fig. 11.2: Speller matrix [7].

The so-called speller matrix (figure 11.2) further illustrates the use,
but also the limitiations of EEG for BMIs [8]. The matrix contains the
letters of the alphabet and the numbers 0, . . . , 9 in a quadratic table.
There are six rows and columns. The matrix is equipped with a small
light source for each letter and number. With the light sources, we can
highlight either an entire row of the matrix, or a column, as illustrated
in the figure. Assume a subject (or patient) wears a cap with EEG
electrodes. Our goal is to establish communication between the sub-
ject and a second person in another room, solely via the EEG signals.
We do this letter by letter.

Suppose now the subject wishes to transmit the letter ‘P’. We highlight
the rows and columns of the matrix in a random pattern. Thus, for
example, row 1 flashes for several seconds. After this, column 3 would
flash. The target letter ‘P’ is neither contained in row 1 nor in column
3. However, when we activate column 4 (containing ‘P’), we obtain
a detectable reaction in the EEG. More specifically, mismatch and
match conditions in visual tasks are typical data extractable from EEG
signals.
Similar EEG-based interfaces have been developed for wheel-chairs
and avatars in virtual reality environments. Here, EEG analysis de-
tects directional intention (left-right, forward-reverse) [9]. It should
11.2 Brain-Machine Interfaces 367

be noted that EEG analysis for this application is difficult, and long
training phases are needed for each individual patient. For example,
the EEG signal will look very different if the patient simply closes
the eyes. To address such difficulties, an EEG must be combined with
a system for artifact correction, e.g. an electro-oculogram (EOG) for
recording eye movement.

The inherent limitations of EEG raise the requirements for the control
intelligence of the robotic exoskeleton. One further shortcoming of
EEG in this context is the lack of an upstream sensory feedback path.

EMG

Several other control channels such as speech control, eye tracking,

micro joysticks and tongue pads have been developed for controlling
prostheses. One of the most widely used methods is EMG. In this
case, electrical signals are produced in healthy or leftover muscle tis-
sue. In a long established method for upper limb amputees, electrodes
are placed in the socket of the anatomical prosthesis. By changing
muscular tension in the remaining part of the limb (and with it the
electrical output of muscles), prostheses can be triggered to run pre-
programmed grip routines. This so-called electromyographic signal
(EMG signal) can be acquired with macroscopic electrodes from all
superficial muscles by thresholding the filtered and amplified signal.
Since only big and superficial muscles give good EMG signals, the
number and resolution of control channels is limited and does not
match the state of the art hand prostheses. No force or haptic feed-
back is given to the user, other than visual confirmation of position in
space.
A major improvement of myographic control was achieved in 2005.
The procedure is called ‘Targeted Muscle Re-Innervation’ (TMR). Pa-
tients undergo a surgical procedure, which rewires leftover, but now
unused arm nerves like the median, ulnar, and radial nerves to the big
pectoral muscle in the chest [10]. Thus larger muscle masses become
accessible for high resolution EMG recordings.
With this method, patients can control robotic hand-arm prostheses
[11], even for two arms. Although the surgical procedure is not
368 11 Rehabilitation, Neuroprosthetics and Brain-Machine Interfaces

Nerves
normally Chest Electrodes
connecting muscles
the spine to
the hand

Robotic
arm

Missing arm

Before TMR After TMR

surgery surgery

Fig. 11.3: Targeted Muscle Re-Innervation (TMR)

without risk, it has a major advantage: It can provide upstream sensory

paths by electrically stimulating the afferent nerve fibers through the
skin. In this way, during motion of the prosthesis, a motion feedback
can be given back to the patient. For a robotic arm with a large num-
ber of degrees of freedom, successful TMR can produce dozens of
separable electrical control signals, which enables the bearer to use a
highly complex hand-arm prosthesis. The American electrician Jesse
Sullivan lost both arms in a work accident in 2004, and was one of the
first patients to receive a complete two-arm robotic prosthesis. After
TMR-surgery, he was able to perform complex two-arm tasks, such
as grasping an empty glass and a bottle of water and pouring water
into the glass. One of the research goals in this field is to minimize
learning on the side of the patient, and incorporate most of the motion
learning into the robotic system.
11.2 Brain-Machine Interfaces 369

We count TMR (after the initial surgery) amongst the non-invasive

recording methods. More invasively, electrodes can be attached to the
exposed surface of the brain (electrocorticography, ECoG) or inserted
into the deep brain (multielectrode arrays, MEAs).
MEAs in the deep brain require invasive interfacing to neurons on
a cellular level by microelectrodes. This may seem impossible, given
the number of cells, and the complexity of the information processing.
Several major discoveries helped to find a path towards this goal. In
the 1950s, Hubel and Wiesel showed that the activation of one single
cell in a frog’s visual system was sufficient to trigger its tongue-reflex
to shoot for a target [12]. It was also shown that different cells are
sensitive for different, extremely detailed pieces of information stem-
ming from the visual field [13]. One cell might be sensitive to a dark
spot moving on bright background (possibly a flying insect), another
cell is most sensitive to a pattern moving over a visual scene (a flock
of birds). Cells can be tuned to a particular input: specific stimulation
patterns correspond to maximal action potentials. To acquire mean-
ingful control signals from a mass of cells, one would need to find the
right cell for the particular control task - clearly impossible given the
number of cells. In the late 1990s it was discovered that by integrating
over the tuning curve of many neurons, it is possible to predict limb
movement. This information code of the brain is now called the neur-
onal population code [14]. Based on this, BMIs were implemented for
the control of a robot arm [15, 16].
Clearly, electrical recording is the most widely used method for ob-
taining information from the brain. However, information processing
in the brain not only relies on electrical signals, but also on chemical
processes. For spectroscopy, we do not read electrical signals from
neurons, but detect changes in local neurotransmitter concentrations.
There are several technical approaches to do this, the most common of
which are MRI spectroscopy and micro-dialysis. MRI spectroscopy
is a variant of MRI imaging, where local concentrations of GABA
and glutamate can be measured. For micro-dialysis, a thin catheter is
placed in the brain. In this case, measurements for several neurotrans-
mitters including GABA and glutamate, serotonin and dopamine can
be taken.
370 11 Rehabilitation, Neuroprosthetics and Brain-Machine Interfaces

Finally, we can also listen to the local signal processing in the brain
via imaging. The first such method is fMRI. Here, the patient or sub-
ject must be inside an MRI scanner, i.e. cannot walk around. A second
method is optical coherence tomography (OCT). It is capable of de-
tecting local morphology in tissue to a depth of 1-2 mm with very high
resolution. We conjecture [17] that neuronal activity is associated with
small morphologic changes, i.e. neurons move or contract when they
fire. OCT allows for several extensions, which can be of help here.
Firstly, it is possible to combine OCT with microscopy, giving a tech-
nique called OCM (optical coherence microscopy). Secondly, OCT
and OCM devices can be miniaturized and placed at the tip of a thin
needle, giving a technique called endoscopic OCM.

Electrode

Pulse
Generator

Fig. 11.4: Deep brain stimulation

11.3 Steerable Needles

Tissue damage in the brain (after injury, disease, or stroke) can cause
cognitive deficits and movement disorders. For example, Parkinson’s
disease can be treated with deep brain stimulation (DBS), if medic-
ation does not or no longer help (figure 11.4). With DBS, electrodes
are implanted into the deep brain, targeting specific nuclei respons-
ible for the symptoms. A pulse generator is implanted in the chest and
connected to the electrode. The electrode thus stimulates the affected
11.3 Steerable Needles 371

neurons directly. Besides Parkinson’s disease, epilepsy, depression, or

obsessive compulsive disorders are treated with DBS.

Fig. 11.5: Needle Steering

Typically, electrodes are placed with needle-based stereotaxic meth-

ods (see chapters 1 and 10). The insertion process can be improved if
the needle-system is steerable. This means we bend the needle appro-
priately while inserting it, to avoid critical regions in the brain. The
insertion path will then no longer be a straight line, but a curve.

Fig. 11.6: Needle with bevel tip

Several kinematic principles for needle-steering have been developed

in recent years. As an example, we consider a system with two
needles, one of which is hollow, while the other is much thinner, and
bent (figure 11.5). We insert the second needle into the first. Since
the second needle is bent, it will come out at the bottom end of the
first needle on a curved path. When inserting the second needle into
the first, we can also rotate it. In this way, we can reach a range of
three-dimensional positions with the needle tip, along curved paths.
372 11 Rehabilitation, Neuroprosthetics and Brain-Machine Interfaces

By taking more than two such needles, we can produce more complex
curvature patterns.

The neurosurgeon controls the position of the needle tip via acoustic
signals. The firing patterns of individual regions of the brain are char-
acteristic. This can be detected acoustically during needle insertion,
by reading from the electrodes at the tip of the needle and sending
the signals to a speaker. If we notice a slight deviation from the path,
we would have to retract the needle pair and insert it again. This is a
disadvantage of the above steering method with hollow needles.
This difficulty can be addressed by an alternative steering scheme (fig-
ure 11.6). Here we have a single highly flexible needle with a bevel tip.
The bevel tip causes the needle to move into the tissue along curved
paths. By rotating the needle appropriately during the inserting pro-
cess, we can steer the tip [18].

As noted above, MEAs can record signals from the deep brain. To
apply MEAs, we need to place the electrodes into the appropriate loc-
ation in the deep brain. Similarly, in the future, endosopic imaging
(e.g. OCT or OCM imaging) may be capable of recording such signals
from individual neurons. The goal of OCM in this case is to read the
entire firing pattern of three-dimensional clusters of neurons, which
would not be possible with MEAs [17].
Finally, it is possible to give sensory and haptic feedback from an
artificial hand back to the brain [19]. To this end the activity of the
peripheral nerve of a patient’s arm is recorded with flexible micro
electrode arrays. This enables the patient not only to send control sig-
nals to a prosthetic hand, but also to use the afferent nerve structures
to obtain shape information via the artificial hand.
11.3 Steerable Needles 373

Exercises

Outer needle

l1 z0
x0
Inner needle l2 = π

Fig. 11.7: Needle kinematics

Exercise 11.1

a) Derive forward and inverse kinematic equations for needle steer-

ing with two needles as shown in figure 11.5. Assume the inner
needle has the shape of a walking stick, i.e. a half-circle mounted
to a straight top (figure 11.7) and the curvature radius is 1.
The position of the tip is determined by three parameters θ , l1 and
l2 . Assume the needle is always inserted along the negative z0 -
axis. l1 (and l2 ) are the insertion depths of the outer (and inner)
needle. At l2 = π, the inner needle is fully inserted, and the tip of
the inner needle is at the position (2, 0, 0)T . At l2 = 0 the tip is at
the origin.
b) Generalize the result from part a) to the case of an inner needle
with elliptic curvature.
374 11 Rehabilitation, Neuroprosthetics and Brain-Machine Interfaces

References

[1] P. S. Lum, C. G. Burgar, and P. C. Shor, “Evidence for im-

proved muscle activation patterns after retraining of reaching
movements with the MIME robotic system in subjects with post-
stroke hemiparesis,” Neural Systems and Rehabilitation Engin-
eering, IEEE Transactions on, vol. 12, no. 2, pp. 186–194, 2004.
[2] C. H. Hwang, J. W. Seong, and D.-S. Son, “Individual finger
synchronized robot-assisted hand rehabilitation in subacute to
chronic stroke: a prospective randomized clinical trial of effic-
acy,” Clinical Rehabilitation, vol. 26, no. 8, pp. 696–704, 2012.
[3] R. Colombo, F. Pisano, S. Micera, A. Mazzone, C. Delconte,
M. C. Carrozza, P. Dario, and G. Minuco, “Robotic techniques
for upper limb evaluation and rehabilitation of stroke patients,”
IEEE Transactions on Neural Systems and Rehabilitation Engin-
eering, vol. 13, no. 3, pp. 311–324, 2005.
[4] H. I. Krebs, B. T. Volpe, D. Williams, J. Celestino, S. K. Charles,
D. Lynch, and N. Hogan, “Robot-aided neurorehabilitation: A
robot for wrist rehabilitation,” IEEE Transactions on Neural Sys-
tems and Rehabilitation Engineering, vol. 15, no. 3, pp. 327–
335, 2007.
[5] R. Riener, L. Lunenburger, S. Jezernik, M. Anderschitz,
G. Colombo, and V. Dietz, “Patient-cooperative strategies for
robot-aided treadmill training: first experimental results,” IEEE
Transactions on Neural Systems and Rehabilitation Engineering,
vol. 13, no. 3, pp. 380–394, 2005.
[6] Y.-L. Park, B.-r. Chen, D. Young, L. Stirling, R. J. Wood,
E. Goldfield, and R. Nagpal, “Bio-inspired active soft orthotic
device for ankle foot pathologies,” in 2011 IEEE/RSJ Interna-
tional Conference on Intelligent Robots and Systems (IROS),
2011, pp. 4488–4495.
[7] L. A. Farwell and E. Donchin, “Talking off the top of your head:
toward a mental prosthesis utilizing event-related brain poten-
tials,” Electroencephalography and Clinical Neurophysiology,
vol. 70, no. 6, pp. 510–523, 1988.
[8] P.-J. Kindermans, D. Verstraeten, and B. Schrauwen, “A
bayesian model for exploiting application constraints to enable
References 375

unsupervised training of a P300-based BCI,” PLoS ONE, vol. 7,

no. 4, p. e33758, 2012.
[9] G. Vanacker, J. d. R. Millán, E. Lew, P. W. Ferrez, F. G. Moles,
J. Philips, H. Van Brussel, and M. Nuttin, “Context-based fil-
tering for assisted brain-actuated wheelchair driving,” Computa-
tional Intelligence and Neuroscience, vol. 2007, p. 25130, 2007.
[10] T. A. Kuiken, G. A. Dumanian, R. D. Lipschutz, L. A. Miller,
and K. A. Stubblefield, “The use of targeted muscle reinnerv-
ation for improved myoelectric prosthesis control in a bilateral
shoulder disarticulation amputee,” Prosthetics and Orthotics In-
ternational, vol. 28, no. 3, pp. 245–253, 2004.
[11] T. A. Kuiken, G. Li, A. Lock, R. D. Lipschutz, L. A. Miller, K. A.
Stubblefield, and K. B. Englehart, “Targeted muscle reinnerva-
tion for real-time myoelectric control of multifunction artificial
arms,” JAMA, vol. 301, no. 6, pp. 619–628, 2009.
[12] D. H. Hubel and T. N. Wiesel, “Receptive fields of single neur-
ones in the cat’s striate cortex,” The Journal of Physiology, vol.
148, no. 3, pp. 574–591, 1959.
[13] ——, “Receptive fields, binocular interaction and functional ar-
chitecture in the cat’s visual cortex,” The Journal of Physiology,
vol. 160, no. 1, pp. 106–154, 1962.
[14] J. K. Chapin, K. A. Moxon, R. S. Markowitz, and M. A. L.
Nicolelis, “Real-time control of a robot arm using simultan-
eously recorded neurons,” Nature Neuroscience, vol. 2, no. 7,
pp. 664–670, 1999.
[15] L. R. Hochberg, D. Bacher, B. Jarosiewicz, N. Y. Masse, J. D.
Simeral, J. Vogel, S. Haddadin, J. Liu, S. S. Cash, P. van der
Smagt, and J. P. Donoghue, “Reach and grasp by people with
tetraplegia using a neurally controlled robotic arm,” Nature, vol.
485, no. 7398, pp. 372–375, 2012.
[16] M. Velliste, S. Perel, M. C. Spalding, A. S. Whitford, and A. B.
Schwartz, “Cortical control of a prosthetic arm for self-feeding,”
Nature, vol. 453, no. 7198, pp. 1098–1101, 2008.
[17] R. Ansari, C. Myrtus, R. Aherrahrou, J. Erdmann, A. Sch-
weikard, and G. Hüttmann, “Ultrahigh-resolution, high-speed
spectral domain optical coherence phase microscopy,” Optics
Letters, vol. 39, no. 1, pp. 45–47, 2014.
376 11 Rehabilitation, Neuroprosthetics and Brain-Machine Interfaces

[18] R. J. Webster, J. S. Kim, N. J. Cowan, G. S. Chirikjian, and A. M.

Okamura, “Nonholonomic modeling of needle steering,” The In-
ternational Journal of Robotics Research, vol. 25, no. 5-6, pp.
509–525, 2006.
[19] G. S. Dhillon and K. W. Horch, “Direct neural sensory feedback
and control of a prosthetic arm,” Neural Systems and Rehabil-
itation Engineering, IEEE Transactions on, vol. 13, no. 4, pp.
468–472, 2005.
Appendix A
Image Modalities and Sensors

The main modalities applicable to medical navigation are: x-ray, flu-

oroscopy, ultrasound, infrared tracking, magnetic tracking, computed
tomography (CT) and magnetic resonance imaging (MRI). A num-
ber of newer modalities are also used: positron emission tomography
(PET), single photon emission computed tomography (SPECT), dif-
fusion tensor imaging (DTI), optical coherence tomography (OCT)
and photo-acoustic tomography (PAT). Further methods are variants
and extensions of the basic methods. They include: four-dimensional
CT and MRI, optical coherence microscopy (OCM), functional mod-
alities such as functional MRI (fMRI) and magnetic particle imaging
(MPI).
We briefly summarize characteristics of these modalities and sensors
with respect to navigation and robotics.

X-ray imaging and CT

Radiologic navigation is based on x-ray imaging with a C-arm. This

method is described in chapter 1. Typical domains of application are
cardiac surgery and orthopedic surgery. By moving the C-arm along
an arc in space, we can acquire CT-images. The process of recon-
structing a 3D CT from a series of C-arm images is discussed in ex-
ercise 1.2. Standard CT scanners are sometimes used directly during
surgery. However, CT image data from CT scanners mostly serves as
preoperative reference, and is often registered to intra-operative C-

377
378 A Image Modalities and Sensors

arm images. For older C-arms, the images must be calibrated, i.e. we
must place a calibration phantom into the beam path. The geometry of
the phantom is assumed to be known and we can thus determine the
projection geometry and distortion features.

MRI

In MRI, a strong magnetic field (B0 -field) is generated together with

gradient fields. The gradient fields allow for selectively measuring a
tissue response to an external pulse. Selective measurement means
that only the response of points in tissue within a certain region (e.g. a
plane in space) determines the measurement. By repeating the meas-
urement with modified parameters, this region can be moved. This
generates an equation system which can be solved in a reconstruction
process. The solution of the reconstruction yields a correlation to the
proton density at points in a three-dimensional grid within the ana-
tomy. MR-imaging was originally developed for diagnostics. To use
MR-imaging in navigation, images must be calibrated. Distortions in
MR images are difficult to correct, for the following reasons. Ana-
tomic tissue is susceptible to magnetization. Thus the B0 -field will
cause a magnetization of the tissue. Different types of tissue (bone,
soft tissue, air cavities) are more or less susceptible to such magnet-
ization. This results in small inhomogeneities in the B0 -field, causing
distortions. Notice that we cannot use calibration phantoms here. An
ad hoc method for calibrating MR images for navigation is described
in [1]. In this ad hoc method, no registration to other imaging mod-
alities and no calibration phantoms are used. Instead, the effects of
the magnetization caused by the B0 -field are simulated in an equation
system, using a table of different magnetic susceptibilities of tissue
types. A forward calculation on the (distorted) image will then cause
additional distortion. This additional distortion is inverted, giving an
approximation vector field for correcting the original distortion.
If we compare the images in figure 1.6 (CT image and MR image
of the same anatomy), it becomes clear that MR images show better
contrast in the soft tissue. However, the bone can be segmented easily
in the CT image, e.g. by thresholding. Likewise, CT imaging is the
A Image Modalities and Sensors 379

modality of choice in a number of applications in radiation therapy

and radiosurgery. Here it is necessary to compute dose distributions
for radiation treatments. Tissues with different absorption coefficients
can be directly differentiated in a CT image.

Four-dimensional Imaging

In most cases, CT and MR are used in single image mode. This means
that a single image stack is taken at a fixed point in time. Breathing
artifacts can be reduced with respiratory gating. An alternative is four-
dimensional CT or MR imaging. A four dimensional respiratory CT
data set consists of 10 image stacks, where each stack corresponds
to one single time point in the respiratory cycle [2]. Applications of
four-dimensional imaging (e.g. in radiosurgery and cardiology) are
discussed in chapter 7.

Ultrasound

Ultrasound is non-invasive, and can provide functional information

(Doppler ultrasound). However, images are distorted, (often more than
MR-images) and they contain speckle noise and artifacts. For distor-
tion correction, ultrasound images can be registered to CT or MR im-
ages. Here, three-dimensional ultrasound is of advantage. Ultrasound
distortions are in the range between 2 to 7 percent of the distance
between the target and the ultrasound camera. The ultrasound camera
is placed directly onto the skin surface, so that this distance estimate
becomes meaningful. It is possible to estimate the distortions in an
ultrasound image directly, using a table of Hounsfield values for dif-
ferent types of tissue. Thus assume a CT image of the anatomical tar-
get region is given. Hounsfield values are derived from the CT image,
and the values can be mapped to acoustic properties of the individual
tissue types via a table look-up. We then map Hounsfield values to the
acoustic impedance of the tissue type, and to the acoustic speed in that
tissue type. With this information, the distortions can be corrected [3].
380 A Image Modalities and Sensors

Furthermore, it is possible to compute an optimized viewing window

for the ultrasound camera. After simulating the acoustic properties of
the tissue between camera and target, one can compute the optimal
camera window from the CT image. The ultrasound camera can thus
be positioned by a robot arm to track moving targets.
In laparoscopic surgery (also called keyhole surgery), surgical instru-
ments are inserted via a small incision while the operation process is
displayed on a computer monitor. Thus, the surgeon cannot directly
see the operation site. Ultrasound imaging is now a standard imaging
modality for laparoscopy. Endoscopes, laparoscopic instruments and
laparoscopic ultrasound must be navigated to allow for registering im-
ages from different modalities.

Infrared Tracking

An infrared tracking system computes the position and orientation

of one or more probes in real time. Current infrared tracking sys-
tems generate position/orientation data with up to 4000 Hz. Typic-
ally, the output is: x-, y-, z-values for the probe tip and a quaternion
for probe orientation. Several types of systems can be distinguished.
Two-camera systems compute the position of spherical markers on the
probe by triangulation. Thus the cameras take an image pair showing
the scene from two distinct viewing directions, with known relative
positions of the cameras. The stereo view allows for determining the
exact position of each marker in space. The geometry of the arrange-
ment of markers is known in advance. Three camera-systems for in-
frared tracking do not use triangulation. Instead the position of the
markers on the probe is computed from three images. Here, each cam-
era is a line camera (figure A.1). The marker position is computed in
the following way: each line camera reports the position of a flash-
ing marker as the brightest point along the camera line. This brightest
point will identify a plane in space, which must contain the marker.
Intersecting these three planes gives the spatial position of the marker.
A Image Modalities and Sensors 381

line cameras

Fig. A.1: Three-camera system for infrared tracking.

Magnetic Tracking

The physical principle for magnetic tracking is very different from in-
frared tracking. The position of small active coils is computed from
readings of a magnetic sensor array. The advantage of magnetic track-
ing over infrared tracking is the following: the marker (coil) does not
have to be visible in a camera image. Thus magnetic tracking is suit-
able for navigating laparoscopic instruments, endoscopes and biopsy
needle tips. However, the coil must be connected to the host unit by
a cable. In surgical applications, the cable must be sterile. Newer sys-
tems use small cable-less electromagnetic transponders, which can be
implanted into the target region before the operation, and will then
serve as landmarks during the operation.

Optical Coherence Tomography

Optical coherence tomography (OCT) generates 3D images in real-

time. The physical principle is the following: light is projected onto
the anatomic target. The reflected light is compared to the projec-
ted light with respect to run-time, via spectrometry [4]. Thus, OCT
is sometimes called ‘ultrasound with light’. If the light path is a single
line, orthogonal to the tissue surface, the resulting scan is called an A-
scan. Thus an A-scan collects information about a series of voxels
along a short line segment. Likewise, sweeping this line segment
along the tissue gives information on all voxels in a small rectangle,
382 A Image Modalities and Sensors

perpendicular to the tissue surface. This acquisition sequence is called

a B-scan. Finally, the last spatial direction is obtained by sweeping this
rectangle sideways across a volume of tissue. OCT is used for navig-
ation during implant positioning (e.g. cochlea implants). A limitation
for the application of OCT is the penetration depth in tissue, which is
at most 2 mm.

Fig. A.2: Motor cortex mapping with fMRI (left), and robotic TMS
(right).

fMRI

Functional MRI (fMRI) images are taken directly in the MR-scanner,

using a special driver software. Magnetic properties of hemoglobin
change with the oxygen level of the blood [5]. For navigation, it is use-
ful to register an fMRI image to a morphological MRI image. Thus,
one can obtain a functional map for the motor cortex (e.g. before a
neurosurgical operation). Figure A.2 (left) shows an example: a sub-
ject is asked to tap with the little finger, while fMRI images are taken.
For comparison, the right part of the figure shows the result of motor
A Image Modalities and Sensors 383

cortex mapping with transcranial magnetic stimulation (TMS). Here,

a robot moves the TMS coil. We activate the coil at the points of a
fine grid close to the subject’s head. At each grid point, the motor
evoked potential (MEP) at the finger is measured. The figure shows
the interpolated grid position the producing peak MEP.

Fig. A.3: Magnetization curve for supraparamegnetic iron oxide

particles.

Magnetic Particle Imaging

Magnetic particle imaging (MPI) is a functional modality, and shows

the diffusion of a contrast agent in the blood vessel tree. In MPI, a
field-free point (within a strong static field) is generated by an appro-
priate spatial arrangement of magnetic coils. The key observation is
the following [6]: superparamagnetic nanoparticles have a non-linear
magnetization curve, implying that a saturation point can be reached
(figure A.3). Beyond the saturation point, increasing the field strength
will only cause a very small increase in magnetization of the particles.
Particles outside the field-free point are under a strong external field,
have thus reached magnetic saturation, and will not react to external
pulses. Thus, the particles at the field-free point produce an isolated
reaction to an external pulse, and this reaction can be measured ex-
ternally. By adapting currents in the external coils, the field-free point
can be moved over an anatomical field of view. MPI thus allows for
determining the spatial distribution of the nanoparticles in real-time.
384 A Image Modalities and Sensors

References

[1] S. Burkhardt, A. Schweikard, and R. Burgkart, “Numerical de-

termination of the susceptibility caused geometric distortions in
magnetic resonance imaging,” Medical Image Analysis, vol. 7,
no. 3, pp. 221–236, Sep 2003.
[2] D. A. Low, M. Nystrom, E. Kalinin, P. Parikh, J. F. Dempsey,
J. D. Bradley, S. Mutic, S. H. Wahab, T. Islam, G. Christensen,
D. G. Politte, and B. R. Whiting, “A method for the reconstruction
of four-dimensional synchronized CT scans acquired during free
breathing,” Medical Physics, vol. 30, no. 6, pp. 1254–1263, 2003.
[3] R. Bruder, F. Ernst, A. Schlaefer, and A. Schweikard, “TH-
C-304A-07: Real-time tracking of the pulmonary veins in 3D
ultrasound of the beating heart,” in 51st Annual Meeting of
the AAPM, ser. Medical Physics, vol. 36, no. 6. Anaheim,
CA, USA: American Association of Physicists in Medicine, Jul.
2009, p. 2804, motion Compensation in Radiosurgery. [Online].
Available: http://www.aapm.org/meetings/09AM/
[4] D. Huang, E. Swanson, C. Lin, J. Schuman, W. Stinson,
W. Chang, M. Hee, T. Flotte, K. Gregory, C. Puliafito, and J. G.
Fujimoto, “Optical coherence tomography,” Science, vol. 254, no.
5035, pp. 1178–1181, 1991.
[5] N. K. Logothetis, J. Pauls, M. Augath, T. Trinath, and A. Oelter-
mann, “Neurophysiological investigation of the basis of the fMRI
signal,” Nature, vol. 412, no. 6843, pp. 150–157, 2001.
[6] B. Gleich and J. Weizenecker, “Tomographic imaging using the
nonlinear response of magnetic particles,” Nature, vol. 435, no.
7046, pp. 1214–1217, Jun. 2005.
Appendix B
Selected Exercise Solutions

Solution for Exercise 1.3

d) Assume x(n) is not in the plane ai x − bi = 0. We must show that (1)

x(n+1) is in this plane, and (2) the vector x(n+1) − x(n) is a non-zero
multiple of the normal vector ai , i.e. x(n+1) − x(n) = λ ai , with λ 6=
0. For (1), we directly find that ai x(n+1) − bi = 0, by inserting the
definition of x(n+1) . For (2), we see from the definition of x(n+1)
that λ = − ai x a2−bi .
(n)

Solution for Exercise 2.3

b) For  
nx ox ax px
ny oy ay py 
M=
nz oz az
 (B.1)
pz 
0 0 0 1
the inverse is given by
 
nx ny nz −np
ox oy oz −op
M−1 = 
ax ay az −ap
 (B.2)
0 0 0 1

385
386 B Selected Exercise Solutions

Solution for Exercise 2.5

There are several equivalent ways to find the YPR-angles given a ro-
tation matrix. One is the following:
Start from equation
 
nx ox ax
R = R(z, γ)R(y, β )R(x, α) = ny oy ay  (B.3)
nz oz az
Then insert the elementary rotations (see equation 2.17) giving
   
cγ cβ cγ sβ sα − sγ cα cγ sβ cα + sγ sα nx ox ax
sγ cβ sγ sβ sα + cγ cα sγ sβ cα − cγ sα  = ny oy ay  (B.4)
−sβ cβ sα cβ cα nz oz az
We thus have

sβ = −nz (B.5)
We could solve this equation for β using the arcsin-function, i.e. we
could simply set β = arcsin −nz . However, arcsin only returns values
in the range [−π/2, π/2]. There may be solutions outside this range.
We could define a function asin2 similar to the atan2-function (see
equation 2.34). Instead we prefer to use atan2 since we have already
defined it. Since sin2 β + cos2 β = 1, we have
q
cβ = ± (1 − n2z ) (B.6)
Given equations B.5 and B.6, we are in a position to apply the function
atan2, i.e.
q
β1,2 = atan2(−nz , ± (1 − n2z )) (B.7)
After having solved for β , we can insert our value into equation B.4,
and obtain values for α and γ.
From the upper two elements of the first matrix column in equa-
tion B.4 we obtain the value for γ.
 
ny nx
γ = atan2 , (B.8)
cβ cβ
B Selected Exercise Solutions 387

The last two elements of the last matrix line in equation B.4 give α.
 
oz az
α = atan2 , (B.9)
cβ cβ
Notice that in equations B.8 and B.9 we assume cβ 6= 0. We must
handle the case cβ = 0 separately. In this case, we have two possibil-
ities:
1. β = π/2 and sβ = 1,
2. β = −π/2 and sβ = −1.
For the first possibility, the matrix equation B.4 simplifies to
   
0 sα−γ cα−γ nx ox ax
 0 cα−γ −sα−γ  = ny oy ay  (B.10)
−1 0 0 nz oz az
Here we have used the trigonometric addition formulas to simplify
expressions (for example: cγ sα − sγ cα is written as sα−γ ).
Likewise, for the second possibility we obtain:
   
0 −sα+γ −cα+γ nx ox ax
0 cα+γ −sα+γ  = ny oy ay  (B.11)
1 0 0 nz oz az
In both cases the solutions for α and γ can again be found with the
atan2-function from the matrix equations B.10 and B.11. Notice that
in both cases the solutions for γ and α will not be unique. We can only
obtain expressions for α + γ or α − γ. The interpretation of this fact
is the following: at sβ = 0, we have a singularity, and the two angles
γ and α can compensate each other.

Solution for Exercise 2.7

b) Given the quaternion q = (q1 , q2 , q3 , q4 ), set θ = 2 arccos(q1 ) and

(ux , uy , uz ) = sin1θ /2 (q2 , q3 , q4 ) and insert c = cosθ , s = sin θ and
(ux , uy , uz ) into equation 2.48.
388 B Selected Exercise Solutions

c) Consider

 
c + u2x (1 − c) ux uy (1 − c) − uz s ux uz (1 − c) + uy s
uy ux (1 − c) + uz s c + u2y (1 − c) uy uz (1 − c) − ux s =
uz ux (1 − c) − uy s uz uy (1 − c) + ux s c + u2z (1 − c)
 
nx ox ax
ny oy ay 
nz oz az
(B.12)

From the main diagonal we obtain

nx + oy + az = (1 − c)(u2x + u2y + u2z ) + 3c (B.13)

We have (u2x + u2y + u2z ) = 1, hence
nx + oy + az
θ = arccos( ) (B.14)
2
Furthermore, from equation B.12, we have

oz − ay = 2sux
ax − nz = 2suy
ny − ax = 2suz

From this, we can directly derive the vector (ux , uy , uz ).

Solution for Exercise 4.8

For part a) we have

θ 0 (0) = 0 (B.15)

θ 0 (1) = 0 (B.16)

θ (0) = a1 (B.17)
B Selected Exercise Solutions 389

θ (1) = b1 (B.18)
and

θ (t) = c0 + c1t + c2t 2 + c3t 3 . (B.19)

Taking the derivative with respect to t gives

θ 0 (t) = c1 + 2c2t + 3c3t 2 . (B.20)

From equation B.17, we obtain

c0 = a1 (B.21)
Combining equation B.15 and equation B.20 we have

c1 = 0 (B.22)
Hence

θ (t) = a1 + c2t 2 + c3t 3 . (B.23)

From equation B.18 we see that

b1 = a1 + c2 + c3 (B.24)
Combining equation B.16 and equation B.20

0 = 2c2 + 3c3 (B.25)

The last two equations are linear equations for c2 and c3 , and we find:

c0 = a1
c1 = 0
c2 = 3(b1 − a1 )
c3 = −2(b1 − a1 )
(B.26)

Solution for Exercise 5.6

390 B Selected Exercise Solutions

Show that H(A) ≥ 0 for a random variable A.

By definition

H(A) = − ∑ pA (a) log pA (a) (B.27)

a
and

0 ≤ pA (a) ≤ 1. (B.28)
Thus for each a, we have

pA (a) log(1/pA (a)) ≥ 0 (B.29)

so that

H(A) = ∑ pA (a) log(1/pA (a)) ≥ 0. (B.30)

a

Solution for Exercise 7.2

Looking at only two sample points x1 and x2 , explain why the quad-
ratic program
Minimize
w21 + w22
Subject to

y1 (wx1 + d) ≥ 0
y2 (wx2 + d) ≥ 0
(B.31)

where y1 = 1 and y2 = −1, computes a maximum margin separator

line.
Represent the line by its parameters (w, d), where w is the normal
vector of the line. Then this representation of a line can be multiplied
by a factor λ , without changing the line. Thus (λ w, λ d) for non-zero
λ is the same line as (w, d). First show that is suffices to consider only
those lines having
B Selected Exercise Solutions 391

y1 (wx1 + d) = 1
y2 (wx2 + d) = 1
(B.32)

Why is it enough to look at those lines only which can indeed be

scaled in this way? The reason for this will be explained next: Since
we are looking for a maximum margin separator line, we can safely
assume that any line we look at has

y1 (wx1 + d) = a
where a > 0, and

y2 (wx2 + d) = b
with b > 0 as well. Otherwise our line would cross one of the points
x1 , x2 , and then it can certainly not be a maximum margin separator
line. But now we can also assume a = b. Otherwise, if a were not
equal to b for a given line we could simply move this line slightly and
obtain a line with a yet bigger margin. But now, since a = b, we can
scale with a factor λ to have the above form equation B.32. But now
recall that for any line, we obtain the point-line distance by scaling the
normal vector w to unit length, so that the margin is given by:
w d
x1 + (B.33)
kwk kwk
To see why this is indeed the margin, recall that the distance of the
origin from a line with equation wx + d = 0 is d, if the normal vector
w has unit length, i.e. kwk = 1. Likewise distances of the points x1 , x2
from the given line can be calculated. But equation B.33 is the same
as
1
(wx1 + d) (B.34)
kwk
And by equation B.32 we have wx1 + d = 1. Hence the margin is
1
simply kwk . To maximize the margin, we must minimize the length of
w.
392 B Selected Exercise Solutions

It may seem strange to minimize the length of a normal vector, since

it would appear that a minimal length normal vector would simply be
the zero vector. But this is not so, since we look at only those (scaled!)
normal vectors satisfying equation B.32!

Solution for Exercise 7.3

Show that the system in equation 7.16 is actually a quadratic program.

b) Define 2m variables θ1 , . . . , θ2m by setting:
θi = αi − αi0 for i = 1, . . . , m
and
θi+m = αi0 for i = 1, . . . , m.
For two sample points x1 , x2 , i.e. m = 2 set
 
x1 x1 x1 x2 0 0
x2 x1 x2 x2 0 0
H=  0
 (B.35)
0 0 0
0 0 00
.
Based on our definition of the variables θi , we must define con-
straints on the variables αi and αi0 . For example (again in the case
m = 2), we have α1 = θ1 + θ3 , and thus translate the constraint
α1 ≥ 0 into θ1 + θ3 ≥ 0. To call the Matlab-routine, we must set
up a matrix A and a vector b, such that Aθ ≤ b. To express our
constraint as a ≤-constraint, we write −θ1 − θ3 ≤ 0. Thus the first
line in the constraint matrix A is given by the four entries

−1 0 −1 0 (B.36)
and the first entry of the vector b is 0.
Now define the constraint matrix A and the vector b according to
B Selected Exercise Solutions 393

A b meaning
−1 0 −1 0 0 α1 ≥ 0
0 −1 0 −1 0 α2 ≥ 0
1 0 1 0 C α1 ≤ C
0 1 0 1 C α2 ≤ C
0 0 −1 0 0 α10 ≥ 0 (B.37)
0 0 0 −1 0 α20 ≥ 0
0 0 1 0 C α10 ≤ C
0 0 0 1 C α20 ≤ C
1 1 0 0 0 (α1 − α1 ) + (α2 − α20 ) ≤ 0
0

−1 −1 0 0 0 (α1 − α10 ) + (α2 − α20 ) ≥ 0

.
Notice that the last two lines of the matrix are obtained by resolv-
ing the constraint (α1 − α10 ) + (α2 − α20 ) = 0.

Fig. B.1: Regression with dual quadratic programming, result for two
points x1 = 1, x2 = 2, y1 = 1, y2 = 2

Finally, we define f as
 
ε − y1
ε − y2 
f=
 2ε 
 (B.38)
2ε
394 B Selected Exercise Solutions

Now call θ = quad prog(H, f, A, b). Your result should look like
figure B.1.

Solution for Exercise 7.4

Consider the optimization problem

Minimize

f (x, y) = x2 + y2
Subject to

g(x, y) = x + y = 2
a) Find the Lagrange function for this system.
b) Find the optimum with the help of the Lagrange function.
We have L(x, y, α) = f (x, y) − αg(x, y) = x2 + y2 + α(x + y − 2).
Taking 5L = 0, we have

x+y = 2
2x = α
2y = α
This is a linear system with the solution x = 1, y = 1, α = 2.

Solution for Exercise 7.5

a) Set up the Lagrange function for the linear program

maximize

c1 x + c2 y (B.39)
subject to

a11 x + a12 y = b1
a21 x + a22 y = b2 (B.40)
B Selected Exercise Solutions 395

Take derivatives of Λ with respect to the Lagrange multipliers αi

and the primal variables x, y, and set the resulting expressions to
zero. Inserting these equations back into L, show that Λ = bα, and
AT α = c.
We have

Λ = c1 x + c2 y − α1 (a11 x + a12 y − b1 ) − α2 (a21 x + a22 y − b2 )

(B.41)
Taking the partial derivatives and setting to zero we obtain

∂Λ
= a11 x + a12 y − b1 = 0 (B.42)
∂ α1
∂Λ
= a21 x + a22 y − b2 = 0 (B.43)
∂ α2
∂Λ
= c1 − α1 a11 − α2 a21 = 0 (B.44)
∂x
∂Λ
= c2 − α1 a12 − α2 a22 = 0 (B.45)
∂y
From the last two equations, we directly obtain

c1 = α1 a11 + α2 a21 (B.46)

c2 = α1 a12 + α2 a22 (B.47)

and hence AT α = c.
Inserting equations B.42 and B.43 into the expression for Λ , we
have

Λ = c1 x + c2 y (B.48)
From equations B.46 and B.47 we have

Λ = (α1 a11 + α2 a21 )x + (α1 a12 + α2 a22 )y. (B.49)

Rearranging and again applying equations B.42 and B.43 gives
396 B Selected Exercise Solutions

Λ = α1 (a11 x + a12 y) + α2 (a21 x + a22 y) = α1 b1 + α2 b2(, B.50)

hence Λ = αb.

Solution for Exercise 11.1

We first assume the needle only moves in the x0 - z0 - plane, i.e. we

ignore the rotation of the needle by the angle θ . Thus, our target point
is denoted by
   
px px
 
p = py = 0   (B.51)
pz pz
We place a point q = (1, 0, 0)T (figure B.2). We have
     
0 cos(l2 ) 1 − cos(l2 )
p =  0 +q− 0  =  0  (B.52)
−l1 sin(l2 ) −l1 − sin(l2 )

z0
l1
q
x0

l2

Fig. B.2: Kinematics for needle steering with two needles

We write this in matrix form, i.e. we set up the matrix expressing a

purely translational transformation to the needle tip.
B Selected Exercise Solutions 397
 
1 0 0 1 − cos(l2 )
0 1 0 0 
Mp =  
0 0 1 −l1 − sin(l2 ) (B.53)
100 1
To add the rotation by θ , we premultiply this matrix by the 4x4 matrix
representing z-rotation about the origin. We obtain the product
  
cos(θ ) − sin(θ ) 0 0 1 0 0 1 − cos(l2 )
 sin(θ ) cos(θ ) 0 0 0 1 0 0 
M p (θ ) = 
 0

 
 (B.54)
0 10 0 0 1 −l1 − sin(l2 )
1 0 01 100 1
Evaluating the matrix product we obtain the forward matrix for the
needle tip.
 
cos(θ ) − sin(θ ) 0 cos(θ )(1 − cos(l2 ))
 sin(θ ) cos(θ ) 0 sin(θ )(1 − cos(l2 )) 
M p (θ ) = 
 0
 (B.55)
0 1 −l1 − sin(l2 ) 
1 0 0 1
From the last column of the matrix, we find the position p of the
needle tip.
   
px cos(θ )(1 − cos(l2 ))
p =  py  =  sin(θ )(1 − cos(l2 ))  (B.56)
pz −l1 − sin(l2 )
Then (unless cos(l2 ) = 1) we have

θ = atan2(py , px ) (B.57)
From first two lines in equation B.56 we obtain two cases:
If sin2 (θ ) > ε, we set

l2 = a cos(1 − py / sin(θ )) (B.58)

Otherwise
l2 = a cos(1 − px / cos(θ )) (B.59)
Finally, from the last line in equation B.56

l1 = − sin(l2 ) − pz (B.60)
398 B Selected Exercise Solutions

References

[1] R. Manseur, Robot Modeling & Kinematics, ser. Da Vinci Engin-

eering. Clifton Park, NY, USA: Delmar Cengage Learning, 2006.
Appendix C
Geometric Jacobian for the Six-Joint
Elbow Manipulator

The elbow manipalator with spherical wrist is a six-joint robot with

revolute joints (figure 3.2). We summarize the results of the forward
analysis and derive the geometric Jacobi-matrix for this robot. The
DH-table is:

i αi ai di θi
1 -90 0 0 θ1
2 0 a2 0 θ2
3 90 0 0 θ3
4 -90 0 d4 θ4
5 90 0 0 θ5
6 0 0 d6 θ6

Table C.1: DH-table for the six-joint elbow manipulator with spherical
wrist

i−1 M = T (0, 0, di )R(z,Θi )T (ai , 0, 0)R(x, αi )

i

 
cos (θi ) − sin (θi ) cos (αi ) sin (θi ) sin (αi ) cos (θi ) ai
 

i−1 M = 
sin (θ i ) cos (θ i ) cos (α i ) − cos (θ i ) sin (α i ) sin (θ i ) ai 
i

 0 sin (α ) cos (α ) d 
 i i i 
0 0 0 1

399
400 C Geometric Jacobian for the Six-Joint Elbow Manipulator

DH-matrices:
 
c1 0 −s1 0
 

0M = 
s1 0 c1 0 
1

 0 −1 0 0 
 
0 0 0 1
 
c2 −s2 0 c2 a2
 
1M
 s2 c2 0 s2 a2 
2

= 

0 0 1 0 
0 0 0 1
 
c3 0 s3 0
 
2M
 s3 0 −c3 0 
3 =
 0 1 0 0

 
0 0 0 1
 
c4 0 −s4 0
 
3M
 s4 0 c4 0 
4

= 
0 −1 0 d 
 4

0 0 0 1
 
c5 0 s5 0
 
4M
 s5 0 −c5 0 
5 =
 0 1 0 0

 
0 0 0 1
 
c6 −s6 0 0
 

5M = 
s6 c6 0 0 
6

0 0 1d 
 6

0 0 0 1
C Geometric Jacobian for the Six-Joint Elbow Manipulator 401

Forward kinematics:
 
c1 0 −s1 0
 

0M = 
s1 0 c1 0 
1

 0 −1 0 0 
 
0 0 0 1
 
c1 c2 −c1 s2 −s1 c1 c2 a2
 
0M
 s1 c2 −s1 s2 c1 s1 c2 a2 
2

= 

 −s2 −c2 0 −s2 a2 
0 0 0 1
 
c1 (−s2 s3 + c2 c3 ) −s1 c1 (c2 s3 + s2 c3 ) c1 c2 a2
 
0M
 s1 (−s2 s3 + c2 c3 ) c1 s1 (c2 s3 + s2 c3 ) s1 c2 a2 
3 =
 −s c − c s

 2 3 2 3 0 −s2 s3 + c2 c3 −s2 a2  
0 0 0 1
 
m11 m12 m13 m14
 
0M
 m21 m22 m23 m24 
4

= 
m m m m 
 31 32 33 34 
0 0 0 1
where

m11 = −c1 c4 s2 s3 + c1 c4 c2 c3 − s1 s4
m12 = −c1 (c2 s3 + s2 c3 )
m13 = c1 s4 s2 s3 − c1 s4 c2 c3 − s1 c4
m14 = c1 (d4 c2 s3 + d4 s2 c3 + c2 a2 )
402 C Geometric Jacobian for the Six-Joint Elbow Manipulator

m21 = −s1 c4 s2 s3 + s1 c4 c2 c3 + c1 s4
m22 = −s1 (c2 s3 + s2 c3 )
m23 = s1 s4 s2 s3 − s1 s4 c2 c3 + c1 c4
m24 = s1 (d4 c2 s3 + d4 s2 c3 + c2 a2 )

m31 = −(c2 s3 + s2 c3 )c4

m32 = s2 s3 − c2 c3
m33 = (c2 s3 + s2 c3 )s4
m34 = −d4 s2 s3 + d4 c2 c3 − s2 a2

 
m11 m12 m13 m14
 
0M
 m21 m22 m23 m24 
5

= 

 m31 m32 m33 m34 
0 0 0 1
where

m11 = −c5 c1 c4 s2 s3 + c5 c1 c4 c2 c3 − c5 s1 s4 − c1 s5 c2 s3 − c1 s5 s2 c3
m12 = c1 s4 s2 s3 − c1 s4 c2 c3 − s1 c4
m13 = −s5 c1 c4 s2 s3 + s5 c1 c4 c2 c3 − s5 s1 s4 + c1 c5 c2 s3 + c1 c5 s2 c3
m14 = c1 (d4 c2 s3 + d4 s2 c3 + c2 a2 )

m21 = −c5 s1 c4 s2 s3 + c5 s1 c4 c2 c3 + c5 c1 s4 − s1 s5 c2 s3 − s1 s5 s2 c3
m22 = s1 s4 s2 s3 − s1 s4 c2 c3 + c1 c4
m23 = −s5 s1 c4 s2 s3 + s5 s1 c4 c2 c3 + s5 c1 s4 + s1 c5 c2 s3 + s1 c5 s2 c3
m24 = s1 (d4 c2 s3 + d4 s2 c3 + c2 a2 )
C Geometric Jacobian for the Six-Joint Elbow Manipulator 403

m31 = −c4 c5 c2 s3 − c4 c5 s2 c3 + s5 s2 s3 − s5 c2 c3
m32 = (c2 s3 + s2 c3 ) s4
m33 = −c4 s5 c2 s3 − c4 s5 s2 c3 − c5 s2 s3 + c5 c2 c3
m34 = −d4 s2 s3 + d4 c2 c3 − s2 a2

 
m11 m12 m13 m14
 

0M = 
m21 m22 m23 m 24 
6

m m m m 
 31 32 33 34 
0 0 0 1
where

m11 = −c6 c5 c1 c4 s2 s3 + c6 c5 c1 c4 c2 c3 − c6 c5 s1 s4 − c6 c1 s5 c2 s3 −
c6 c1 s5 s2 c3 + s6 c1 s4 s2 s3 − s6 c1 s4 c2 c3 − s6 s1 c4
m12 = s6 c5 c1 c4 s2 s3 − s6 c5 c1 c4 c2 c3 + s6 c5 s1 s4 + s6 c1 s5 c2 s3 +
s6 c1 s5 s2 c3 + c6 c1 s4 s2 s3 − c6 c1 s4 c2 c3 − c6 s1 c4
m13 = −s5 c1 c4 s2 s3 + s5 c1 c4 c2 c3 − s5 s1 s4 + c1 c5 c2 s3 + c1 c5 s2 c3
m14 = −d6 s5 c1 c4 s2 s3 + d6 s5 c1 c4 c2 c3 − d6 s5 s1 s4 + d6 c1 c5 c2 s3 +
d6 c1 c5 s2 c3 + c1 d4 c2 s3 + c1 d4 s2 c3 + c1 c2 a2

m21 = −c6 c5 s1 c4 s2 s3 + c6 c5 s1 c4 c2 c3 + c6 c5 c1 s4 − c6 s1 s5 c2 s3 −
c6 s1 s5 s2 c3 + s6 s1 s4 s2 s3 − s6 s1 s4 c2 c3 + s6 c1 c4
m22 = s6 c5 s1 c4 s2 s3 − s6 c5 s1 c4 c2 c3 − s6 c5 c1 s4 + s6 s1 s5 c2 s3 +
s6 s1 s5 s2 c3 + c6 s1 s4 s2 s3 − c6 s1 s4 c2 c3 + c6 c1 c4
m23 = −s5 s1 c4 s2 s3 + s5 s1 c4 c2 c3 + s5 c1 s4 + s1 c5 c2 s3 + s1 c5 s2 c3
m24 = −d6 s5 s1 c4 s2 s3 + d6 s5 s1 c4 c2 c3 + d6 s5 c1 s4 + d6 s1 c5 c2 s3 +
d6 s1 c5 s2 c3 + s1 d4 c2 s3 + s1 d4 s2 c3 + s1 c2 a2
404 C Geometric Jacobian for the Six-Joint Elbow Manipulator

m31 = −c6 c4 c5 c2 s3 − c6 c4 c5 s2 c3 + c6 s5 s2 s3 − c6 s5 c2 c3 + s4 s6 c2 s3 +
s4 s6 s2 c3
m32 = s6 c4 c5 c2 s3 + s6 c4 c5 s2 c3 − s6 s5 s2 s3 + s6 s5 c2 c3 + s4 c6 c2 s3 +
s4 c6 s2 c3
m33 = −c4 s5 c2 s3 − c4 s5 s2 c3 − c5 s2 s3 + c5 c2 c3
m34 = −d6 c4 s5 c2 s3 − d6 c4 s5 s2 c3 − d6 c5 s2 s3 + d6 c5 c2 c3 − d4 s2 s3 +
d4 c2 c3 − s2 a2

Geometric Jacobian

" #
Jν
J=
Jω
 
m11 m12 . . . m16
 m21 m22 . . . m26 
 
J =  .. .. . . . .. 
 . . . 
m61 m62 . . . m66
where
C Geometric Jacobian for the Six-Joint Elbow Manipulator 405

m11 = d6 s5 s1 c4 s2 s3 − d6 s5 s1 c4 c2 c3 − d6 s5 c1 s4 − d6 s1 c5 c2 s3 −
d6 s1 c5 s2 c3 − s1 d4 c2 s3 − s1 d4 s2 c3 − s1 c2 a2
m12 = −c1 (d6 c4 s5 c2 s3 + d6 c4 s5 s2 c3 + d6 c5 s2 s3 − d6 c5 c2 c3 +
d4 s2 s3 − d4 c2 c3 + s2 a2 )
m13 = −c1 (d6 c4 s5 c2 s3 + d6 c4 s5 s2 c3 + d6 c5 s2 s3 − d6 c5 c2 c3 +
d4 s2 s3 − d4 c2 c3 )
m14 = s5 (c1 s4 s2 s3 − c1 s4 c2 c3 − s1 c4 ) d6
m15 = −d6 (c5 c1 c4 s2 s3 − c5 c1 c4 c2 c3 + c5 s1 s4 + c1 s5 c2 s3 + c1 s5 s2 c3 )
m16 = 0

m21 = −d6 s5 c1 c4 s2 s3 + d6 s5 c1 c4 c2 c3 − d6 s5 s1 s4 + d6 c1 c5 c2 s3 +
d6 c1 c5 s2 c3 + c1 d4 c2 s3 + c1 d4 s2 c3 + c1 c2 a2
m22 = −s1 (d6 c4 s5 c2 s3 + d6 c4 s5 s2 c3 + d6 c5 s2 s3 − d6 c5 c2 c3 +
d4 s2 s3 − d4 c2 c3 + s2 a2 )
m23 = −s1 (d6 c4 s5 c2 s3 + d6 c4 s5 s2 c3 + d6 c5 s2 s3 − d6 c5 c2 c3 +
d4 s2 s3 − d4 c2 c3 )
m24 = s5 (s1 s4 s2 s3 − s1 s4 c2 c3 + c1 c4 ) d6
m25 = −d6 (s1 s5 c2 s3 + s1 s5 s2 c3 + c5 s1 c4 s2 s3 − c5 s1 c4 c2 c3 − c5 c1 s4 )
m26 = 0
406 C Geometric Jacobian for the Six-Joint Elbow Manipulator

m31 = 0
m32 = d6 s5 c4 s2 s3 − d6 s5 c4 c2 c3 − d6 c5 c2 s3 − d6 c5 s2 c3 − d4 c2 s3 −
d4 s2 c3 − c2 a2
m33 = d6 s5 c4 s2 s3 − d6 s5 c4 c2 c3 − d6 c5 c2 s3 − d6 c5 s2 c3 −
d4 c2 s3 − d4 s2 c3
m34 = s5 s4 (c2 s3 + s2 c3 ) d6
m35 = d6 (−c4 c5 c2 s3 − c4 c5 s2 c3 + s5 s2 s3 − s5 c2 c3 )
m36 = 0

m41 = 0
m42 = −s1
m43 = −s1
m44 = c1 (c2 s3 + s2 c3 )
m45 = c1 s4 s2 s3 − c1 s4 c2 c3 − s1 c4
m46 = −s5 c1 c4 s2 s3 + s5 c1 c4 c2 c3 − s5 s1 s4 + c1 c5 c2 s3 + c1 c5 s2 c3

m51 = 0
m52 = c1
m53 = c1
m54 = s1 (c2 s3 + s2 c3 )
m55 = s1 s4 s2 s3 − s1 s4 c2 c3 + c1 c4
m56 = −s5 s1 c4 s2 s3 + s5 s1 c4 c2 c3 + s5 c1 s4 + s1 c5 c2 s3 + s1 c5 s2 c3
C Geometric Jacobian for the Six-Joint Elbow Manipulator 407

m61 = 1
m62 = 0
m63 = 0
m64 = −s2 s3 + c2 c3
m65 = (c2 s3 + s2 c3 ) s4
m66 = −c4 s5 c2 s3 − c4 s5 s2 c3 − c5 s2 s3 + c5 c2 c3
Appendix D
Geometric Jacobian for the Seven-Joint
DLR-Kuka Robot

The seven-joint robot in figure 3.10 is an extension of the six-joint ro-

bot, obtained by adding a revolute joint between joints two and three.

DH-table:

i αi ai di θi
1 -90 0 d1 θ1
2 90 0 0 θ2
3 -90 0 d3 θ3
4 90 0 0 θ4
5 -90 0 d5 θ5
6 90 0 0 θ6
7 0 0 d7 θ7

Table D.1: DH-table for the seven-joint DLR-Kuka robot

DH-matrices:
 
c1 0 −s1 0
 

0M = 
s1 0 c1 0 
1

 0 −1 0 d 
 1

0 0 0 1

409
410 D Geometric Jacobian for the Seven-Joint DLR-Kuka Robot
 
c2 0 s2 0
 

1M = 
s2 0 −c2 0 
2

 0 1 0 0
 
0 0 0 1
 
c3 0 −s3 0
 
2M
 s3 0 c3 0 
3

= 

 0 −1 0 d3 
0 0 0 1
 
c4 0 s4 0
 
3M
 s4 0 −c4 0 
4 =
 0 1 0 0

 
0 0 0 1
 
c5 0 −s5 0
 
4M
 s5 0 c5 0 
5

= 
0 −1 0 d 
 5

0 0 0 1
 
c6 0 s6 0
 
5M
 s6 0 −c6 0 
6 =
 0 1 0 0

 
0 0 0 1
 
c7 −s7 0 0
 

6M = 
s7 c7 0 0 
7

0 0 1d 
 7

0 0 0 1
D Geometric Jacobian for the Seven-Joint DLR-Kuka Robot 411

Forward kinematics

 
c1 0 −s1 0
 
0M
 s1 0 c1 0 
1

= 

 0 −1 0 d1 
0 0 0 1
 
c1 c2 −s1 c1 s2 0
 
0M
 s1 c2 c1 s1 s2 0 
2 =
 −s 0 c d 

 2 2 1

0 0 0 1
 
c1 c2 c3 − s1 s3 −c1 s2 −c1 c2 s3 − s1 c3 c1 s2 d3
 
0M
 s1 c2 c3 + c1 s3 −s1 s2 −s1 c2 s3 + c1 c3 s1 s2 d3 
3

= 
 −s2 c3 −c2 s2 s3 c2 d3 + d1 

0 0 0 1

 
m11 m12 m13 m14
 
0M
 m21 m22 m23 m24 
4

= 

 m31 m32 m33 m34 
0 0 0 1
where

m11 = c4 c1 c2 c3 − c4 s1 s3 − c1 s2 s4
m12 = −c1 c2 s3 − s1 c3
m13 = s4 c1 c2 c3 − s4 s1 s3 + c1 s2 c4
m14 = c1 s2 d3
412 D Geometric Jacobian for the Seven-Joint DLR-Kuka Robot

m21 = c4 s1 c2 c3 + c4 c1 s3 − s1 s2 s4
m22 = −s1 c2 s3 + c1 c3
m23 = s4 s1 c2 c3 + s4 c1 s3 + s1 s2 c4
m24 = s1 s2 d3

m31 = −s2 c3 c4 − c2 s4
m32 = s2 s3
m33 = −s2 c3 s4 + c2 c4
m34 = c2 d3 + d1

 
m11 m12 m13 m14
 
0M
 m21 m22 m23 m24 
5 =
m m m m 

 31 32 33 34 
0 0 0 1
where

m11 = c5 c4 c1 c2 c3 − c5 c4 s1 s3 − c5 c1 s2 s4 − s5 c1 c2 s3 − s5 s1 c3
m12 = −s4 c1 c2 c3 + s4 s1 s3 − c1 s2 c4
m13 = −s5 c4 c1 c2 c3 + s5 c4 s1 s3 + s5 c1 s2 s4 − c5 c1 c2 s3 − c5 s1 c3
m14 = d5 s4 c1 c2 c3 − d5 s4 s1 s3 + d5 c1 s2 c4 + c1 s2 d3

m21 = c5 c4 s1 c2 c3 + c5 c4 c1 s3 − c5 s1 s2 s4 − s5 s1 c2 s3 + s5 c1 c3
m22 = −s4 s1 c2 c3 − s4 c1 s3 − s1 s2 c4
m23 = −s5 c4 s1 c2 c3 − s5 c4 c1 s3 + s5 s1 s2 s4 − c5 s1 c2 s3 + c5 c1 c3
m24 = d5 s4 s1 c2 c3 + d5 s4 c1 s3 + d5 s1 s2 c4 + s1 s2 d3
D Geometric Jacobian for the Seven-Joint DLR-Kuka Robot 413

m31 = −c5 s2 c3 c4 − c5 c2 s4 + s2 s3 s5
m32 = s2 c3 s4 − c2 c4
m33 = s5 s2 c3 c4 + s5 c2 s4 + s2 s3 c5
m34 = −d5 s2 c3 s4 + d5 c2 c4 + c2 d3 + d1

 
m11 m12 m13 m14
 
0M
 m21 m22 m23 m24 
6 =
m m m m 

 31 32 33 34 
0 0 0 1
where

m11 = c6 c5 c4 c1 c2 c3 − c6 c5 c4 s1 s3 − c6 c5 c1 s2 s4 − c6 s5 c1 c2 s3 −
c6 s5 s1 c3 − s6 s4 c1 c2 c3 + s6 s4 s1 s3 − s6 c1 s2 c4
m12 = −s5 c4 c1 c2 c3 + s5 c4 s1 s3 + s5 c1 s2 s4 − c5 c1 c2 s3 − c5 s1 c3
m13 = s6 c5 c4 c1 c2 c3 − s6 c5 c4 s1 s3 − s6 c5 c1 s2 s4 − s6 s5 c1 c2 s3 −
s6 s5 s1 c3 + c6 s4 c1 c2 c3 − c6 s4 s1 s3 + c6 c1 s2 c4
m14 = d5 s4 c1 c2 c3 − d5 s4 s1 s3 + d5 c1 s2 c4 + c1 s2 d3

m21 = c6 c5 c4 s1 c2 c3 + c6 c5 c4 c1 s3 − c6 c5 s1 s2 s4 − c6 s5 s1 c2 s3 +
c6 s5 c1 c3 − s6 s4 s1 c2 c3 − s6 s4 c1 s3 − s6 s1 s2 c4
m22 = −s5 c4 s1 c2 c3 − s5 c4 c1 s3 + s5 s1 s2 s4 − c5 s1 c2 s3 + c5 c1 c3
m23 = s6 c5 c4 s1 c2 c3 + s6 c5 c4 c1 s3 − s6 c5 s1 s2 s4 − s6 s5 s1 c2 s3 +
s6 s5 c1 c3 + c6 s4 s1 c2 c3 + c6 s4 c1 s3 + c6 s1 s2 c4
m24 = d5 s4 s1 c2 c3 + d5 s4 c1 s3 + d5 s1 s2 c4 + s1 s2 d3
414 D Geometric Jacobian for the Seven-Joint DLR-Kuka Robot

m31 = −c6 c5 s2 c3 c4 − c6 c5 c2 s4 + c6 s2 s3 s5 + s6 s2 c3 s4 − s6 c2 c4
m32 = s5 s2 c3 c4 + s5 c2 s4 + s2 s3 c5
m33 = −s6 c5 s2 c3 c4 − s6 c5 c2 s4 + s6 s2 s3 s5 − c6 s2 c3 s4 + c6 c2 c4
m34 = −d5 s2 c3 s4 + d5 c2 c4 + c2 d3 + d1

 
m11 m12 m13 m14
 

0M = 
m 21 m22 m23 m24 
7

m m m m 
 31 32 33 34 
0 0 0 1
where

m11 = c7 c6 c5 c4 c1 c2 c3 − c7 c6 c5 c4 s1 s3 − c7 c6 c5 c1 s2 s4 − c7 c6 s5 c1 c2 s3 −
c7 c6 s5 s1 c3 − c7 s6 s4 c1 c2 c3 + c7 s6 s4 s1 s3 − c7 s6 c1 s2 c4 −
s7 s5 c4 c1 c2 c3 + s7 s5 c4 s1 s3 + s7 s5 c1 s2 s4 − s7 c5 c1 c2 s3 − s7 c5 s1 c3
m12 = −s7 c6 c5 c4 c1 c2 c3 + s7 c6 c5 c4 s1 s3 + s7 c6 c5 c1 s2 s4 + s7 c6 s5 c1 c2 s3 +
s7 c6 s5 s1 c3 + s7 s6 s4 c1 c2 c3 − s7 s6 s4 s1 s3 + s7 s6 c1 s2 c4 −
c7 s5 c4 c1 c2 c3 + c7 s5 c4 s1 s3 + c7 s5 c1 s2 s4 − c7 c5 c1 c2 s3 − c7 c5 s1 c3
m13 = s6 c5 c4 c1 c2 c3 − s6 c5 c4 s1 s3 − s6 c5 c1 s2 s4 − s6 s5 c1 c2 s3 − s6 s5 s1 c3 +
c6 s4 c1 c2 c3 − c6 s4 s1 s3 + c6 c1 s2 c4
m14 = d7 s6 c5 c4 c1 c2 c3 − d7 s6 c5 c4 s1 s3 − d7 s6 c5 c1 s2 s4 − d7 s6 s5 c1 c2 s3 −
d7 s6 s5 s1 c3 + d7 c6 s4 c1 c2 c3 − d7 c6 s4 s1 s3 + d7 c6 c1 s2 c4 +
d5 s4 c1 c2 c3 − d5 s4 s1 s3 + d5 c1 s2 c4 + c1 s2 d3
D Geometric Jacobian for the Seven-Joint DLR-Kuka Robot 415

m21 = c7 c6 c5 c4 s1 c2 c3 + c7 c6 c5 c4 c1 s3 − c7 c6 c5 s1 s2 s4 − c7 c6 s5 s1 c2 s3 +
c7 c6 s5 c1 c3 − c7 s6 s4 s1 c2 c3 − c7 s6 s4 c1 s3 − c7 s6 s1 s2 c4 −
s7 s5 c4 s1 c2 c3 − s7 s5 c4 c1 s3 + s7 s5 s1 s2 s4 − s7 c5 s1 c2 s3 + s7 c5 c1 c3
m22 = −s7 c6 c5 c4 s1 c2 c3 − s7 c6 c5 c4 c1 s3 + s7 c6 c5 s1 s2 s4 + s7 c6 s5 s1 c2 s3 −
s7 c6 s5 c1 c3 + s7 s6 s4 s1 c2 c3 + s7 s6 s4 c1 s3 + s7 s6 s1 s2 c4 −
c7 s5 c4 s1 c2 c3 − c7 s5 c4 c1 s3 + c7 s5 s1 s2 s4 − c7 c5 s1 c2 s3 + c7 c5 c1 c3
m23 = s6 c5 c4 s1 c2 c3 + s6 c5 c4 c1 s3 − s6 c5 s1 s2 s4 − s6 s5 s1 c2 s3 + s6 s5 c1 c3 +
c6 s4 s1 c2 c3 + c6 s4 c1 s3 + c6 s1 s2 c4
m24 = d7 s6 c5 c4 s1 c2 c3 + d7 s6 c5 c4 c1 s3 − d7 s6 c5 s1 s2 s4 − d7 s6 s5 s1 c2 s3 +
d7 s6 s5 c1 c3 + d7 c6 s4 s1 c2 c3 + d7 c6 s4 c1 s3 + d7 c6 s1 s2 c4 +
d5 s4 s1 c2 c3 + d5 s4 c1 s3 + d5 s1 s2 c4 + s1 s2 d3

m31 = −c7 c6 c5 s2 c3 c4 − c7 c6 c5 c2 s4 + c7 c6 s2 s3 s5 + c7 s6 s2 c3 s4 −
c7 s6 c2 c4 + s7 s5 s2 c3 c4 + s7 s5 c2 s4 + s7 s2 s3 c5
m32 = s7 c6 c5 s2 c3 c4 + s7 c6 c5 c2 s4 − s7 c6 s2 s3 s5 − s7 s6 s2 c3 s4 +
s7 s6 c2 c4 + c7 s5 s2 c3 c4 + c7 s5 c2 s4 + c7 s2 s3 c5
m33 = −s6 c5 s2 c3 c4 − s6 c5 c2 s4 + s6 s2 s3 s5 − c6 s2 c3 s4 + c6 c2 c4
m34 = −d7 s6 c5 s2 c3 c4 − d7 s6 c5 c2 s4 + d7 s6 s2 s3 s5 − d7 c6 s2 c3 s4 +
d7 c6 c2 c4 − d5 s2 c3 s4 + d5 c2 c4 + c2 d3 + d1
416 D Geometric Jacobian for the Seven-Joint DLR-Kuka Robot

Geometric Jacobian

" #
Jν
J=
Jω
 
m11 m12 . . . m16
 m21 m22 . . . m26 
 
J =  .. .. . . .. 
 . . . . 
m61 m62 . . . m66
where

m11 = −d7 s6 c5 c4 s1 c2 c3 − d7 s6 c5 c4 c1 s3 + d7 s6 c5 s1 s2 s4 +
d7 s6 s5 s1 c2 s3 − d7 s6 s5 c1 c3 − d7 c6 s4 s1 c2 c3 − d7 c6 s4 c1 s3 −
d7 c6 s1 s2 c4 − d5 s4 s1 c2 c3 − d5 s4 c1 s3 − d5 s1 s2 c4 − s1 s2 d3
m12 = −c1 (d7 s6 c5 s2 c3 c4 + d7 s6 c5 c2 s4 − d7 s6 s2 s3 s5 + d7 c6 s2 c3 s4 −
d7 c6 c2 c4 + d5 s2 c3 s4 − d5 c2 c4 − c2 d3 )
m13 = −s1 d7 s6 c5 c3 c4 + s1 d7 s6 s3 s5 − s1 d7 c6 c3 s4 − s1 d5 c3 s4 −
c2 d7 s6 c5 c4 c1 s3 − c2 d7 s6 s5 c1 c3 − c2 d7 c6 s4 c1 s3 − c2 d5 s4 c1 s3
m14 = −c1 c3 d7 s6 c5 c2 s4 − c1 d7 s6 c5 s2 c4 + s1 s3 d7 s6 c5 s4 +
c1 c3 d7 c6 c2 c4 − c1 d7 c6 s2 s4 − s1 s3 d7 c6 c4 + c1 c3 d5 c2 c4 −
c1 d5 s2 s4 − s1 s3 d5 c4
m15 = −s6 (c5 c1 c2 s3 − s5 c1 s2 s4 − s5 c4 s1 s3 + c5 s1 c3 + s5 c4 c1 c2 c3 ) d7
m16 = −d7 (c6 s5 c1 c2 s3 + c6 s5 s1 c3 + s6 s4 c1 c2 c3 − c6 c5 c4 c1 c2 c3 +
c6 c5 c1 s2 s4 + c6 c5 c4 s1 s3 + s6 c1 s2 c4 − s6 s4 s1 s3 )
m17 = 0
D Geometric Jacobian for the Seven-Joint DLR-Kuka Robot 417

m21 = d7 s6 c5 c4 c1 c2 c3 − d7 s6 c5 c4 s1 s3 − d7 s6 c5 c1 s2 s4 − d7 s6 s5 c1 c2 s3 −
d7 s6 s5 s1 c3 + d7 c6 s4 c1 c2 c3 − d7 c6 s4 s1 s3 + d7 c6 c1 s2 c4 +
d5 s4 c1 c2 c3 − d5 s4 s1 s3 + d5 c1 s2 c4 + c1 s2 d3
m22 = −s1 (d7 s6 c5 s2 c3 c4 + d7 s6 c5 c2 s4 − d7 s6 s2 s3 s5 + d7 c6 s2 c3 s4 −
d7 c6 c2 c4 + d5 s2 c3 s4 − d5 c2 c4 − c2 d3 )
m23 = −c2 d7 s6 c5 c4 s1 s3 − c2 d7 s6 s5 s1 c3 − c2 d7 c6 s4 s1 s3 − c2 d5 s4 s1 s3 +
d7 s6 c5 c4 c1 c3 − d7 s6 s5 c1 s3 + d7 c6 s4 c1 c3 + d5 s4 c1 c3
m24 = −s2 d7 c6 s4 s1 + s3 d7 c6 c1 c4 + s1 c3 d7 c6 c2 c4 − s2 d5 s4 s1 + s3 d5 c1 c4 +
s1 c3 d5 c2 c4 − s2 d7 s6 c5 c4 s1 − s3 d7 s6 c5 c1 s4 − s1 c3 d7 s6 c5 c2 s4
m25 = −s6 (−c5 c1 c3 + s5 c4 s1 c2 c3 + c5 s1 c2 s3 − s5 s1 s2 s4 + s5 c4 c1 s3 )d7
m26 = d7 (c6 c5 c4 s1 c2 c3 + c6 c5 c4 c1 s3 − c6 c5 s1 s2 s4 − c6 s5 s1 c2 s3 +
c6 s5 c1 c3 − s6 s4 s1 c2 c3 − s6 s4 c1 s3 − s6 s1 s2 c4 )
m27 = 0

m31 = 0
m32 = −d7 s6 c5 c2 c3 c4 + d7 s6 c5 s2 s4 + d7 s6 c2 s3 s5 − d7 c6 c2 c3 s4 −
d7 c6 s2 c4 − d5 c2 c3 s4 − d5 s2 c4 − s2 d3
m33 = s2 (d7 s6 c5 c4 s3 + d7 s6 s5 c3 + d7 c6 s4 s3 + d5 s4 s3 )
m34 = −c2 d5 s4 − c3 d7 c6 s2 c4 + c3 d7 s6 c5 s2 s4 − c2 d7 c6 s4 −
c2 d7 s6 c5 c4 − c3 d5 s2 c4
m35 = s6 (s5 s2 c3 c4 + s5 c2 s4 + s2 s3 c5 ) d7
m36 = −d7 (−c6 s2 s3 s5 + c6 c5 c2 s4 − s6 s2 c3 s4 + c6 c5 s2 c3 c4 + s6 c2 c4 )
m37 = 0
418 D Geometric Jacobian for the Seven-Joint DLR-Kuka Robot

m41 = 0
m42 = −s1
m43 = c1 s2
m44 = −c1 c2 s3 − s1 c3
m45 = s4 c1 c2 c3 − s4 s1 s3 + c1 s2 c4
m46 = −s5 c4 c1 c2 c3 + s5 c4 s1 s3 + s5 c1 s2 s4 − c5 c1 c2 s3 − c5 s1 c3
m47 = s6 c5 c4 c1 c2 c3 − s6 c5 c4 s1 s3 − s6 c5 c1 s2 s4 − s6 s5 c1 c2 s3 −
s6 s5 s1 c3 + c6 s4 c1 c2 c3 − c6 s4 s1 s3 + c6 c1 s2 c4

m51 = 0
m52 = c1
m53 = s1 s2
m54 = −s1 c2 s3 + c1 c3
m55 = s4 s1 c2 c3 + s4 c1 s3 + s1 s2 c4
m56 = −s5 c4 s1 c2 c3 − s5 c4 c1 s3 + s5 s1 s2 s4 − c5 s1 c2 s3 + c5 c1 c3
m57 = s6 c5 c4 s1 c2 c3 + s6 c5 c4 c1 s3 − s6 c5 s1 s2 s4 − s6 s5 s1 c2 s3 +
s6 s5 c1 c3 + c6 s4 s1 c2 c3 + c6 s4 c1 s3 + c6 s1 s2 c4

m61 = 1
m62 = 0
m63 = c2
m64 = s2 s3
m65 = −s2 c3 s4 + c2 c4
m66 = s5 s2 c3 c4 + s5 c2 s4 + s2 s3 c5
m67 = −s6 c5 s2 c3 c4 − s6 c5 c2 s4 + s6 s2 s3 s5 − c6 s2 c3 s4 + c6 c2 c4
Appendix E
Notation

Vectors and Matrices

a column vector
aT transpose of the column vector a
T
a b scalar product of column vectors a, b
ab shorthand notation for aT b
l : nx − d = 0 line l with normal vector n
P : nx − d = 0 plane P with normal vector n
M matrix
R(x, α), R(y, β ), R(z, γ) elementary 3 × 3 rotation matrices
RT transpose of matrix R
−1
R matrix inverse
q quaternion
k·k vector norm

Kinematics
B, S0 base coordinate system
Si DH-coordinate system i
pi origin of Si (given as vector in base coordinates)
x0 , y0 , z0 coordinate axes of system S0
xi , yi , zi coordinate axes of system Si
ai , αi , θi , di DH-parameters
0M DH-matrix transforming fom S0 to S1
1
i−1 M DH-matrix transforming fom Si−1 to Si
i
li length of a link in a kinematic chain

419
420 E Notation

Forces, Torques and Velocity Kinematics

t joint torque
f force vector
m moment vector
τ magnitude of a joint torque
v linear velocity
ω angular velocity
Jv linear velocity part of a geometric Jacobian
Jω angular velocity part of a geometric Jacobian

Machine Learning

si , s0i slack variables in linear programming

ξi , ξi0 slack variables in quadratic programming
αi , αi0 dual variables in quadratic programming, Lagrange multipliers
ε, C parameters of a support vector machine
F kernel function
∂f
∂x partial derivative of f with respect to variable x
∇ f gradient vector of f
Λ Lagrange function
List of Figures

1.1 Surgical saw mounted to a robotic arm . . . . . . . . . . . . . . 6

1.2 Schematic notation for revolute joints and prismatic
joints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.3 Navigating a surgical drill . . . . . . . . . . . . . . . . . . . . . . . . 8
1.4 Epiphyseolysis. Left images: epiphysial lines of the
femur and x-ray image of epiphyseolysis . . . . . . . . . . . . 9
1.5 Treatment of epiphyseolysis . . . . . . . . . . . . . . . . . . . . . . . 10
1.6 Cortical bone and spongy bone in CT and MRI . . . . . . . 10
1.7 Placement of pedicle screws . . . . . . . . . . . . . . . . . . . . . . 11
1.8 Corrective osteotomy for the femur bone . . . . . . . . . . . . 11
1.9 C-arm x-ray imaging . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.10 Joints of a C-arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.11 Infrared tracking system with two markers . . . . . . . . . . 14
1.12 Radiologic navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
1.13 Five-joint mechanism for stereotaxic neurosurgery. . . . 16
1.14 Stereotaxic navigation in neurosurgery . . . . . . . . . . . . . . 16
1.15 Imaging directions: axial, sagittal, coronal . . . . . . . . . . 18
1.16 Navigated transcranial stimulation . . . . . . . . . . . . . . . . . 19
1.17 Robotic Radiosurgery . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
1.18 Replicating the surgeon’s hand motion with a robotic
interface mounted to the patient couch . . . . . . . . . . . . . . 21
1.19 Reconstruction. 2D projection images taken from a
series of angles and the reconstructed 3D image. . . . . . 22
1.20 Rotating source-detector assembly for CT imaging or
3D imaging with a C-arm. . . . . . . . . . . . . . . . . . . . . . . . . 22

421
422 List of Figures

1.21 Robotic C-arm imaging system for angiography . . . . . . 23

1.22 Surgical microscope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
1.23 Robotic hand for stroke rehabilitation . . . . . . . . . . . . . . . 24
1.24 Joint angle θ4 for the neurosurgical frame in
Figure 1.13 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
1.25 Joint angle θ5 for the frame in Figure 1.13 . . . . . . . . . . 25
1.26 Input data for reconstruction . . . . . . . . . . . . . . . . . . . . . . 28
1.27 Visualization of the ART algorithm. . . . . . . . . . . . . . . . . 29
1.28 Synthetic binary image of a rectangle and a projection
line through the image . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

2.1 Vector p describes the position of the coordinate

system S with respect to the base coordinate system B. 34
2.2 Coordinate system S after a change of orientation. . . . . 34
2.3 Coordinate system S for a surgical saw. . . . . . . . . . . . . . 36
2.4 Drill tip coordinate system. . . . . . . . . . . . . . . . . . . . . . . . 38
2.5 Matrix transformations. The matrix describing
position and orientation of the drill tip with respect to
the base is the product of two matrices B MS and S MS0 . 39
2.6 Transforming the coordinates of a point given with
respect to S. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
2.7 Planar robot with a single revolute joint. . . . . . . . . . . . . 41
2.8 Base coordinate system for the robot in Figure 2.7.
The robot has a single revolute joint. The axis of
rotation of this joint is the z-axis of the base system. . . 42
2.9 Gripper coordinate system for the robot in Figure 2.7. 42
2.10 Intermediate coordinate system S1 for the robot in
Figure 2.7. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
2.11 The intermediate coordinate system S1 rotates with
the robot, (angle θ ). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
2.12 Placement of intermediate coordinate system S1 with
respect to the base coordinate system, depending on
the angle θ of rotation of the robot in Figure 2.7 . . . . . . 44
2.13 Three-joint robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
2.14 Joint coordinate systems for transforming between
base system S0 = (x0 , y0 , z0 ) via Si = (xi , yi , zi ) to the
gripper coordinate system SG = (xG , yG , zG ). . . . . . . . . 47
List of Figures 423

2.15 Placement of the origin Oi of system Si . . . . . . . . . . . . . 51

2.16 Calculating the joint angle θ for reaching a given
point p . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
2.17 Variant of the three-joint robot in Figure 2.13 . . . . . . . . 62
2.18 Hand assembly with three revolute joints. . . . . . . . . . . . 62

3.1 Six-joint robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

3.2 Joint axes for the six-joint robot. . . . . . . . . . . . . . . . . . . . 72
3.3 Coordinate systems for the hand assembly of the
six-joint robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
3.4 Left-shoulder and right-shoulder configurations of
the robot. The position of the tool flange with respect
to the base plate is the same in both cases. . . . . . . . . . . . 75
3.5 Elbow-up and elbow-down configuration of the robot.
In both cases, the robot is in left-shoulder configuration. 75
3.6 Hand configurations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
3.7 Solving for angle θ1 . The origin of the base system
(O) and q are marked (black dots). . . . . . . . . . . . . . . . . . 78
3.8 Solutions for the angle θ1 . The robot has several
ways for reaching the given point q. Figure a shows
the so-called left-shoulder configuration. . . . . . . . . . . . . 78
3.9 Solving for angle θ3 . The distance kqk between
the origin of the base system and the point q only
depends on θ3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
3.10 Seven-joint DLR-Kuka robot. . . . . . . . . . . . . . . . . . . . . . 90
3.11 Schematic of the seven-joint DLR-Kuka robot with
R − PRP − RPR structure. . . . . . . . . . . . . . . . . . . . . . . . . 91
3.12 The null-space of a seven-joint robot, for elbow-up
and elbow-down configurations. . . . . . . . . . . . . . . . . . . . 92
3.13 Elbow position redundancy of the seven-joint robot.
Center points of joints 2,4,6 are in the x0 -z0 -plane. . . . . 92
3.14 Elbow position redundancy of the seven-joint robot.
Center points of joints 2,4,6 are in a plane parallel to
the x0 − y0 -plane. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
3.15 Null-space of the seven-joint robot, for an
elbow-down configuration. . . . . . . . . . . . . . . . . . . . . . . . . 94
424 List of Figures

3.16 Slack value ±∆ for the first joint of the seven-joint

robot. The value of ∆ controls the placement of the
elbow. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
3.17 Eight-joint robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
3.18 The first two coordinate systems S0 , S1 and the
origins O3 to O5 for the forward kinematic analysis
of a C-arm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
3.19 Coordinate systems S2 , S3 , S4 , S5 . . . . . . . . . . . . . . . . . . . . 105
3.20 Offsets a4 and a5 for a non-isocentric C-arm. . . . . . . . . 108
3.21 Target position and orientation for the inverse
analysis of the C-arm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
3.22 Decomposition and projection of vectors. . . . . . . . . . . . 109
3.23 Calculation of θ4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
3.24 Computing θ5 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112
3.25 Computing d3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
3.26 The vectors u⊥v and v⊥u point into the directions of
the offsets. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
3.27 Image stitching (without registration) for a femur bone. 116
3.28 Registration-less navigation with the C-arm. . . . . . . . . 117
3.29 Vector chain for the linear equation system in
Equation 3.142. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
3.30 Center-of-arc kinematic construction . . . . . . . . . . . . . . . 119
3.31 Dexterity volume for the six-joint version of the
seven-joint robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
3.32 Dexterity volume for the seven-joint robot. . . . . . . . . . . 122
3.33 Dexterity volume for the six-joint robot, with joint
ranges enhanced to −150◦ , . . . , +150◦ for the P-joints. 123
3.34 Six-joint Dextor robot with unlimited joint ranges.
Self-collisions are mechanically excluded. . . . . . . . . . . . 124
3.35 Seven-joint version of Dextor. . . . . . . . . . . . . . . . . . . . . . 125

4.1 Two link manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131

4.2 The point O4 is the center of rotation of the C-arm’s
C. a4 and a5 are the offsets. . . . . . . . . . . . . . . . . . . . . . . . 133
4.3 Rotating the C-arm’s C, while keeping the target
point O5 static requires compensating motion with
the two prismatic joints. . . . . . . . . . . . . . . . . . . . . . . . . . . 133
List of Figures 425

4.4 Deriving explicit joint velocity functions d1 (t) and

d3 (t). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134
4.5 Linear velocity v resulting from an angular velocity ω. 147
4.6 Linear velocity v of the point pn , resulting from the
motion of a single joint. . . . . . . . . . . . . . . . . . . . . . . . . . . 148
4.7 Null position and coordinate systems for CT imaging
with the C-arm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158
4.8 Joint velocity (for joint 2) for moving the tool tip
of a two-link manipulator along a line segment, see
Equation 4.34. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
4.9 Two link manipulator for position θ1 = π/2 and
θ2 = −π/2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165

5.1 Registration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169

5.2 Registration of a femur head with C-arm x-ray
images (2D-3D registration) . . . . . . . . . . . . . . . . . . . . . . 170
5.3 Digitally reconstructed radiographs . . . . . . . . . . . . . . . . 172
5.4 Stereo x-ray imaging. Two fixed x-ray sources and
two fixed detectors. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173
5.5 Registration with landmarks . . . . . . . . . . . . . . . . . . . . . . 175
5.6 Registration with surface points . . . . . . . . . . . . . . . . . . . . 176
5.7 Two-dimensional illustration of contour-based
registration. Known camera positions C,C0 . Tangents
touch the contour of the object. . . . . . . . . . . . . . . . . . . . . 178
5.8 Estimated bone position. We refine the estimate
by minimizing the point-line distance between the
original tangents (dashed lines) and the tangent points
a, b, a0 , b0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 179
5.9 Small rotations may move tangent points by large
amounts. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 180
5.10 Abstract example images. 16 pixels, only three
potential gray values. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182
5.11 Abstract example images. An appropriate matching
of gray levels must be computed by the registration
algorithm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182
5.12 Abstract example images. Four pixels, two gray
values (0 and 1). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185
426 List of Figures

5.13 Gray level 0 is mapped to gray level 1 and vice versa.

Here the mutual information I(A, B) also equals one! . . 186
5.14 Image A gives no information on the content of image
B. In this case the mutual information I(A, B) = 0. . . . . 187
5.15 In this example I(A, B) = 0.32. . . . . . . . . . . . . . . . . . . . . 188
5.16 Table for computing the joint distribution pA,B . Place
the number of voxels having gray level a in A and
gray level b in B at the table position with index a, b. . . 189
5.17 Interpolation of the voxel position. . . . . . . . . . . . . . . . . . 190
5.18 Deformation of a grid of equidistant points. . . . . . . . . . 193
5.19 Distortion correction with bilinear interpolation . . . . . . 193
5.20 Interpolation function . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194
5.21 Deforming a thin metal plate. The plate is deformed
by a force pushing downwards at point pi . The
magnitude of the displacement at pi is zi . . . . . . . . . . . . 196
5.22 Calibration disk. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 198
5.23 Bilinear interpolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 199
5.24 Interpolation function . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200
5.25 Hand-Eye-Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . 202
5.26 Hand-Eye-Calibration for an ultrasound camera.
A small lead ball acts as a marker. The marker is
attached to the robot. The marker is in a water tank.
The lead ball is detected in the ultrasound image. . . . . 205
5.27 Projection geometry of a C-arm. The focal length f
is the distance between the source and the detector. . . . 208
5.28 k-d-tree . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 209

6.1 Implant for intertrochanteric femur osteotomy. To

adjust the position of the femur head, a small bone
wedge is removed. The size, position and angle of the
wedge are computed in the planning phase. During
the intervention, a channel is drilled into the femur
neck. The top part of the implant is then placed into
this channel. The shaft of the implant is attached to
the lateral femur shaft with screws. . . . . . . . . . . . . . . . . 220
6.2 The implant must be placed before the cut is made. . . . 220
List of Figures 427

6.3 Forward and inverse planning for intertrochanteric

femur osteotomy. Frontal view onto the right femur.
In this view, a valgus of 15 degrees is specified.
Original posture of the femur (solid) and new posture
(transparent). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 222
6.4 Irradiating a spherical target from multiple directions.
Dose is accumulated in the target region while
healthy tissue is spared. . . . . . . . . . . . . . . . . . . . . . . . . . . 224
6.5 Isocentric treatment. Beam axes cross in a single
point, the treated volume is spherical. . . . . . . . . . . . . . . . 224
6.6 Non-spherical target region T and critical region B. . . . 225
6.7 Sphere packing for non-spherical target shape results
in uneven coverage (hot spots). . . . . . . . . . . . . . . . . . . . . 227
6.8 3D Volume obtained by sweeping a sphere . . . . . . . . . . 228
6.9 Sweeping an isocenter point along a line segment. . . . . 228
6.10 Placement of isocenter points for irregular shapes. . . . . 228
6.11 Beam weighting and arrangements . . . . . . . . . . . . . . . . . 229
6.12 Feasible region for the linear constraints in Equation 6.5231
6.13 Infeasible constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . 232
6.14 Feasible region and objective function . . . . . . . . . . . . . . 233
6.15 Solving a system of linear equations does not
necessarily yield a feasible point . . . . . . . . . . . . . . . . . . . 235
6.16 Maximal cell. Lines are oriented, as indicated by
arrows. The number of lines having the shaded cell in
their interior half-space attains a local maximum. . . . . . 236
6.17 CT slice for sample case with polygon delineating
the target . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 237
6.18 Inverse planning input and output for sample case
in Figure 6.17. (b) Stack of tumor polygons in each
positive slice. (c) 80% isodose curve after computing
the beam weights with linear programming. (d)
overlay of (b) and (c). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 238
6.19 Phantom: photographic film is exposed as the planned
treatment for the case in Figure 6.17 is executed.
(Notice treated region closely matches the input
tumor shape). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 239
428 List of Figures

6.20 Dose-volume histogram for interactive evaluation

of dose distributions. Tumor volume (bold curve),
esophagus (thin dotted line), spine (dashed line) . . . . . . 240
6.21 Beam collimator. The beam is not cylindrical, but a
cone. The radiation source is at the tip of the cone. . . . . 240
6.22 Multi-leaf collimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 241
6.23 Motion connecting treatment beam positions . . . . . . . . . 242
6.24 Motion of organs in soft tissue. The target moves
faster than a critical structure in the vicinity of the
target. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243
6.25 Four-dimensional CT . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243

7.1 Tracking a moving target in external beam radiation

therapy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 251
7.2 Motion correlation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 252
7.3 Correlation-based tracking: the bold curve stems
from the real-time sensor (here optical tracking).
A series of points on the dashed curve are known
(here: x-ray images), but these points are in the past,
since we need segmentation time. The bold curve
and the dashed curve (here: ground truth and target
position) are in correlation. Our goal is to estimate
the unknown dashed curve from this data. . . . . . . . . . . . 253
7.4 Data points from optical tracking and x-ray imaging.
Both types of sensor data points have time stamps.
This allows for establishing the correspondences
between the data points. . . . . . . . . . . . . . . . . . . . . . . . . . . 255
7.5 Line Regression . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 256
7.6 Hystersis: inhale path of target differs from exhale path.258
7.7 Regression function for a cloud of points . . . . . . . . . . . . 259
7.8 Separating two clouds of points . . . . . . . . . . . . . . . . . . . . 260
7.9 Separator line for the classification problem in
Equation 7.10 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 262
7.10 Maximum margin separator line for the classification
problem in Equation 7.10 . . . . . . . . . . . . . . . . . . . . . . . . . 263
List of Figures 429

7.11 Random sample points in interval [-2,2] by [-2,2].

Positive samples marked as ’+’, negative samples
marked as dots. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 270
7.12 Sample points from Figure 7.11 mapped under F :
(x1 , x2 )T → (x̃1 , x̃2 , x̃3 , x̃4 )T = (x1 x1 , x1 x2 , x2 x1 , x2 x2 )T .
Cross-sectional (x̃1 , x̃2 , x̃4 )-subspace. . . . . . . . . . . . . . . . 270
7.13 Motion correlation measured with two-plane
fluoroscopy. The x-axis shows the motion of an
optical marker along a single coordinate in space.
The y-axis shows the motion of an internal marker,
again along a single coordinate. We fit a line to
the training data points (large dots). The small dots
represent the data points from fluoroscopy. The effect
of hysteresis is clearly visible. . . . . . . . . . . . . . . . . . . . . . 274
7.14 Support vector regression for motion correlation . . . . . . 275
7.15 Representing hysteresis by a correlation model
consisting of two polynomial segments . . . . . . . . . . . . . 275
7.16 Motion traces for the data in Figure 7.13. Top row:
polynomial fitting. Bottom row: support vector
regression. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 276
7.17 Validating motion correlation with ultrasound. A
pressure sensor measures the force applied to the skin
surface of the patient. The robot adjusts the position
of the camera accordingly. . . . . . . . . . . . . . . . . . . . . . . . . 276
7.18 Phantom set-up for latency calibration. . . . . . . . . . . . . . 277
7.19 Latency measurement with two robots . . . . . . . . . . . . . . 277
7.20 Double correlation. The motion of the rib cage
is detected by digitally reconstructed radiographs
(DRRs). Before treatment the target is marked in a
series of CT scans, showing distinct respiratory states
(4D CT). DRRs are computed from a 4D CT data
set. Live shots are matched to the DRRs to detect the
current respiratory state, and compute ground truth
location of the target. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 278
7.21 Level curves of the function f (x, y) = −x2 − y2 . . . . . . 282

8.1 Signal Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 291

430 List of Figures

8.2 Prediction horizon . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 292

8.3 Respiratory signal (measured with infrared tracking) . . 293
8.4 Cardiac motion and respiratory motion . . . . . . . . . . . . . . 293
8.5 Prediction error (dotted curve) . . . . . . . . . . . . . . . . . . . . . 295
8.6 MULIN predictor for a motion signal . . . . . . . . . . . . . . . 297
8.7 Gradient descent . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 299
8.8 Respiratory motion curve recorded with an infrared
tracking system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 302
8.9 Wavelet decomposition for an input signal . . . . . . . . . . . 306
8.10 Histograms for evaluating predictor output . . . . . . . . . . 315
8.11 Grid search in parameter space for the wLMS algorithm316

9.1 Motion Replication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 326

9.2 Tremor data recorded with infrared tracking . . . . . . . . . 327
9.3 Wavelet decomposition of tremor data, original signal
in the top row, recorded with infrared tracking. . . . . . . . 329
9.4 Force sensing at the end effector . . . . . . . . . . . . . . . . . . . 330
9.5 Five-joint kinematics for a cable-driven master robot
arm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 331
9.6 Definition of joint torque. A stick is attached to a
base with a revolute joint (joint angle θ1 ). A force
vector f acts at the tip of a stick. The force applied
pushes the stick towards the y-axis. . . . . . . . . . . . . . . . . . 331
9.7 Joint torques for a two-link manipulator . . . . . . . . . . . . . 332
9.8 Joint torque . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 336
9.9 Non-planar force . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 336
9.10 Proof of Theorem 9.1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 337
9.11 Joint torque T resulting from an effector torque m . . . . 340

10.1 Five-joint kinematics for a LINAC radiosurgery

system. The dashed arrow shows the axis of the
gantry rotation. The patient couch has three built-in
translation axes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 348
10.2 Total hip replacement surgery . . . . . . . . . . . . . . . . . . . . . 351
10.3 Cutting planes for knee replacement surgery . . . . . . . . . 351
10.4 Femur condyles and free-from implant . . . . . . . . . . . . . . 351
10.5 Laparoscopy: reversal of motion direction at the skin
incision point. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353
List of Figures 431

10.6 Maze procedure for treating atrial fibrillation. . . . . . . . . 354

11.1 Exoskeleton and mobile cart for gait training . . . . . . . . 364

11.2 Speller matrix [7]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 366
11.3 Targeted Muscle Re-Innervation (TMR) . . . . . . . . . . . . . 368
11.4 Deep brain stimulation . . . . . . . . . . . . . . . . . . . . . . . . . . . 370
11.5 Needle Steering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 371
11.6 Needle with bevel tip . . . . . . . . . . . . . . . . . . . . . . . . . . . . 371
11.7 Needle kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 373

A.1 Three-camera system for infrared tracking. . . . . . . . . . . 381

A.2 Motor cortex mapping with fMRI (left), and robotic
TMS (right). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 382
A.3 Magnetization curve for supraparamegnetic iron
oxide particles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 383

B.1 Regression with dual quadratic programming, result

for two points x1 = 1, x2 = 2, y1 = 1, y2 = 2 . . . . . . . . . . 393
B.2 Kinematics for needle steering with two needles . . . . . . 396
432 List of Figures
List of Tables

2.1 DH-table . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
2.2 DH-table for the three-joint robot . . . . . . . . . . . . . . . . . . 53

3.1 DH-table for the DLR-Kuka robot (Figure 3.10) . . . . . 90

3.2 DH-Parameters for the C-arm . . . . . . . . . . . . . . . . . . . . . 106
3.3 Link parameters (in mm) and joint ranges the
seven-joint robot in the example . . . . . . . . . . . . . . . . . . . 121

5.1 grid points and shift values for Figure 5.23 and
Figure 5.24. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 199

8.1 Wavelet decomposition with two bands . . . . . . . . . . . . . 305

8.2 Wavelet decomposition with three bands . . . . . . . . . . . . 305
8.3 Results of relative RMS errors obtained with different
prediction methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 315

9.1 Maximum joint torques (in Nm) for a force vector f

of 50 N acting at the effector. Positions of the effector
ranging over a point grid in the work space, and
directions of f defined as grid points on a hemisphere. 341

C.1 DH-table for the six-joint elbow manipulator with

spherical wrist . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 399

D.1 DH-table for the seven-joint DLR-Kuka robot . . . . . . . . 409

433
434 List of Tables
Index

3 × 3 orientation matrix, 136, axial, 17, 58

137
4 × 4 matrix, 33, 35, 95, 138 base coordinate system, 7, 38,
B0 -field, 378 334
P-joint, 63, 89 base vector, 33
R-joint, 63, 89 beam weights, 228
k − d-tree, 177 bilinear interpolation, 193
à trous wavelet decomposition, bimodal histogram, 201
321 binning, 192
2D-3D registration, 171 biopsy, 17, 355
3D printer, 363 bone wedge, 220
4D CT, 278, 379 brain electrode, 5, 363
4D imaging, 115 brain-computer interface, 5, 25
breathing pattern, 253
acoustic impedance, 379
angiography, 23 C-arm, 12, 58, 103, 104, 157,
angle, 33 171, 208, 221, 353, 377
angular velocity, 146 calibration, 117, 198, 273, 378
angulation, 104 cardiac motion, 115, 293, 313
arrangement, 228 cardiac surgery, 330, 378
ART algorithm, 28, 205, 257 cartesian control, 115
atan2, 54, 70, 81, 103, 387 catheter, 354, 369
atrial fibrillation, 354 center-of-arc kinematics, 119
auditory brain stem implant, CI, 363
363 cochlea implant, 363

435
436 INDEX

computed tomography, 377 DH-table, 52, 71, 89, 103,

condyle, 350 106, 399, 409
configuration, 74 diffusion tensor imaging, 377
configuration space, 91, 92 digitally reconstructed
conformality, 226 radiograph, 171
constraint, 233, 311 distortion, 378, 379
contour-based registration, 177 dopamine, 369
convex, 229 Doppler ultrosound, 379
coordinate system, 12, 33 dose attenuation, 237
coronal, 17, 58 dose distribution, 239
correlation model, 253 dose-volume histogram, 239
correlation-based tracking, double correlation, 278
253 drill, 38
cortical bone, 8, 181 DRR, 171, 278, 349
critical structure, 223 DTI, 377
cross correlation, 200, 349 dual linear program, 244
cross-product, 332 dual quadratic program, 266,
CT, 7, 115, 223, 350, 377 271
cubic spline interpolation, 194 dualization, 266
Cyberknife, 358
cylinder collimator, 239, 349 ear drum, 363
ECoG, 368
da Vinci system, 352 EEG, 365
Daubechies wavelet, 319 effector, 43, 146
DBS, 370 effector coordinate system,
decomposition, 302 335
deep brain stimulation, 370 eight-joint robot, 103
Denavit-Hartenberg notation, elastic registration, 192, 200
49 elbow, 76
denoising, 318 elbow manipulator, 46, 63
dental implant, 8 elbow-down configuration, 74
depression, 371 elbow-up configuration, 74
detector, 21 electro-oculogram, 367
determinant, 141 electrocorticography, 368
dexterity, 162, 326, 330 electrode, 17, 365, 370
DH-matrix, 52, 103, 409 electroencephalography, 365
DH-rules, 50, 147 electromyography, 365
INDEX 437

elementary rotation, 45, 52, four-dimensional planning,

139, 386 241
elementary translation, 52 Fourier decomposition, 298
EMG, 367 frame, 35
endoscope, 353 frame of reference, 33
endoscopic imaging, 356 frequency band, 303
enthropy, 186 functional magnetic resonance
EOG, 367 tomography, 377
epilepsy, 371
epiphysis, 8 GABA, 369
error curve, 294 GammaKnife, 348
Euler angles, 60 Gaussian elimination, 308
Gaussian least squares, 194
exhalation, 252, 311
geometric Jacobian, 146, 341,
exoskeleton, 5, 24, 363
416
glutamate, 369
fast-lane method, 313
gold landmark, 254
feasibility test, 232
gold marker, 273
feasible region, 233
gradient descent, 299
feature space, 271
gradient field, 378
femur bone, 7, 171
gravity, 330
femur head, 116, 171, 220
gray level histogram, 188
femur neck isthmus, 221
gripper, 41
femur shaft, 221
gripper coordinate system, 42
fiducial, 15
ground truth, 253
field-free point, 383
flexion, 37 Haar transform, 318
fluoroscopy, 273, 377 Haar Wavelet, 317
fMRI, 369, 377, 382 half-plane, 236
focal length, 208 hand, 76
force feedback, 330 hand configuration, 76
force sensing, 330 hand flip, 74
forces, 329 hand-eye calibration, 14, 201
forward kinematic analysis, 45 heart surgery, 19
forward kinematics, 55, 104, hip replacement surgery, 350
373 homogeneous 4 × 4 matrix, 35
forward planning, 222 homogeneous matrix, 59
four-dimensional CT, 241 Hounsfield, 379
438 INDEX

hyperplane, 260 Kalman filter, 314

hysteresis, 258, 274 kernel function, 197, 268, 273
kinematic chain, 43
ICP, 177 kinematics, 67, 326, 348, 371,
image deformation, 192 396
image stitching, 115, 213 knee replacement, 350
image-guided, 355
implant, 220, 352 label, 229
inequality, 262 Lagrange function, 266, 394
infrared marker, 14 Lagrange multiplier, 266, 395
infrared tracking, 14, 377, 380 landmark, 175, 193
inhalation, 252, 311 laparoscopy, 352, 380
intensity-based registration, latency, 20, 273, 274
181 least means square prediction,
interpolation, 253 298
intertrochanteric femur left-handed coordinate system,
osteotomy, 220 58
inverse analysis, 116 left-shoulder, 74
inverse kinematics, 55, 67, LINAC system, 348
141, 373 linear accelerator, 20
inverse planning, 222 linear equation system, 14,
inverse quaternion, 56 234, 307
isocentric, 107 linear interpolation, 255
isocentric treatment, 224, 226 linear programming, 20, 228,
iterative-closest-point method, 262, 312, 349
177 linear velocity, 146
linearized rotation matrix, 180
Jacobi-matrix, 137, 335 link, 41
Jacobian, 138 linkage, 41, 55, 59
jitter, 314 local minimum, 259
joint angle, 41 localizer, 15
joint distribution, 189 long bone, 115
joint histogram, 189
joint increment, 138 machine learning, 303
joint space, 91, 166 magnetic particle imaging,
joint torque, 331, 335, 341 377, 383
joint velocity, 135 magnetic susceptibility, 378
jointed mechanism, 5 magnetic tracking, 377
INDEX 439

margin, 263, 391 nanoparticle, 383

marker, 12, 202, 254, 349, 352 navigation, 5
Matlab, 272, 392 nearest neighbor interpolation,
matrix product, 39 190
matrix rank, 162 needle steering, 370, 396
matrix transformation, 38 neural prostheses, 363
maximal cell, 237 neuro-prosthetics, 24
maximum margin separator, neurology, 18, 89, 363
264, 391 neurosurgery, 5, 15, 347, 355
Maze procedure, 354 neurotransmitter, 369
MEA, 368, 372 nLMS, 317
medical imaging, 5 normal equation, 257, 273
MEP, 383 null position, 41
MI algorithm, 183 null-space, 92
micro-dialysis, 369 objective function, 233
microsurgery, 22 obsessive compulsive disorder,
minimal cell, 237 371
minimally invasive surgery, 5 OCM, 370, 377
motion compensation, 18, 326, OCT, 14, 370, 372, 377
355 online regression, 311
motion correlation, 252, 274, ophtalomolgy, 22
291, 349 optical coherence microscopy,
motion learning, 19 377
motion planning, 241 optical coherence tomography,
motion prediction, 20, 349 377, 382
motion replication, 5, 325, 355 optical tracking, 12, 202, 203,
motion scaling, 5, 325 221, 352, 377
motion tracking, 19 optimization, 225
motor evoked potential, 383 orbital rotation, 104
moving target, 291 orientation, 12, 33, 37, 55
MPI, 377 orientation matrix, 136
MR, 14, 223, 377 origin, 38
MULIN, 313 orthogonal coordinate system,
MULIN algorithm, 294 59
multi-leaf collimator, 239, 320 orthogonal matrix, 59, 141
multielectrode array, 368 orthopedic surgery, 19, 378
mutual information, 183 osteotomy, 9
440 INDEX

osteotomy plane, 221 quadratic programming, 259,

over-determined, 308 264
quaternion, 12, 55, 204
panorama image, 115
parallel kinematics, 120 radiologic navigation, 12, 170,
Parkinson, 370 208, 377
partial derivative, 138 radiosurgery, 19, 89, 320
passive arm, 364 random variable, 183
passive joint, 6 reconstruction, 171, 378
PAT, 377 registration, 14, 18, 170, 253,
pedicle, 8 349
periodic motion, 326 registration-less navigation,
PET, 377 116
phantom object, 274 regression, 257, 292, 308
photo-acoustic tomography, regular matrix, 161
377 rehabilitation, 5, 363
physiotherapy, 363 rehabilitation robotics, 24
pitch joint, 63, 89 relative RMS, 313
planar robot, 41 replicator, 327
planning, 355 respiratory cycle, 252
polygon, 226, 228 respiratory motion, 115, 252,
polytope, 232 292
position, 12, 33 revolute joint, 6, 12, 147, 337
positron emission tomography, rib cage, 278
377 right-handed coordinate
prediction, 292 system, 58, 222
prediction horizon, 292 right-shoulder, 74
predictor, 307 RMS error, 313
primal quadratic program, 266 Robodoc system, 350
prismatic joint, 6, 12, 135, 157 robot, 5, 294, 325, 331
projection geometry, 208 robotic C-arm, 12
prostheses, 363 robotic imaging, 352
prosthesis, 5 robotic radiosurgery, 20, 349
prosthetics, 5 roll joint, 63, 89
pulmonary vein, 292
pulsation, 258, 326 sagittal, 17, 58
scale, 309
quadprog(), 272, 394 separating line, 262, 269
INDEX 441

serial kinematic chain, 49 surgical microscope, 22, 120

serotonin, 369 surgical saw, 6, 36
shoulder, 76 system of inequalities, 230
single photon emission
computed tomography, TCP, 63
377 template matching, 200
singular, 145 thin-plate splines, 195
singular configuration, 162 three-joint robot, 46, 67, 363
singular matrix, 161 threshold value, 237
singularity, 162 time lag, 320, 330
six-joint robot, 70 time step, 292
skew-symmetric matrix, 141, time-stamp, 253
175 tool, 43, 334
slack variable, 235, 265 tool center point, 63
smoothing, 302 tool flange, 63, 103
solid angle, 121 TPS, 197
SPECT, 377 tracking, 252
spectroscopy, 365 training, 292, 308, 326
speller matrix, 366 trajectory, 166
spherical wrist, 63, 89, 94 transcranial magnetic
stimulation, 18, 383
spine surgery, 8
treatment planning, 20, 219,
spirometer, 258
349
spongy bone, 8, 181
tremor filtering, 5, 325
stereo x-ray imaging, 253, 278
triangulation, 380
stereotaxic, 350, 355, 371
trigonometric addition
stereotaxic body radiosurgery,
formula, 81, 387
349
trilinear interpolation, 190
stereotaxic frame, 15, 348
trilinear partial volume
stereotaxic navigation, 15,
interpolation, 190
170, 347
tumor, 8, 252, 294, 349
stereotaxic radiosurgery, 347
two-link manipulator, 332
stroke, 5, 24, 363
support vector, 258, 310, 325 ultrasound, 14, 274, 377
support vector regression, 258, unit quaternion, 56
328 urologic surgery, 352
surgical drill, 7
surgical instrument, 325 valgus, 37, 222
442 INDEX

varus, 37 wavelet band, 305

velocity function, 141 wLMS, 301
vertebra, 8
via-point, 166 x-ray, 7, 13, 171, 208, 221,
virtual wall, 355 245, 278, 352, 377
virtual work, 343 yaw pitch roll, 37, 60, 61, 89
YPR, 37, 55, 386
warm-start method, 312
wavelet, 298, 302, 312, 328 Zeus, 358