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

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 To find the optimal dynamic performance of a robot, it is
the dynamic performance of an industrial robot with aid of important to know the architecture of its kinematics first.
vision system technology. It consists of three major phases; From forward or inverse kinematics analysis of that particular
(1) theoretical, simulation and practical of forward and robot, we will obtain an exact position and orientation for each
inverse kinematics for Fanuc LR Mate 200iB robot to of its movements. This will help to find the most suitable
determine their D-H parameters, (2) optimization of robot position and orientation to achieve optimal performance.
movements, and (3) implementation of practical tasks. The Figure 1 describe the overall diagram for the system.
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 CAMERA
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.
MANIPULATOR

Index terms: Vision-Guided Manipulators, Optimal Control,

Path-Finding, Image Processing, Dynamic Performance.
Fig. 1: Proposed vision guided manipulators system
I. INTRODUCTION
Moving in an unknown environment is an experience for II. KINEMATIC ANALYSIS
all animated systems. This is possible because the visual To develop and demonstrate our approach in the real-
information data stream holds a large number of useful world experiments, the articulated-arm robot with 6 d.o.f.
parameters that all these system interpret and translate into FANUC LR Mate 200iB equipped with a parallel-jawed
actions as well as into behaviors. gripper have been used. So, the inverse and forward
It is now well accepted that the use of vision systems kinematics architecture for this robot must be identified first
coupled with a robot manipulator can considerably improve before studying the dynamic performance.
their robustness and efficiency and allow the realization of
complex industrial tasks when a single robot manipulator is A. Forward Kinematics
likely to fail. The concept of reconfigurable manipulators has Kinematics is the science of motion which treats motion
been recently developed to facilitate the synthesis of without regard to the forces that cause it [1]. Here, a method to
manipulators structures to best suit specific tasks. compute the position and orientation of the robots end-
In this project, one of the sensors that are currently being effector relative to the base of the robot or we call it the
used in controlling robot mechanism is vision. Optimal-based forward kinematics, will be described.
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.
147
4th Student Conference on Research and Development (SCOReD 2006), Shah Alam, Selangor, MALAYSIA, 27-28 June, 2006

(c!i -)s!i 0 0% (1 0 0 0% (1 0 0 ai % (1 0 0 0%
i-1
Ti = &s!i c!i 0 0# &0 1 0 0## &&0 1 0 0## &&0 c"i -)s"i 0## (2)
& #&
& 0 0 1 0# &0 0 1 di # &0 0 1 0# &0 -)s"i c"i 0#
& #& #& #& #
' 0 0 0 1$ '0 0 0 1$ '0 0 0 1$ '0 0 0 1$

(c!i -)s!ic"i s!is"i aic!i %

i-1
Ti = &s! # (3)
& i c!ic"i - c!is"i ais!i #
&0 s"i c"i di #
& #
' 0 0 0 1 $

0
Ti = 0T1 1T2 2T3.i-2Ti-1 i-1Ti (4)

Fig. 2: Link frame assignments A.2 Forward Kinematics Analysis of FANUC

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 ai perpendicular
intersects the joint i axis. Xi points along ai 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, di is the joint variable and the
other three quantities are fixed link parameters. The definition Fig. 3: FANUC LR Mate 200iB workspace
of mechanisms by means of these quantities is a convention
usually called the Denavit-Hartenberg (D-H) notation [1]. y6 y4
Furthermore, the following definitions of the link parameters x5
z6 z4
can be applied: x4
x6 x3
z5 y3
y5
ai = the distance from i to i+1 measured along Xi; z3
!i = the angle between i and i+1 measured about Xi;
x2 z1
di = the distance from Xi-1 to Xi measured along i;
y2 x1
i = the angle between Xi-1 and Xi measured about Xi.
z2 y1

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 zi-1 by i: Rot( z, i)
(2) Translation of the new frame along z by di: Trans(0,0,di)
(3) Translation of the new frame along x by ai :
Trans(ai,0,0)
(4) Rotation of the new frame about x by !i : Rot(x,ai )
The overall transformation is obtained by
multiplication of individual transformations:
i-1
Ti Rot( z,! i)Trans(0,0, d i)Trans(0,0, a i)Rot( x," i ) (1)
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 ai, i
= 1, . . . , 6) at each of the joints, as shown in Figure 4. This
post 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

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

forward kinematics function 0T6 via a link-by-link iteration of The transformation matrix of frame x6 y6 z6 with
the form respect to frame x6 y6 z6 is obtained as
0
T6 = 0T1 .1T2 . 2T3, i = 0, , 6 (5) (c6 - s6 0 0 %
5
T6 = &s - c 0 0 # (11)
&6 6 #
Furthermore, it is useful to explicitly have the interlink &0 0 1 80#
homogeneous transformations, i-1Ti, that can then be computed & #
'0 0 0 1$
as by products.
where s6 =sin 6 and c6 =cos 6
TABLE 1: DENAVIT-HARTENBERG PARAMETERS OF THE FANUC By using (6), (7), (8), (9), (10) and (11), considering the initial
ROBOT position, all = 0, the forward kinematics equation of the
i !i-1 ai-1 di i robot can be derived as
0
0 0 0 0 0 T6 = 0T1.1T2. 2T3..3T4. 4T5.5T6
1 90 150 0 1 (1 0 0 475%
0
2 0 250 0 2
T6 = &0 -1 0 0 ## (12)
&
3 90 75 0 3 &0 0 +1 +370#
4 90 0 290 & #
'0 0 0 1 $
4
5 -90 0 0 5
6 0 0 80 6
B. Inverse Kinematics
According to general transformation equation of D-H Inverse Kinematics does the reverse of forward
method and the parameters in Table I, the transformation kinematics. Given the end point of the structure, what angles
matrix of frame x1 y1 z1 with respect to frame x0 y0 z0 is do the joints need to be to achieve that end point. It can be
obtained as difficult, and there are usually many or infinitely many
(c1 0 s1 150* c1%
solutions. Most of the real systems are under constrained, so
0
T1 = &s 0 - c 150*s # (6) for a given goal position, there could be infinite solutions (i.e.
&1 1 1#
many different joint configurations could lead to the same
&0 1 0 0 #
& # endpoint). The field of robotics has developed many inverse
'0 0 0 1 $ kinematics systems which, due to their constraints, have
where s1 =sin 1 and c1 =cos 1 closed-form solutions.
The transformation matrix of frame x2 y2 z2 with One of the techniques that had been used to find the exact
respect to frame x1 y1 z1 is obtained as angle of each joint to reach the goal target is by using the
(c2 -)s2 0 250* c2 % Jacobian method which described briefly in [1].
1
T2 = &s 0 250*s2 ## (7)
& 2 c2
&0 0 1 0 # B.1 Inverse Kinematics Analysis of Fanuc
& # After considering the Jacobian method trough the
'0 0 0 1 $
where s2 =sin 2 and c2 =cos 2 theoretical calculation based, the analysis which is using the
The transformation matrix of frame x3 y3 z3 with Matlab Robotics tools were done. To find the inverse
respect to frame x2 - y2 - z2 is obtained as kinematics in Matlab, the function that had been used is q =
ikine(robot, T). This function returns the joint
(c3 0 s3 75* c3 %
2
T3 = &s 0 - c 75*s # (8) coordinates for the manipulator described by the object robot
&3 3 3#
whose end-effector homogeneous transform is given by T.
&0 1 0 0 #
& # The complete lists of function regarding this analysis
'0 0 0 1 $ using Matlab are as such:
where s3 =sin 3 and c3 =cos 3
The transformation matrix of frame x4 y4 z4 with %Entering the D-H parameter information for FANUC
L1=link([pi/2 150 0 0 0])
respect to frame x3 y3 z3 is obtained as L2=link([0 250 0 0 0])
(c4 0 s4 0% L3=link([pi/2 75 0 0 0])
3
T4 = &s 0 - c 0 ## (9) L4=link([pi/2 0 0 290 0])
&4 4 L5=link([4.7124 0 0 0 0])
&0 1 0 290# L6= link([0 0 0 80 0])
& #
'0 0 0 1$ %Searching for FANUC Position and Orientation Matrix
fanuc = robot({L1 L2 L3 L4 L5 L6})
where s4 =sin 4 and c4 =cos 4
The transformation matrix of frame x5 y5 z5 with %Generate the transform corresponding to a particular joint
coordinate
respect to frame x4 y4 z4 is obtained as q=[0 0 0 0 0 0]
(c5 0 - s5 0%
4
T5 = &s 0 c5 0## (10) %Forward Kinematic
& 5 T=fkine(fanuc,q)
& 0 +1 0 0# T1=transl(0,0,370)*T
& #
' 0 0 0 1$ %Inverse Kinematic
qi=ikine(fanuc,T1)
where s5 =sin 5 and c5 =cos 5

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

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)
Fig. 5: Initial position of FANUC robot
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
Fig. 6: Adjustment position of FANUC robot
0 0 0 1.0000

T1 = After simulating using Matlab, virtual robot simulation is

1.0000 0 0 475.0000 done. After selecting the model Fanuc 200iB as a model for
0 -1.0000 0.0000 0.0009
0 -0.0000 -1.0000 0.0000 the virtual robot, simulation is executed. Figure 7 shows the
0 0 0 1.0000 actual position of the end effector which is [475,0,370]. The
virtual robot is programmed to change the orientation on link
qi = 3 by 90. After the simulate button in the Roboguide software
0.0000 1.2808 -0.5248 0.0000 0.7560 0.0000
is clicked, it is found that the position of end effector is
The output result above had shown that to go to the target [155,0,540] as shown in Figure 8.
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 Fig. 7: Initial position of virtual FANUC robot
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].
150
4th Student Conference on Research and Development (SCOReD 2006), Shah Alam, Selangor, MALAYSIA, 27-28 June, 2006
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.
151