4

th
Student Conference on Research and Development (SCOReD 2006), Shah Alam, Selangor, MALAYSIA, 27-28 June, 2006


147
Vision Guided Manipulator for Optimal Dynamic
Performance


M. Herman Jamaluddin, M. Azmi Said, Marizan Sulaiman, Chong Shin Horng

Faculty of Electrical Engineering
Kolej Universiti Teknikal Kebangsaan Malaysia,
Locked Bag 1200, Ayer Keroh, 75450 Melaka, MALAYSIA
Tel: +606-555 2216 Fax: +606-555 2222

Abstract - This project develops an algorithm to optimize
the dynamic performance of an industrial robot with aid of
vision system technology. It consists of three major phases;
(1) theoretical, simulation and practical of forward and
inverse kinematics for Fanuc LR Mate 200iB robot to
determine their D-H parameters, (2) optimization of robot
movements, and (3) implementation of practical tasks. The
camera is used as a vision sensor to incorporate an
intelligent algorithm for the recognition of coordinates of
target objects and to guide the robot to perform certain
tasks. The optimization process involves the control of two
parameters known as position () and speed () of the 3
main axes of the robot. Its forward and inverse kinematics
problems will be simulated using Robotic tools of Matlab
7.0 and Roboguide V2.3.2. The simulation results will be
compared with the real practical movements. This paper
will describe the technique, calculation and method on how
the first phase was done.

Index terms: Vision-Guided Manipulators, Optimal Control,
Path-Finding, Image Processing, Dynamic Performance.

I. INTRODUCTION
Moving in an unknown environment is an experience for
all animated systems. This is possible because the visual
information data stream holds a large number of useful
parameters that all these system interpret and translate into
actions as well as into behaviors.
It is now well accepted that the use of vision systems
coupled with a robot manipulator can considerably improve
their robustness and efficiency and allow the realization of
complex industrial tasks when a single robot manipulator is
likely to fail. The concept of reconfigurable manipulators has
been recently developed to facilitate the synthesis of
manipulators structures to best suit specific tasks.
In this project, one of the sensors that are currently being
used in controlling robot mechanism is vision. Optimal-based
control strategies will be developed to perform manipulative
actions. This vision guided robot could perform the tasks with
improved efficiency using the optimal-based control
strategies. The advantages of these optimal control strategies
can be seen during performing the tasks with fewer
movements of angles and improved time motion that result in
more efficient usage of energy.
To find the optimal dynamic performance of a robot, it is
important to know the architecture of its kinematics first.
From forward or inverse kinematics analysis of that particular
robot, we will obtain an exact position and orientation for each
of its movements. This will help to find the most suitable
position and orientation to achieve optimal performance.
Figure 1 describe the overall diagram for the system.












CAMERA
MANIPULATOR
Fig. 1: Proposed vision guided manipulators system

II. KINEMATIC ANALYSIS
To develop and demonstrate our approach in the real-
world experiments, the articulated-arm robot with 6 d.o.f.
FANUC LR Mate 200iB equipped with a parallel-jawed
gripper have been used. So, the inverse and forward
kinematics architecture for this robot must be identified first
before studying the dynamic performance.

A. Forward Kinematics
Kinematics is the science of motion which treats motion
without regard to the forces that cause it [1]. Here, a method to
compute the position and orientation of the robots end-
effector relative to the base of the robot or we call it the
forward kinematics, will be described.

4
th
Student Conference on Research and Development (SCOReD 2006), Shah Alam, Selangor, MALAYSIA, 27-28 June, 2006


148

Fig. 2: Link frame assignments

In order to describe the location of each link relatives to
its neighbors we define a frame attached to each link. The link
frames are named by number according to the link to which
they are attached. That is, frame {i} is attached rigidly to link
i. To locate frames on the links, this convention is used: The
-axis of frame {i}, called
i
is coincident with the joint axis i.
The origin of frame {i} is located where the a
i
perpendicular
intersects the joint i axis. X
i
points along a
i
in the direction
from joint i to joint i + 1. Figure 2 shows the location of
frames {i-1} and {i} for a general manipulator.
In the usual case of a revolute joint,
i
is called the joint
variable, and the other three quantities would be fixed link
parameters. For prismatic joints, d
i
is the joint variable and the
other three quantities are fixed link parameters. The definition
of mechanisms by means of these quantities is a convention
usually called the Denavit-Hartenberg (D-H) notation [1].
Furthermore, the following definitions of the link parameters
can be applied:

a
i
= the distance from
i
to
i+1
measured along X
i
;

i
= the angle between
i
and
i+1
measured about X
i
;
d
i
= the distance from X
i-1
to X
i
measured along
i
;

i
= the angle between X
i-1
and X
i
measured about X
i.

A.1 Transformation from i-1 to i

There are four steps to do the transformation from i-1 to i
which are:
(1) Rotation of frame i-1about z
i
-1 by
i
: Rot( z,
i
)
(2) Translation of the new frame along z by d
i
: Trans(0,0,d
i
)
(3) Translation of the new frame along x by a
i
:
Trans(a
i
,0,0)
(4) Rotation of the new frame about x by
i
: Rot(x,a
i
)

The overall transformation is obtained by post
multiplication of individual transformations:

i-1
T
i
= Rot( z,u
i
)Trans(0,0, d
i
)Trans(0,0, a
i
)Rot( x,o
i
) (1)

i-1
T
i
= (2)
(
(
(
(

1 0 0 0
0 1 0 0
0 0 c s
0 0 - c
i i
i i
s
u u
u u
(
(
(
(

1 0 0 0
d 1 0 0
0 0 1 0
0 0 0 1
i
(
(
(
(

1 0 0 0
0 1 0 0
0 0 1 0
a 0 0 1
i
(
(
(
(

1 0 0 0
0 - 0
0 - c 0
0 0 0 1
i i
i i
c s
s
o o
o o

i-1
T
i
= (3)
(
(
(
(

1 0 0 0
c s 0
s s c - c c s
c s s c - c
i i i
i i i i i i i
i i i i i i i
d
a
a s
o o
u o u o u u
u o u o u u

0
T
i
=
0
T
1

1
T
2

2
T
3
.
i-2
T
i-1
i-1
T
i
(4)

A.2 Forward Kinematics Analysis of FANUC











Fig. 3: FANUC LR Mate 200iB workspace













z
1
y
1
x
1
x
2
y
2
z
2
z
3
y
3
x
3
z
4
x
4
5
y
4
z
z
6
y
6
x
6
y
5
x
5
Fig. 4: The coordinate frames of FANUC LR Mate 200iB

The FANUC LR Mate 200iB is a 6 d.o.f manipulator
with nonzero offsets (denoted by the nonzero link lengths a
i
, i
= 1, . . . , 6) at each of the joints, as shown in Figure 4. This
robot has its own limitation on its workspace as shown in
Figure 3. Denavit-Hartenberg (D-H) link frame assignments
are given in accordance with the convention described
previously. The D-H parameters for the FANUC are given in
Table 1. The associated interlink homogeneous transformation
matrices are easily found from the D-H parameter values listed
in Table 1. It is more efficient to numerically compute the
4
th
Student Conference on Research and Development (SCOReD 2006), Shah Alam, Selangor, MALAYSIA, 27-28 June, 2006


149
forward kinematics function
0
T
6
via a link-by-link iteration of
the form
0
T
6
=
0
T
1
.1
T
2
.

2
T
3
, i = 0, , 6 (5)

Furthermore, it is useful to explicitly have the interlink
homogeneous transformations,
i-1
T
i
, that can then be computed
as by products.
TABLE 1: DENAVIT-HARTENBERG PARAMETERS OF THE FANUC
ROBOT
i
i-1
a
i-1
d
i

i
0 0 0 0
0
1 90 150 0
1
2 0 250 0
2
3 90 75 0
3
4 90 0 290
4
5 -90 0 0
5
6 0 0 80
6

According to general transformation equation of D-H
method and the parameters in Table I, the transformation
matrix of frame x
1
y
1
z
1
with respect to frame x
0
y
0
z
0
is
obtained as
0
T
1
= (6)
(
(
(
(

1 0 0 0
0 0 1 0
s 150 c - 0 s
c 150 s 0 c
1 1 1
1 1 1
where s
1
=sin
1
and c
1
=cos
1
The transformation matrix of frame x
2
y
2
z
2
with
respect to frame x
1
y
1
z
1
is obtained as

1
T
2
= (7)
(
(
(
(

1 0 0 0
0 1 0 0
s 250 0 c s
c 50 2 0 - c
2 2 2
2 2 2
s
where s
2
=sin
2
and c
2
=cos
2
The transformation matrix of frame x
3
y
3
z
3
with
respect to frame x
2
- y
2
- z
2
is obtained as
2
T
3
= (8)
(
(
(
(

1 0 0 0
0 0 1 0
s 75 c - 0 s
c 75 s 0 c
3 3 3
3 3 3
where s
3
=sin
3
and c
3
=cos
3
The transformation matrix of frame x
4
y
4
z
4
with
respect to frame x
3
y
3
z
3
is obtained as

3
T
4
= (9)
(
(
(
(

1 0 0 0
290 0 1 0
0 c - 0 s
0 s 0 c
4 4
4 4
where s
4
=sin
4
and c
4
=cos
4
The transformation matrix of frame x
5
y
5
z
5
with
respect to frame x
4
y
4
z
4
is obtained as
4
T
5
= (10)
(
(
(
(

1 0 0 0
0 0 1 0
0 c 0 s
0 s - 0 c
5 5
5 5
where s
5
=sin
5
and c
5
=cos
5
The transformation matrix of frame x
6
y
6
z
6
with
respect to frame x
6
y
6
z
6
is obtained as
5
T
6
= (11)
(
(
(
(

1 0 0 0
80 1 0 0
0 0 c - s
0 0 s - c
6 6
6 6
where s
6
=sin
6
and c
6
=cos
6
By using (6), (7), (8), (9), (10) and (11), considering the initial
position, all = 0, the forward kinematics equation of the
robot can be derived as
0
T
6
=
0
T
1
.1
T
2
.

2
T
3
..3
T
4
.

4
T
5
.5
T
6

0
T
6
= (12)
(
(
(
(

1 0 0 0
370 1 0 0
0 0 1 - 0
475 0 0 1

B. Inverse Kinematics
Inverse Kinematics does the reverse of forward
kinematics. Given the end point of the structure, what angles
do the joints need to be to achieve that end point. It can be
difficult, and there are usually many or infinitely many
solutions. Most of the real systems are under constrained, so
for a given goal position, there could be infinite solutions (i.e.
many different joint configurations could lead to the same
endpoint). The field of robotics has developed many inverse
kinematics systems which, due to their constraints, have
closed-form solutions.
One of the techniques that had been used to find the exact
angle of each joint to reach the goal target is by using the
Jacobian method which described briefly in [1].

B.1 Inverse Kinematics Analysis of Fanuc
After considering the Jacobian method trough the
theoretical calculation based, the analysis which is using the
Matlab Robotics tools were done. To find the inverse
kinematics in Matlab, the function that had been used is q =
ikine(robot, T). This function returns the joint
coordinates for the manipulator described by the object robot
whose end-effector homogeneous transform is given by T.
The complete lists of function regarding this analysis
using Matlab are as such:
%Entering the D-H parameter information for FANUC
L1=link([pi/2 150 0 0 0])
L2=link([0 250 0 0 0])
L3=link([pi/2 75 0 0 0])
L4=link([pi/2 0 0 290 0])
L5=link([4.7124 0 0 0 0])
L6= link([0 0 0 80 0])
%Searching for FANUC Position and Orientation Matrix
fanuc = robot({L1 L2 L3 L4 L5 L6})
%Generate the transform corresponding to a particular joint
coordinate
q=[0 0 0 0 0 0]
%Forward Kinematic
T=fkine(fanuc,q)
T1=transl(0,0,370)*T
%Inverse Kinematic
qi=ikine(fanuc,T1)
4
th
Student Conference on Research and Development (SCOReD 2006), Shah Alam, Selangor, MALAYSIA, 27-28 June, 2006


150
The outputs for that executed function are as below:

L1 =
1.570796 150.000000 0.000000 0.000000 R (std)
L2 =
0.000000 250.000000 0.000000 0.000000 R (std)
L3 =
1.570796 75.000000 0.000000 0.000000 R (std)
L4 =
1.570796 0.000000 0.000000 290.000000 R (std)
L5 =
4.712400 0.000000 0.000000 0.000000 R (std)
L6 =
0.000000 0.000000 0.000000 80.000000 R (std)
fanuc =
noname (6 axis, RRRRRR)
grav = [0.00 0.00 9.81] standard D&H
parameters
alpha A theta D R/P
1.570796 150.000000 0.000000 0.000000 R (std)
0.000000 250.000000 0.000000 0.000000 R (std)
1.570796 75.000000 0.000000 0.000000 R (std)
1.570796 0.000000 0.000000 290.000000 R (std)
4.712400 0.000000 0.000000 0.000000 R (std)
0.000000 0.000000 0.000000 80.000000 R (std)
q =
0 0 0 0 0 0
T =
1.0000 0 0 475.0000
0 -1.0000 0.0000 0.0009
0 -0.0000 -1.0000 -370.0000
0 0 0 1.0000
T1 =
1.0000 0 0 475.0000
0 -1.0000 0.0000 0.0009
0 -0.0000 -1.0000 0.0000
0 0 0 1.0000
qi =
0.0000 1.2808 -0.5248 0.0000 0.7560 0.0000
The output result above had shown that to go to the target
position at coordinate [475,0,0] from [475,0,-370], the
changes of joint angle are as such:

1
= 0
2
= 73.4
3
= -30

4
= 0
5
=43.3
6
= 0

III. SIMULATION

In order to make sure the same value of Denavit-
Hartenberg parameters can be applied to the real Fanuc robotic
arm, the simulation using the Robotics tool of Matlab 7.0 were
done. It needs to prove that the position and orientation of the
robots end-effector that had been computed earlier are same
with the real application. The simulation algorithm were
developed exactly using the same value of link length and
joint angle of the real Fanuc arm.
Figure 5 describe the output of the simulation for the
initial position of simulated Fanuc robot. The position of end
effector stated there was [475,0,370]. Here, if the orientation
value of link 3 is change from 0 to 90 (1.5708rad), the
position of the end effector also will make a change, as shown
in Figure 6. Now, the position of end effector was [155,0,540].












Fig. 5: Initial position of FANUC robot










Fig. 6: Adjustment position of FANUC robot

After simulating using Matlab, virtual robot simulation is
done. After selecting the model Fanuc 200iB as a model for
the virtual robot, simulation is executed. Figure 7 shows the
actual position of the end effector which is [475,0,370]. The
virtual robot is programmed to change the orientation on link
3 by 90. After the simulate button in the Roboguide software
is clicked, it is found that the position of end effector is
[155,0,540] as shown in Figure 8.


Fig. 7: Initial position of virtual FANUC robot

4
th
Student Conference on Research and Development (SCOReD 2006), Shah Alam, Selangor, MALAYSIA, 27-28 June, 2006


151

Fig. 8: Initial position of virtual FANUC robot

IV. EXPERIMENTAL RESULT
Based on the data and information on simulation results
done previously, it is time to prove that the forward kinematics
applied on the actual Fanuc robot is true. That actual robot
will be in its home position with the reading from the teach
pendant of its end-effector displayed as [475.0,0.0,370.0].
What will happen if orientation of link 3 is changed to +90?
Figure 9 show the initial position of actual Fanuc robotic arm.
The position of link 3 of the robot is moved manually
using the teach pendant with a magnitude of +90 and then its
position and orientation readings from the teach pendant is
recorded. It is found that the reading of the teaching is as such:

J1:0 J2:90 J3:90 J4:0 J5: 0 J6:0
[x=155.0 y=0.0 z=540]

Figure 10 shows the adjustment orientation of link 3 which
affect the changes in end effector position.

Fig. 9: Initial position of actual FANUC robot


Fig. 10: Adjustment position of actual FANUC robot

V. CONCLUSION
To study the optimal dynamic performance of vision
guided manipulator, one must know the forward and inverse
kinematics of that robot which will then be used to change the
position and orientation of that robot based on its dynamic
performance. The study done in this paper encompasses the
use of simulation of the forward and inverse kinematics of the
6 degree of freedom arm robot using the Matlab and
Roboguide software. It is then applied on the real Fanuc robot
Based on the simulation and experimental result of the real
robot, it can be concluded that forward and inverse kinematics
can be applied to the FANUC LR 200iB. Thus in future, the
optimal dynamic performance of the robot can be easier
studied and obtained.


VI. REFERENCES
[1] J.J. Craig, Introduction to Robotics. Addison-Wesley, 1989.
[2] Zvi Shiller and Satish Sundar, Design of Robotic Manipulators for
Optimal Dynamic Performance, : Proceedings of the IEEE
International Conference on Robotics and Automation, Sacramento,
California, April 1991.
[3] Minh-Chinh Ngunyen and Volker Graefe, Object Manipulation by
Learning Stereo Vision-Based Robots, : Proceedings of the IEEE
International Conference on Intelligent Robots and Systems, Maui,
Hawaii, USA, November 2001.
[4] Minh-Chinh Ngunyen and Volker Graefe, Self-Learning Vision-Guided
Robots for Searching and Grasping Objects, : Proceedings of the IEEE
International Conference on Robotics and Automation, San Francisco,
April 2000.
[5] Alan Bowling and Oussama Khatib, Design of Macro / Mini
Manipulators for Optimal Dynamic Performance, : Proceedings of the
IEEE International Conference on Robotics and Automation,
Albuquerque, New Mexico, April 1997.