Modeling and Simulation of 5 DOF Educational Robot Arm

Mohammed Abu Qassem Iyad Abuhadrous Hatem Elaydi

Engineering Department, Palestine Engineering Department, Palestine Engineering Faculty, Islamic
Technical College, Gaza, Palestine Technical College, Gaza, Palestine University of Gaza, Gaza, Palestine
[email protected] [email protected] [email protected]

Abstract - Many universities and institutes experience introduction to the subject of robotics must include a
difficulty in training people to work with expensive equipments. rigorous treatment of the topics in this paper.
A common problem faced by educational institutions concerns There is a large amount of literature which discuss the
the limited availability of expensive robotics equipments, with robot molding and analysis of industrial robots [2]. The
which students in the didactic program can work, in order to majority of their importance comes from discussing the low
acquire valuable "ha nds on" experience. Therefore, the Robot cost educational robot arms. Therefore, in this paper,
Simulation Software (RSS) nowadays is paramount important.
mathematical model and kinematical analysis of the AL5B
This paper presents the development of a visual software
educational robot arm shown in Fig. 1 will be studied.
package where a AL5B Robot arm has been taken as a case A visual software program will be also developed to
study. It utilizes Matlab/Simulink and AutoCAD as the tools show the robot arm motion with respect to its mathematical
for testing motional characteristics of the AL5B Robot arm. A analysis
robot model will be developed; the Forward, Inverse
Kinematics, Velocity Kinematics "Jacobian" and Path
Planning problems will be implemented and tested.

The developed package will be used as an educational tool. We

think that this work will increase the education, training,
research and development possibilities for robotics classes in
graduate and under graduate studies.

Keywords: Modeling, Simulation, MATLAB/Simulink, 5DOF

Robot arm, Forward Kinematics, IK, Path Planning, Jacobian.

I. INTRODUCTION Figure 1. AL5B robotic arm

Robotics is a relatively young field of modern technology This paper presents the development of a visual model
that crosses traditional engineering boundaries. for the AL5B robot arm to be use educational institution in
Understanding the complexity of robots and their Gaza Strip to teach robotic courses.
applications requires knowledge of electrical engineering, The paper is organized as follow: section II gives a full
mechanical engineering, systems and industrial engineering, description of AL5B robot arm, section III gives the
computer science, economics, and mathematics. New complete kinematic modeling, section IV covers the software,
disciplines of engineering, such as manufacturing section V gives the results, and section VI concludes this
engineering, applications engineering, and knowledge paper.
engineering have emerged to deal with the complexity of the
field of robotics and factory automation [1]. II. ROBOT DESCRIPTION
This paper is concerned with the fundamentals of AL5B robot arm has 5 directions of motion (DOF) plus a
robotics, including kinematics, motion planning, Jacobian, grip movement (5+1). It is also similar to human arm from
interface and control. Our goal is to introduce the most the number of joints point of view. These joints provide
important concepts in these subjects as applied to industrial shoulder rotation, shoulder back and forth motion, elbow
robot manipulators, mobile robots, and other mechanical motion, wrist up and down motion, wrist rotation and gripper
systems. A complete treatment of the discipline of robotics motion.
would require several volumes. Nevertheless, now, the AL5B has five rotational joints and a moving grip. Joint
majority of robot applications deal with industrial robot arms 1 represents the shoulder and its axis of motion is z1. This
operating in structured factory environments so that a first joint provides a rotational θ1 angular motion around z1 axis
in x1y1 plane. Joint 2 is identified as the Upper Arm and its
axis is perpendicular to Joint 1 axis. It provides a rotational
θ2 angular motion around z2 axis in x2y2 plane. z3 axes of
978-1-4244-5848-6/10/$26.00 ©2010 IEEE Joint 3 (Forearm) and Joint 4 (Wrist) are parallel to Joint 2 z-

569
axis; they provide θ3 and θ4 angular motions in x3y3 and
x4y4 planes respectively. Joint five is identified as the grip
rotation. Its z5 axis is vertical to z4 axis and it provides θ5
angular motions in x5y5 plane. A graphical view of all the
joints was displayed in Fig. 2.
A rigid body is completely described in space by its
position to a reference frame (translation) and its orientation.
AL5B Robot Arm rotational joints and the grip are
controlled by dedicated servo motors. These motors are
connected to a serial servo controller card Cubloc (CB280)
to control the AL5B from a computer through the serial port. Figure 2. Coordinate Frames of AL5B Robotic Arm
A sequence of 3 consecutive unsigned bytes is sent to
serial servo controller from the computer. These are the By substituting these parameters; the transformation
default sync byte, the joint servo identifier byte and the matrices T1 to T6 can be obtained as shown bellow. For
desired position byte. Rotational position of a servo motor is example, T1 shows the transformation between frames 0 and
determined by a specific angle by closed loop feedback. This 1 (designating Ci as cos θi and Si as sin θi etc).
angle value, provided by the computer, was digitized to  cθ − sθ 0 0  cθ − sθ 0 0
   
1 1 2 2

generate discrete motional steps by the CB280 card.

 sθ cθ 0 0  0 0 −1 0 
CB280 servo control card provides the hardware T1 =
0 1 1
T2 =
1

interface between computer and the robot arm. It has a time 0 1 0 d1   sθ cθ 0 0

resolution of 1µs for accurate positioning and a dc motor    
2 2

 0 0 0 1   0 0 0 1 
control to generate extremely smooth moves. The time range
is 0.50mS to 2.50mS for an angular range of 0° to 180°. The cθ − sθ 0 a3   cθ − sθ 0 a4 
   
3 3 4 4

card generated motion can be a speed controlled, time sθ cθ 0 0 sθ cθ 0 0

T3 =  T4 = 
2 3
controlled or a combination (speed and time) motion. 3 3 4 4

0 0 1 0 0 0 1 0
III. KINEMATICS    
 0 0 0 1   0 0 0 1 
Many methods can be used in the direct kinematics
calculation. The Denavit-Hartenberg analyses is one of the cθ − sθ 0 0 cθ − sθ 0 0
   
5 5 5 5

most used, in this method the direct kinematics is 0 0 1 d5  sθ cθ 0 0

T5 =  =
4
determinate from some parameters that have to be defined, TGripper 5 5

depending on each mechanism. However, it was chosen to

 sθ cθ 0 0 0 0 1 0
 5 5
  
use the homogeneous transformation matrix. This  0 0 0 1   0 0 0 1 
transformation specifies the location (position and
orientation) of the hand in space with respect to the base of A. Forward Kinematic
the robot, but it does not tell us which configuration of the Calculating the position and orientation of the end
arm is required to achieve this location [3]. effectors with given joint angles is called Forward
D-H parameters for AL5B are defined for the assigned Kinematics analysis. Forward Kinematics equations are
frames in Table I. For example Frame 5 is the grip frame generated from the transformation matrixes shown bellow
with attached end effectors at joint 5. and the forward kinematics solution of the arm is the product
TABLE I. DH PARAMETER FOR AL5B ROBOT ARM of these six matrices identified as 0T6 (with respect to base)
shown in eq.(1)
i α i −1 a i −1
d i
θ i
 nx ox a x d x 
* n oy ay dy 
1 0 0 d1 θ
T =T T T T T T = 
0 0 1 2 3 4 5

y
1
(1)
 nz dz 
6 1 2 3 4 5 6
2 90 0 0 θ
* oz az
2
0 
3 0 a3 0 θ 3
*  0 0 1

4 0 a4 0 θ 4
-90 * The first three columns in the matrices represent the
*
orientation of the end effectors, whereas the last column
5 -90 0 d5 θ 5 represents the position of the end effectors [3]. The
orientation and position of the end effectors can be calculated
6 0 0 0 Gripper in terms of joint angles.
B. Invers Kinematic
Inverse Kinematics analysis determines the joint angles
for desired position and orientation in Cartesian space. Total

570
Transformation matrix Equation will be used to calculate r4 = a3 cos(θ 2 ) + a4 cos(θ 2 + θ 3 ) (8)
inverse kinematics equations. Its solution, however, is much
more complex than direct kinematics since there is no unique z4 = a3 sin(θ 2 ) + a4 sin(θ 2 + θ 3 ) + d1 (9)
analytical solution. Each manipulator needs a particular
method considering the system structure and restrictions [4].

Geometric Approach
Using IK-Cartesian mode, the user specifies the desired
target position of the gripper in Cartesian space as (x, y, z)
where z is the height, and the angle of the gripper relative to
ground, ψ (see Fig.4, is held constant. This constant ψ allows
users to move objects without changing the object’s
orientation (the holding a cup of liquid scenario). In addition,
by either keeping ψ fixed in position mode or keeping the
wrist fixed relative to the rest of the arm, the inverse
kinematic equations can be solved in closed form as we now
show for the case of a fixed ψ.
The lengths d1, a3, a4 and d5 correspond to the base height,
upper arm length, forearm length and gripper length,
respectively, and are constant [4], [5]. The angles θ1, θ2, θ3,
θ4 and θ5 correspond to shoulder rotation, upper arm, and
forearm, wrist, and End effectors, respectively. These angles Figure 4. Arm sagittal planar view of robot
are updated as the specified position in space changes. We
solve for the joint angles of the arm, θ1:4 given desired Now we want to determine θ2 and θ3. We first solve for α,
position (x, y, and z) and ψ which are inserted by the user. β and s (from Fig. 3B) uses the law of cosines as:
From Fig. 3, we clearly see that θ1 = Atan2 ( y, x ) β = A tan 2( s + a3 − a4 , 2sa3 )
2 2 2
(10)
and the specified radial distance from the base d are related α = A tan 2( z 4 − d1 , r4 ) (11)
to x and y by:
s= ( z 4 − d1 ) + r4
2 2

d= xd + yd
2 2
(2) (12)
With these intermediate values, we can now find the
xd = d cos(θ1 ) (3) remaining angle values as:
yd = d sin(θ1 ) (4) θ2 = α ± β (13)
θ 3 = A tan 2( s − a3 − a4 , 2a3 a4 )
2 2 2
(14)
θ4 = ψ − θ2 − θ3 (15)
C. Velocity Kinematics/Arm Jacobian
The Jacobian is one of the most important quantities in
the analysis and control of robot motion. It is used for
smooth trajectory planning and execution in the derivation of
the dynamic equation. To investigate target with specified
velocity, each joint velocity at the specified joint positions
needs to be found. This is accomplished using Jacobian,
Figure 3. AL5B Geometric analysis which are used to relate joint velocities to the linear and
angular velocities of the end-effector. For the AL5B robot
Moving now to the planar view in Fig. 4, we find a arm the Jacobian matrix is equal 6x5
relationship between joint angles θ2, θ3 and θ4 and ψ as z0 ×( o5 −o0 ) z1 ×( o5 −o1) z2 ×( o5 −o2 ) z3 ×( o5 −o3) z4 ×( o5 −o4 )
follows: J( q) = (16)
 z0 z1 z2 z3 z4 
ψ = θ 2 + θ3 + θ 4 (5)
From the forward kinematic we can find:
Since ψ is given, we can calculate the radial distance and
height of the wrist joint: 0 0 0  c1c2a3 
r4 = rd − a5 cos(ψ ) (6) o0 = 0 , o1 =  0  , o2 =  0  , o3 =  c1 s1a3 
       
z4 = z d − a5 sin(ψ ) (7) 0 d1 d1 s2 a3 + d1 
or

571
  c1c23 a4 + c1c2 a3  c1c234 d 5 + c1c23 a4 + c1c2 a3  from the initial configuration to its destination Fig. 4 shows
the Trajectory planning block diagram.
o4 = s1c23 a4 + s1c2 a3 , o5 =  s1c234 d5 + s1c23 a4 + s1c2 a3 
 
   
 s23 a4 + s2 a3 + d1   s234 d 5 + s23 a4 + s2 a3 + d1

Moreover, we can find:

0 0  s1   s1   s1  cc1 234 

          
z0 = 0 , z1 = 0 , z2 = -c1 ,z3 = -c1 , z4 = -c1 , z5 = sc 
         1 234 
1 1  0   0   0  −s234 
J ( q ) = [ J1 J 2 J 3 J 4 J 5 ] (17)
 - sc d - sc
1 234 5 1 23 4 1 2 3
a - sc a  - sc d - sc
1 234 5 1 2 3
a - sc
1 23 4
a Figure 5. Trajectory planning block diagram
 cc d +cc a +cc a   cc d +cc a +cc a 
 1 234 5 1 23 4 1 2 3  1 234 5 1 23 4 1 2 3  Cubic Polynomial Trajectories
 0   0  Suppose that we wish to generate a trajectory between
J1 =   ,J2 =   two configurations, and that we wish to specify the start and
0 0
    end velocities for the trajectory [1]. If we have four
 0   0  constraints to satisfy, such as following equation we require
    a polynomial with four independent coefficients that can be
 1   1  chosen to satisfy these constraints. Thus, we consider a cubic
trajectory of the form
q(t ) = a0 + a1t + a2 t + a3t
2 3

 -c1(s234d5 +s23a4 +s2a3 )  For Distance (18)

 -s1(s234d5 + s23a4 +s2a3 )  Then the desired velocity is given as
  q& (t ) = a1 + 2a2t + 3a3t
2
For Velocity (19)
 s1(sc d + sc a + sc a ) + c (cc d + cc a +cc a )
J3 =  q&&(t ) = 2 a2 + 6a3t
1 234 5 1 23 4 1 2 3 1 1 234 5 1 23 4 1 2 3
 For Acceleration (20)
s1
  Combining above equations with the four constraints
 -c1  yields four equations in four unknowns
  q0 = a0 + a1t 0 + a2 t0 + a3 t0
2 3
(21)
 0 
v0 = a1 + 2a2 t0 + 3a t0
3 2
 -c1 ( s234 d 5 + s23 a4 )  (22)
 -s1 ( s234 d 5 + s23 a4 )  q f = a0 + a1t f + a 2 t f + a3t f
2 3
(23)
 
 s1 ( s1c234 d 5 + s1c23a4 ) + c1 (c1c234 d 5 + c1c23 a4 )  v f = a1 + 2 a 2 t f + 3a3 t f
2
(24)
J4 =  ,
s1 These four equations can be combined into a single
  matrix equation
 -c1 
 
 0   1 t0 2
t0 t 0   a0 
3
 q0 
0 1 
3t 0  a1 
v 
 
3
-c1 s234 d 5 2t 0
   =  0  (25)
 -s1 s234 d 5  1 t f 2
tf t f   a2   q f 
3

 2   2     
 s1 c234 d 5 + c1 c234 d 5 
2
0 1 2t f 3t f   a3   v f 
J5 =  
s1
  IV. SOFTWARE
 -c1  The first step in this process is to design the arm in
  AutoCAD 3D program. The program chosen for this was
 0  Autodesk Inventor. Inventor allows the arm to be designed
D. Trajectory Planning and visualized at the same time. It also allows the arm to be
Trajectory planning approximate the desired path by a checked for possible collisions and link interference.
class of polynomial functions and generates a sequence of Because each link depends upon the previous link, the design
time-based “control set points” for the control of manipulator of the arm needs to begin at the base and finish at the end
effector or gripper. Trunk or base is therefore the first to be

572
designed, followed by shoulder, and so on [6]. This means Figure 6. Cubic polynomial trajectory
that the design process is fairly involved, as each link has to
be redesigned several times. After we finished the graphics
VI. CONCLUSIONS
design by using the AutoCAD 3D the main question is how
to connect it to MATLAB. We use the CAD2Matlab A complete Kinematics analysis of the AL5B robot arm
function, where this function takes a CAD file in (.stl or .slp was investigated. Graphical User Interface (GUI) was
format) and converts it to Matlab [7]. We need to use anther developed to test and simulate the motional characteristics of
program to convert the AutoCAD dwg to slp file like the Robot arm. A physical interface between the AL5B robot
PolyTrans 3D from Okino Computer Graphics, by using this arm and the GUI will be designed.
program we can modifying the robot link and coordinate and The developed system will be identified as an educational
save the file as slp. experimental tool; it can be used in graduate and
Software package was developed to compute the forward undergraduate robotic courses to realize the relationships
kinematics, inverse kinematics, Jacobian, trajectory planning, between theoretical and practical aspects of robot
of AL5B Robot arm. GUI Development Platform with manipulator motions in real time.
MatLab programming language was used for implementation.
An On-line motional simulator of the robot arm was also REFERENCES
included of GUI to show the generated motion in Fig. 7 [1] Mark W. Spong, Seth Hutchinson, and M. Vidyasagar, Robot
based on the theoretical analysis presented in this paper. Modeling and Control, 1st Edition, John Wiley & Sons.2005
[2] Johan J.Craig, Introduction to Robotics Mechanics and Control, 3rd
V. RESULTS AND DISCUSSIONS Edition, pp 109-114, Prentice Hall, 2005
[3] Baki koyuncu, and Mehmet Güzel, "Software Development For the
Mathematical modeling and kinematic analysis of AL5B, Kinematic Analysis Of A Lynx 6 Robot Arm", pwaset volume 24
was developed and tested in this study. Robot arm was october 2007
mathematically modeled with Denavit Hartenberg (D-H) [4] Chun Htoo Aung, Khin Thandar Lwin, and Yin Mon Myint,
method. Forward, Inverse Kinematics, Jacobian and path "Modeling Motion Control System for Motorized Robot Arm using
planning solutions were generated and implemented by the Matlab", Proceedings Of World Academy Of Science, Engineering
developed software. And Technology Vol. 32 August 2008
After testing the forward kinematic as example for the [5] R. Manseur, "A Software Package For Computer-Aided Robotics
Education", pp.1409-1412, 26th Annual Frontiers in Education - Vol
desired position x, y and z (199.3, 199.3, 154.9) the input 3, 1996
angles θ(1, 2, 3, 4, 5) equal (45, 45,-45, 0, 0) and Vis versa for [6] Brandi House, Jonathan Malkin and Jeff Bilmes, "The VoiceBot: A
inverse kinematic as shown in Fig. 7. Voice Controlled Robot Arm", University of Washington, 2009
Fig. 6 shows the Cubic trajectory playing first three [7] Muhammad Ikhwan Jambak, Habibollah Haron, Dewi Nasien,
angles with respect to the base. "Development of Robot Simulation Software for Five Joints
Trajectory using Cubic Polynomial Mitsubishi RV-2AJ Robot using MATLAB/Simulink and V-Realm
50
Builder", Fifth International Conference on Computer Graphics,
Angle

0 Imaging and Visualization, 2008

-50 [8] Leniel Braz de Oliveira Macaferi, "Construction and Simulation of a
0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0.09 0.1
Time Robot Arm with Opengl", May 16, 2007
[9] Arya Wirabhuana1, Habibollah bin Haron "Industrial Robot
1000
Simulation Software Development Using Virtual Reality Modeling
Velocity

0 Approach (VRML) and Matlab- Simulink Toolbox", University

-1000
Teknologi Malaysia, 2004
0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0.09 0.1
Time
4
x 10
5
Acceleration

-5
0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0.09 0.1
Time

573
Figure 7. GRAPHICAL USER INTERFACE

574