FORWARD KINEMATICS AND GEOMETRIC CONTROL OF A
MEDICAL ROBOT
Application to Dental Implantation
Richard Chaumont, Eric Vasselin and Dimitri Lefebvre
GREAH of LE HAVRE University, 25 rue Philippe LEBON. 76058 LE HAVRE Cede, France
Keywords: Surgical robotics, geometric modelling, inverse geometric modelling, dynamic modelling, non linear
system, identification.
Abstract: Recently, robotics has found a new field of research in surgery in which it is used as an assistant of the
surgeon in order to promote less traumatic surgery and minimal incision of soft tissue. In accordance with
the requirements of dental surgeons, we offer a robotic system dedicated to dental implants. A dental
implant is a mechanical device fixed into the patient’s jaw. It is used to replace a single tooth or a set of
missing teeth. Fitting the implant is a difficult operation that requires great accuracy. This work concerns
the prototype of a medical robot. Forward and inverse kinematics as dynamics are considered in order to
drive a control algorithm which is as accurate and safe as possible.
1 INTRODUCTION
Computer-assisted dental implantology is a
multidisciplinary and complex topic that includes
medical imagery, robotics and computer vision
(Langlotz, & al, 2000 ; Nikou & al, 2000). The
fitting of a dental implant is currently the only
technique suitable to permanently restore the teeth.
For this purpose, specific surgery has been recently
developed. Such operations require great accuracy.
Moreover, the spread of this type of surgery justifies
the extension and use of new techniques (Taylor,
1994). This research and development work focuses
on the medical robotics applied to dental
implantology. The main contributions of this article
are to discuss the forward kinematics and kinematics
uncertainties, and also to provide a geometric
control for the orientation of the drill.
2 MEDICAL ROBOTICS
For the last twenty years, new technologies have
been used to improve surgical operations so that
medical research and engineering improvements are
closely linked today. On one hand, data processing,
computer vision and medical imaging are used in an
intensive way in operation rooms. On the other
hand, the three principles of robotics-perception,
reasoning and action - have been adapted for
medical and surgery issues (Lavallee & al, 1995).
The main goal is to bring together the fundamental
principles of robotics and computer vision in order
to assist the surgeon in daily therapeutic operations.
The aims of medical robotics are to provide less
traumatic investigation systems, to provide
simulation tools, and, finally, to provide tools that
are easier and more flexible to use.
The aim of dental implantology is to use bones
and implants in order to provide prosthetic support.
The main advantage in comparison with a
conventional prosthesis is that dental implantology
doesn't mutilate healthy teeth. At the time tooth
extraction is completed, the fitting of a dental
implant allows the consolidation of the prostheses.
The main difficulty is to place the implants
correctly. That is the reason why conventional
prosthesis is still prefered to dental implantology in
many cases.
Dental implants guarantee the patient better
comfort but can also reduce overall cost owing to
their longevity and lack of inherent complications in
comparison with classic prostheses. For difficult
cases (completely toothless patients, weak density
bones, multiple implants, etc), dental surgeons are
confronted with a complex operation. Over the
years, the main difficulty concerned the integration
of the bone-implant.
This problem has been solved by technical
improvements and surgical advances (equipment,
implant shapes, surgical protocol, etc). According to
110
Chaumont R., Vasselin E. and Lefebvre D. (2007).
FORWARD KINEMATICS AND GEOMETRIC CONTROL OF A MEDICAL ROBOT - Application to Dental Implantation.
In Proceedings of the Fourth International Conference on Informatics in Control, Automation and Robotics, pages 110-115
DOI: 10.5220/0001652501100115
Copyright
c
 SciTePress

the opinion of many clinicians, the difficulty is
henceforth to improve fitting techniques. The
position and orientation of implants must take into
account biomechanical and anatomical constraints
(Dutreuil, 2001) involving three main criteria:
mastication, phonetics and aesthetics. In particular,
the problems to be solved are :
• How to adjust the implant position in the
correct axis.
• How to optimize the relative position of
two adjacent implants.
• How to optimize the implant position
according to the bone density.
• How to conduct minimal incision of soft
tissue.
Our answer to these questions is image guided
surgery (Etienne & al, 2000). This solution uses an
optical navigation system with absolute positioning
in order to obtain position and orientation of the
surgeon's tool in real time with respect to the
patient’s movements. The operation is planned on
the basis of scanner data or x-ray images for simple
clinical cases.
Figure 1: Navigation system.
The technique consists in initializing the
superimposing of patient data and data derived form
a set of specific points attached to the patient’s jaw
(Granger, 2003). The patient’s jaw is then analyzed
in real time by the navigation system. The Dental
View navigation machine guides the surgeon via the
image during the operative phase.
However, clinical tests show that supplementary
assistance is necessary to help the surgeon during
the drilling phase in order to fulfil precision
requirements. For this purpose, we developed a
surgical robot which controls orientation during the
drilling phase.
Our robot is a semi-active mechanical device. It
has a passive arm and a motorized wrist with three
degrees of freedom (dof) that are not convergent (i.e.
not a spherical wrist). The basis is passive, that is to
say it is not motorized and can be manipulated by
the surgeon like an instrument. The aim of the
controller is to guide the surgeon so that it will
respect the scheduled orientation.
3 FORWARD KINEMATICS
Drill orientation is characterized by 3 dof RotY,
RotX and RotZ and depends also on contra angle ca
(Fig. 3). The structure is represented in Fig.2.
Figure 2: Robot axis.
3.1 Notations
We use homogeneous transformations to describe
position and orientation from one link to another.
Let us consider the matrix M :
R
T
M
PQ
⎡
⎤
=
⎢
⎥
⎣
⎦
(1)
with P = [0 0 0], Q = [1] is a homothetic coefficient
equal to 1 (orthogonal transformation); R is a
orthogonal rotation matrix; and T is a translation
matrix.
For simplicity, calculations are not given in
detail. Notations will be represented above Fig.3.
Lxo, Lyo, Lzo : distance x, y, z between computer
vision coordinate frame and 4
th
joint coordinate
frame.
Lx5, Lz6 : distance x, z between 4
th
joint and 5
th
joint.
Lyca, Lzca : distance y, z between 5
th
joint and
effector.
Lztool : drill length.
θ
4
, θ
5
, θ
6
: wrist joint variables.
Ca: contra angle.
ε
i
: orthogonal uncertainty between joints.
l
i
: length uncertainty between links.
FORWARD KINEMATICS AND GEOMETRIC CONTROL OF A MEDICAL ROBOT - Application to Dental
Implantation
111

Figure 3: Robot parameters and axes.
3.2 Kinematic Uncertainties
First, we are going to determine the effector position
in the ideal case, without considering uncertainties
related to length and orthogonality links. We use
homogeneous transformations to change the
coordinate frame attached to a joint to the coordinate
frame attached to the next one. We obtain 6 matrices
that change the coordinate frames according to (2) :
100
010
1
001
000 1
Lxo
Lyo
A
Lzo
⎛⎞
⎜⎟
⎜⎟
=
⎜⎟
⎜⎟
⎝⎠
cos 0 sin 0
44
0100
2
sin 0 cos 0
44
0001
A
θθ
θθ
⎛⎞
⎜⎟
⎜⎟
=
⎜⎟
−
⎜⎟
⎝⎠
10 0 5
0cos sin 0
55
3
0sin cos 0
55
00 0 1
Lx
A
θθ
θθ
⎛
⎞
⎜
⎟
−
⎜
⎟
=
⎜
⎟
⎜
⎟
⎝
⎠
cos si n 0 0
66
sin cos 0 0
66
4
0016
0001
A
Lz
θθ
θθ
−
⎛
⎞
⎜
⎟
⎜
⎟
=
⎜
⎟
⎜
⎟
⎝
⎠
10 0 0
0 c os( ) si n( ) 0
5
0sin() cos() 0
00 0 1
ca ca
A
ca ca
⎛
⎞
⎜
⎟
−
⎜
⎟
=
⎜
⎟
⎜
⎟
⎝
⎠
100 0
010
6
001
000 1
Lyca
A
Lzca
⎛⎞
⎜⎟
⎜⎟
=
⎜⎟
⎜⎟
⎝⎠
(2)
Ideal position in flag coordinate frame results
from the preceding matrices :
0 123456 4
() ()
ideal
VRAAAAAAVR=××××××
(3)
with V(R
4
) = [0 0 1 0]
T
to get effector orientation for
the “z” axis and V(R
4
) = [0 0 0 1]
T
to get effector
position.
⎟
⎟
⎟
⎟
⎟
⎠
⎞
⎜
⎜
⎜
⎜
⎜
⎝
⎛
±
±
±
=
1000
100
010
001
'
1
zo
yo
xo
lLzo
lLyo
lLxo
A
10 0 0cos 0sin0cos sin 00
44 22
0 cos sin 0 0 1 0 0 sin cos 0 0
11 22
'
2
0sin cos 0 sin 0cos 0 0 0 10
11 4 4
00 0 1 0001 0 0 01
A
θθ εε
εε εε
εε θ θ
±−±
⎛⎞⎛⎞⎛ ⎞
⎜⎟⎜⎟⎜ ⎟
±−± ± ±
⎜⎟⎜⎟⎜ ⎟
=××
⎜⎟⎜⎟⎜ ⎟
±± −
⎜⎟⎜⎟⎜ ⎟
⎝⎠⎝⎠⎝ ⎠
1 0 0 5 cos 0 sin 0 cos sin 0 0
53 3 44
0 cos sin 0 0 1 0 0 sin cos 0 0
55 4 4
'
3
0 sin cos 0 sin 0 cos 0 0 0 1 0
55 3 3
00 0 1 0 0 0 1 0 0 01
Lx l
x
A
εε εε
θθ ε ε
θθ ε ε
±± ± ±−±
⎛⎞⎛⎞⎛⎞
⎜⎟⎜⎟⎜⎟
−±±
⎜⎟⎜⎟⎜⎟
=××
⎜⎟⎜⎟⎜⎟
−± ±
⎜⎟⎜⎟⎜⎟
⎝⎠⎝⎠⎝⎠
10 0 0cos 0sin 0cos sin0 0
6666
0cos sin 0 0100sincos00
55 66
'
4
0 sin cos 0 sin 0 cos 0 0 0 1 6
55 6 6 6
00 0 1 0 001 0 00 1
A
Lz l
z
εεθθ
εε θθ
εε ε ε
±± −
⎛⎞⎛⎞⎛ ⎞
⎜⎟⎜⎟⎜ ⎟
±−±
⎜⎟⎜⎟⎜ ⎟
=××
⎜⎟⎜⎟⎜ ⎟
±± −± ± ±
⎜⎟⎜⎟⎜ ⎟
⎝⎠⎝⎠⎝ ⎠
100 0
010
'
5
001
000 1
Lyca l
yca
A
Lzca l
z
ca
⎛⎞
⎜⎟
±
⎜⎟
=
⎜⎟
±
⎜⎟
⎜⎟
⎝⎠
1 0 0 0 cos 0 sin 0 cos sin 0 0
55 66
0 cos( ) sin( ) 0 0 1 0 0 sin cos 0 0
'
66
6
0sin() cos() 0 sin 0cos 0 0 0 10
55
00 01 0001 0 001
ca ca
A
ca ca
εε εε
εε
εε
⎛⎞⎛⎞⎛⎞
±± ±−±
⎜⎟⎜⎟⎜⎟
−±±
⎜⎟⎜⎟⎜⎟
=××
⎜⎟⎜⎟⎜⎟
−± ±
⎜⎟⎜⎟⎜⎟
⎜⎟⎜⎟⎜⎟
⎝⎠⎝⎠⎝⎠
(4)
We will model actual position and orientation of
the effector by considering uncertainties relating to
link length (maximum tolerance of 0.1 mm) and also
of frame orthogonal uncertainties (maximum
tolerance 0.1 degree). Because of digital encoder
resolution (200 000 points per revolution) and of
gear reduction encoders uncertainties are negligible.
Moreover, we only consider uncertainties that
concern the robot wrist (the arm is a passive
mechanical structure). For every dof two uncertainty
matrices are added according to the axes that are not
articulated. Equation (4) is obtained as a
consequence.
Actual effector position is given by (5):
0 123456 4
() ' ' ' ' ' ' ()
actual
VRAAAAAAVR
=
××××××
(5)
Figure 4: Magnitude of error function for θ
4
and θ
5
.
Figure 5: Magnitude of error function for θ
4
and θ
6
.
ICINCO 2007 - International Conference on Informatics in Control, Automation and Robotics
112

Figure 6: Magnitude of error function for θ
5
and θ
6
.
From (4) and (5), we know the ideal position and
orientation of the wrist as well as its actual position
and orientation which take kinematic uncertainties
into account. Therefore, we can obtain the position
and orientation errors :
000
() () ()
actual ideal
E
RV RV R=−
(6)
Figs. 4 to 6 represent the maximal uncertainty
magnitude (millimeter) generated by mechanical and
assembly tolerances. One observes that :
• Uncertainty magnitude is always superior
to 1 millimeter,
• Uncertainty magnitude can reach 1.4
millimeters for particular link positions.
In order to fulfil precision requirements
uncertainties must be lower than 1 millimeter.
Therefore, it is necessary to calibrate the robot wrist.
4 DYNAMICS
In this section we will propose a dynamic model for
the axis of the robot. The axis control architecture is
presented in Fig.7. Technical caracteristics of the
electromechanical and electronic devices can be
found in (Chaumont & al, 2006).
Figure 7: Axis control structure.
4.1 Identification of Electromechanical
Device
Closed loop identification for electromechanical
device is proposed in this section (Richalet, 1998).
System output is the velocity and system input is the
current. The robot axis has been placed so that
inertia moment can be considered as constant
whatever the orientation.
The time response (Fig. 8) presents a
dissymmetry between the current generating the
acceleration in comparison with the current
generating the deceleration.
Figure 8: Protocol signature.
Figure 9: Hysteresis system.
The process is non linear. Fig. 9 represents
input/output signature with a dead zone and a
histereses. The electromechanical transfer function
is represented in Fig.13.
4.2 Identification of Electronic Device
Electronic device input is the desired current and
output is the actual current. It represents the
electronic system part that is composed of the PWM
and its controller.
Identification is achieved in closed loop. A
survey of electronic control shows us that the
transfer function is a second order overshoot with a
stable zero.
Figure 10: Protocol signature.
Courant
(
mA
)
Speed (quarter-count/ms)
Consi
g
nes en courant
(
mA
)
Courant (mA)
FORWARD KINEMATICS AND GEOMETRIC CONTROL OF A MEDICAL ROBOT - Application to Dental
Implantation
113

4.3 Discussion
The model is validated with the same desired
current. Results obtained with the model and with
the system are compared according to velocity
kinetics (Fig. 11) and position (Fig. 12).
Figure 11: Velocity response.
Parameters
ω
0
= 730.97; ξ = 0.4258; K = 0.9955; k = 0.5969 ; T = 0.539
Histereses size: 75 mA Dead zone size: 115 mA.
Figure 12: Position response.
Figure 13: Axis module.
5 CONTROL DESIGN
Forward kinematics is given by equation (2). This
model shows how to determine the effector’s
position and orientation in terms of the joint
variables with a function “f”. Inverse kinematics is
concerned with the problem of finding the joint
variables in terms of effector’s position and
orientation. Our aim is to control the orientation of
the effector. The function “f” corresponds therefore
to V
ideal
(R
0
) expressed by equation (3) with V(R
4
) =
[0 0 1 0]
T
(uncertainties are not considered for
control design).
Matrix V
ideal
(R
0
) leads to equations (7) to (9) :
Vx
0
=sin(ca).cos(θ
4
).sin(θ
6
)–sin(ca).sin(θ
4
).
sin(θ
5
).cos(θ
6
)+sin(θ
4
).cos(θ
5
).cos(ca) (7)
Vy
0
= – cos(θ
5
).cos(θ
6
).sin(ca) – sin(θ
5
).cos(ca) (8)
Vz
0
= –sin(ca).sin(θ
4
).sin(θ
6
) –.cos(θ
4
).sin(θ
5
).
cos(θ
6
) + cos(θ
4
).cos(θ
5
).cos(ca) (9)
From equations (7), (8) and (9), expressions θ
4
,
θ
5
and θ
6
are determined.
)sin(
).sin().cos(
asin
0404
6
ca
VzVx
θ
θ
θ
−
=
,
66
θ
π
θ
−=
a
(10)
1
0
5
1
acos a
yV
+
⎟
⎠
⎞
⎜
⎝
⎛
=
ρ
θ
,
1
0
5 .2acos
1
a
yV
a +
⎟
⎠
⎞
⎜
⎝
⎛
−=
ρ
θ
(11)
with :
()
)(cos)sin().cos(
2
2
61
caca += θρ
π
θ
+
⎟
⎠
⎞
⎜
⎝
⎛
=
)sin().cos(
)cos(
atan
6
1
ca
ca
a
if -cos(θ
6
).sin(ca) < 0
or
⎟
⎠
⎞
⎜
⎝
⎛
=
)sin().cos(
)cos(
atan
6
1
ca
ca
a
θ
if -cos(θ
6
).sin(ca) > 0
2
2
0
5
acos a
yV
b +
⎟
⎠
⎞
⎜
⎝
⎛
=
ρ
θ
,
2
2
0
5 .2acos a
yV
c +
⎟
⎠
⎞
⎜
⎝
⎛
−=
ρ
θ
(12)
with :
()
)(cos)sin().cos(
2
2
62
caca
a
+= θρ
π
θ
+
⎟
⎠
⎞
⎜
⎝
⎛
=
)sin().cos(
)cos(
atan
6
2
ca
ca
a
a
if -cos(θ
6a
).sin(ca) < 0
or
⎟
⎠
⎞
⎜
⎝
⎛
=
)sin().cos(
)cos(
atan
6
2
ca
ca
a
a θ
if -cos(θ
6a
).sin(ca) >0
b
ca
+=
σ
θ
θ
)sin().sin(
acos
6
4
b
ca
aa .2
)sin().sin(
cos
6
4
+−=
σ
θ
θ
b
ca
a
a
b
+=
σ
θ
θ
)sin().sin(
cos
6
4
b
ca
a
a
c
.2
)sin().sin(
cos
6
4
+−=
σ
θ
θ
(13)
with :
2
0
2
0
VzVx += σ
(
)
π −−=
0
0
atan
Vx
Vz
b
if Vx
0
< 0
(
)
0
0
atan
Vx
Vz
b −=
if Vx
0
> 0
Equations (10) to (13) show that angle θ
6
depends on
θ
4
, that angles θ
4
and θ
5
depend on θ
6
. A robot is
"resolvable" if a unique solution exists for equation
θ = f
-1
(x). Our study suggest that our medical
robot’s wrist is not resolvable.
5.1 Reachable Workspace
Analysis of the resulting equations shows that the
determination of the desired current according to a
given set of joint variables θ
4
, θ
5
and θ
6
is difficult.
Courant order (mA)
Process speed (quarter-count/ms)
Model speed (quarter-count/ms)
Position(quater-count)
Consigne position (quater-count)
ICINCO 2007 - International Conference on Informatics in Control, Automation and Robotics
114

Indeed, a given orientation has several solutions for
joint variables. Fig. 14 illustrates these points by
defining the reachable workspace. The method we
propose consists in considering θ
4
as a constant
parameter in order to work out joint variables θ
5
and
θ
6
according to θ
4
. The choice of θ
4
is motivated by
optimization method.
Figure 14: Reachable workspace.
5.2 Control Strategy
The controller must satisfy real time requirements.
The sampling frequency is 20 Hz. Moreover, several
applications such as artificial vision and data
processing take place simultaneously. The controller
inputs are :
• Data from artificial vision and image
superimposition,
• Desired drill orientation,
• Actual angular values returned by digital
encoders.
The algorithm determines the values of V
xo
, V
yo
,
and V
zo
that are vector components in the flag
reference scorers of the robot. It determines θ
5
and
θ
6
with respect to θ
4
and verifies that solutions are in
the reachable workspace. If solutions are outside the
reachable workspace, the algorithm increments θ
4
with 1° and recalculates θ
5
and θ
6
. Incrementation is
repeated until a solution is found in the reachable
workspace.
6 FURTHER WORKS
The protocol used for identification will be applied
in a generic way on the other axes in order to obtain
a dynamic model of the robot. As a consequence, we
will be able to simulate the robot’s dynamic
behaviour and to develop safe and efficient control
design. On the other hand, our work will concern the
following points :
• Accuracy, wrist calibration,
• Study of position / orientation decoupling,
• Trajectory planning, ergonomics.
This medical robot is an invasive and semi-active
system. Therefore, an exhaustive study on reliability
will also be necessary (Dombre, 2001) before
starting clinical simulations and experimentations.
ACKNOWLEDGEMENTS
Authors thank the Doctor DERYCKE, a dental
surgeon specialized in implantology, CHIEF
EXECUTIVE OFFICER of DENTAL VIEW.
REFERENCES
Langlotz F, Berlemann U, Ganz R, Nolte LP., 2000.
Computer-Assisted Periacetabular Osteotomy. Oper.
Techniques in Orthop, vol 10, N° 1, pp. 14-19
Nikou C, Di Gioia A, Blackwell M, Jaramaz B, Kanade
T., 2000. Augmented Reality Imaging Technology for
Orthopaedic Surgery. Oper. Techniques in Orthop, vol
10, N° 1, pp. 82-86.
Taylor R.,1994. An Image Directed Robotic System for
Precise Orthopaedic Surgery. IEEE Trans Robot, 10,
pp. 261-275.
Lavallee S, Sautot P, Traccaz J, P. Cinquin P, Merloz P.,
1995. Computer Assisted Spine Surgery : a technique
for accurate transpedicular screw fixation using CT
data and a 3D optical localizer. Journal of Image
Guided Surgery.
Dutreuil J., 2001. Modélisation 3D et robotique médicale
pour la chirurgie. Thèse, Ecole des Mines de Paris.
Etienne D, Stankoff A, Pennnec X, Granger S, Lacan A,
Derycke R., 2000. A new approach for dental implant
aided surgery. The 4
th
Congresso Panamericano de
Periodontologia, Santiago, Chili.
Granger S., 2003. Une approche statistique multi-échelle
au recalage rigide de surfaces : Application à
l’implantologie dentaire. Thèse, Ecole des Mines de
Paris.
Chaumont R, Vasselin E, Gorka M, Lefebvre D., 2005.
Robot médical pour l’implantologie dentaire :
identification d’un axe, JNRR05. Guidel, pp 304-305.
Richalet J., 1998. Pratique de l’identification. Ed. Hermes.
Dombre E., 2001. Intrinsically safe active robotic systems
for medical applications. 1st IARP/IEEE-RAS Joint
Workshop on Technical Challenge for Dependable
Robots in Human Environments, Seoul, Korea.
A
B
C
FORWARD KINEMATICS AND GEOMETRIC CONTROL OF A MEDICAL ROBOT - Application to Dental
Implantation
115