Project in CAM and Robotics

Abstract— To be able to control a robot manipulator as required

by its operation, it is important to consider the kinematic model in design of the control algorithm. In robotics, the kinematic descriptions of manipulators and their assigned tasks are utilized to set up the fundamental equations for dynamics and control.. The objective of this paper is to derive the complete forward kinematic model (analytical and numerical) of a 6 DOF robotic arm (LR Mate 200iC from Fanuc Robotics) and validate it with the data provided by robot’s software.

Keywords — forward kinematic, Denavit-Hartenberg parameters, Robotic Toolbox

I. INTRODUCTION Kinematics is the science of geometry in motion. It is

restricted to a pure geometrical description of motion by means of position, orientation, and their time derivatives. In robotics, the kinematic descriptions of manipulators and their assigned tasks are utilized to set up the fundamental equations for dynamics and control [1]. These non-linear equations are used to map the joint parameters to the configuration of the robot system. The Denavit and Hartenberg notation [2],[3] gives us a standard methodology to write the kinematic equations of a manipulator. This is especially useful for serial manipulators where a matrix is used to represent the pose (position and orientation) of one body with respect to another.

There are two important aspects in kinematic analysis of robots: the Forward Kinematics problem and the Inverse Kinematics problem. Forward kinematics refers to the use of the kinematic equations of a robot to compute the position of the end-effector from specified values for the joint parameters Inverse kinematics refers to the use of the kinematics equations of a robot to determine the joint parameters that provide a desired position of the end-effector.

The purpose of this paper is to obtain the forward kinematic analysis for the Fanuc LR Mate 200iC Robot, to make model of robot using Robotic Toolbox® for MATLAB® and validate it with the data provided by robot’s software. The information can be use in the future design and production of

Daniel Constantin is with Military Technical Academy, Bucharest,

Romania; 39-49 George Cosbuc Blv., 050141, phone +40213354660; fax: +40213355763; e-mail: [email protected].

Marin Lupoae is with Military Technical Academy, Bucharest, Romania; e-mail: [email protected].

Cătălin Baciu is with Military Technical Academy, Bucharest, Romania; e-mail: [email protected].

Dan-Ilie Buliga is with Military Technical Academy, Bucharest, Romania; e-mail: [email protected].

the robot to make it faster and more accurate postwar period for the defensive works executed in different countries around the globe.

II. THEORETICAL ASPECTS The forward kinematics problem is concerned with the

relationship between the individual joints of the robot manipulator and the position and orientation of the tool or end- effector.

A serial-link manipulator comprises a set of bodies, called links, in a chain and connected by joints. A link is considered a rigid body that defines the spatial relationship between two neighboring joint axes. The objective of forward kinematic analysis is to determine the cumulative effect of the entire set of joint variables.

The Denavit and Hartenberg notation for describing serial-link mechanism geometry is a fundamental tool for robot analysis. Given such a description of a manipulator it can be use for established algorithmic techniques to find kinematic solutions, Jacobians, dynamics, motion planning and simulation.

In order to perform a forward kinematic analysis of a serial-link robot, based on Denavit - Hartenberg (D-H) notation it is necessary to achieve a five steps algorithm:

1. Numbering the joints and links A serial-link robot with n joints will have n +1 links.

Numbering of links starts from (0) for the fixed grounded base link and increases sequentially up to (n) for the end-effector link. Numbering of joints starts from 1, for the joint connecting the first movable link to the base link, and increases sequentially up to n. Therefore, the link (i) is connected to its lower link (i - 1) at its proximal end by joint (i) and is connected to its upper link (i + 1) at its distal end by joint (i + 1).

2. Attaching a local coordinate reference frame for each

link (i) and joint (i+1) based on the following method, known as Denavit - Hartenberg convention [2], [3].

Fundamentally is necessary to describe the pose of each link in the chain relative to the pose of the preceding link. It is expected that this to comprise six parameters, one of which is the joint variable the parameter of the joint that connects the two links. The Denavit-Hartenberg formalism [2]-[4], [6] uses only four parameters to describe the spatial relationship between successive link coordinate frames, and this is achieved by introducing two constraints to the placement of

Forward Kinematic Analysis of an Industrial Robot

Daniel Constantin, Marin Lupoae, Cătălin Baciu, Dan-Ilie Buliga

New Developments in Mechanics and Mechanical Engineering

ISBN: 978-1-61804-288-0 90

those frames the axis ix is intersecting or perpendicular to the axis i-1z . The choices of coordinates frames are also not unique, different people will derive different, but correct, coordinate frame assignments.

In this paper it is used the standard D-H notation [4]. It

begins by assignation of iz axes. There are two cases to consider: if joint (i +1) is revolute, iz is the axis of revolution of joint (i +1) and if joint (i +1) is prismatic, iz is the axis of translation of joint (i +1).

Once it was established the iz -axes for the links, it must establish the base frame { }0 . The choice of a base frame is nearly arbitrary. Once frame { }0 has been established, begins an iterative process in which it is defined frame { }i using frame { }i - 1 .

In order to set up frame { }i it is necessary to consider three

cases: (a) Axes i-1z and iz are not coplanar (fig.1): The line

containing the common normal i-1z to iz define ix axis and the point where this line intersects iz is the origin of frame { }i .

(b) Axis i-1z is parallel to axis iz : The origin of frame { }i is the point at which the normal that passes through origin of frame { }i - 1 intersects the iz axis. The axis ix is directed from origin of frame { }i , toward i-1z axis, along the common normal.

(c) Axis i-1z intersects axis iz : The origin of frame { }i is at the point of intersection of axes i-1z and iz . The axis ix is chosen normal to the plane formed by axes i-1z and iz . The positive direction is arbitrary.

In all cases the iy -axis is determined by the right-hand rule: i i iy z x= × .

3. Establish D-H parameters for each link The fundamentals of serial-link robot kinematics and the

Denavit-Hartenberg [2], [3] notation are well covered in standard texts [6]. Each link is represented by two parameters: the link length ( a ), and link twist ( α ), which define the relative location of the two attached joint axes in space. Joints are also described by two parameters: the link offset ( d ), which is the distance from one link to the next along the axis of the joint, and the joint angle (θ ), which is the rotation of one link with respect to the next about the joint axis. For a revolute joint iθ is the joint variable and id is constant, while for a prismatic joint id is variable and iθ is constant. The link length and link twist are constant.

Using the attached frames (fig.1), the four parameters that locate one frame relative to another are defines as:

 iθ (joint angle) is the angle between the i-1x and

ix axes about the i-1z axis;  id (link offset) is the distance from the origin of

frame { }i - 1 to the ix axis along the i-1z axis;  ia (link length) is the distance between the

i-1z and iz axes along the ix axis; for intersecting axes is parallel to i-1 iz z× ;

 iα (link twist) is the angle between the i-1z and

iz axes about the ix axis. 4. Calculate the matrix of homogeneous transformation for

each link. The reference frame { }i can be located relative to

reference frame { }i - 1 (fig.1) by executing a rotation through an angle iθ about i-1z axis, a translation of distance id along

i-1z axis, a translation of distance ia along ix axis and a rotation through an angle iα about ix axis. Trough concatenation of these individual transformations, the equivalent homogeneous transformation is:

( , , , ) ( ) ( ) ( ) ( )i -1 i i i i i z i z i x i x iT d a R T d T a Rθ α θ α= ⋅ ⋅ ⋅ (1) where ( )R and ( )T are the 4 4× homogeneous

transformation matrix for rotation and translation that are well covered in literature [1], [4] and [6].

Equation (1) can be expanded as:

Fig. 1 D-H frames allocation and parameters

(adapted from [4])

New Developments in Mechanics and Mechanical Engineering

ISBN: 978-1-61804-288-0 91

c

0 0 0 0 1

i i i i i i i

i -1 i i i i i i i i

i i i

s c s s a c s c c c c a sT s c d

θ θ α θ α θ θ θ α θ α θ

α α

− ⋅ ⋅ ⋅ ⋅ − ⋅ ⋅=

        

(2)

where: c iθ is cos( )iθ and isθ is sin( )iθ . Since the homogeneous transformation matrix of the

frame { }i related to the frame { }i - 1 , i -1 iT is a function of a single variable, it turns out that three of the above four quantities are constant for a given link, while the fourth parameter, iθ for a revolute joint and id for a prismatic joint, is the joint variable.

5. Compute the kinematics equations of the robot The coordinate transformations along a serial robot

consisting of n links form the kinematics equations of the robot is:

0 n

i -1 n i

i=1

T T= ∏ (3)

where i -1 iT is the homogeneous transformation matrix of the

frame { }i related to the frame { }i - 1 . The result will be a 4 4× matrix that gives us the

information about orientation or rotation ( n - normal vector, o - orientation vector, a - approach vector) matrix and position ( p - vector) vector of the last frame { }n relative to the first frame { }0 :

0

0 0 0 1nT =      n s a p

(4)

III. ANALYTICAL FORWARD KINEMATIC ANALYSIS OF 6 DOF - LR MATE 200IC ROBOT

The LR Mate 200iC is an industrial robot manufactured by FANUC Robotics and designed for a variety of manufacturing and system process. We have that type of robot in our Robotics Laboratory. The manipulator provides a payload of 5 kg capacity in a compact modular construction and reach of 704 mm with enough flexibility and higher reliability trough the harmonic drives in its six-axis. The LR Mate 200iC Robot is an electric servo-driven mini robot offering best-in-class performance in a light, efficient, accurate and nimble package. This robot is ideal for fast and precise applications in all environments [5].

From the fig.2 it can be seen that the LR Mate 200iC has a

serial 6-DOF robotic arm with six revolute joints ( ).

In order to make a forward kinematic analysis for of LR

Mate 200iC Robot it is numbered links and joint and it is attached local coordinate reference frames (fig.3). The origin of frame { }0 it is choosen to be intersection of axis from joint (1) with a perpendicular plan that contains the axis of joint (2).

The Table 1 summarizes the D-H parameters for each link

as follows from fig.2 in accord with step 4 from previous section:

Table 1 D-H parameters for LR Mate 200iC Robot

Link iθ id ia iα 1 1θ 0 1a 2π 2 2θ 0 2a 0 3 3θ 0 3a 2π 4 4θ 4d 0 2π− 5 5θ 0 0 2π 6 6θ 6d 0 0

According to equations (2) and (3) it is obtain matrix of homogeneous transformation for each link and we compute the kinematics equations of the robot:

Fig. 3 Coordinate reference frames for LR Mate 200iC Robot

Fig. 2 Joints of LR Mate 200iC Robot from Fanuc

Corporation

New Developments in Mechanics and Mechanical Engineering

ISBN: 978-1-61804-288-0 92

1 1 1 1 2 2 2 2

1 1 1 1 2 2 2 20 6

0 0 0 0

0 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1

c s a c s a s c a s c a

c c s sT

− −

⋅ ⋅ ⋅ ⋅⋅ ⋅=

                  

4 43 3 3 3

4 43 3 3 3

4

0 00 0 00

0 1 00 1 0 0 0 0 0 1 0 0 0 1

c sc s a s cs c a

d

c s

− −

−

⋅ ⋅⋅ ⋅ ⋅

        

      

(5)

6 65 5 6 65 5

6

0 00 0 0 00 0

0 0 10 1 0 0 0 0 0 1 0 0 0 1 0 0 0 1

x x x x y y y y

z z z z

n o a pc sc s n o a ps cs c

d n o a p

− −⋅ ⋅ =

                     

where:

1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6( ( ) ) ( )xn c c c c c s s s s c s s c c c s= − − + −

1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6( ( ) ) ( )yn s c c c c s s s s c c s c c c s= − − − −

23 4 5 6 4 6 23 5 6( )zn s c c c s s c s c= − −

1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6( ( ) ) ( )xo c c c c s s c s s s s s c s c c= − + + − −

1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6( ( ) ) ( )yo s c c c s s c s s s c s c s c c= − + + + −

23 4 5 6 4 6 23 5 6( )zo s c c s s c c s s= − + − (6)

1 23 4 5 23 5 1 4 5( )xa c c c s s c s s s= + +

1 23 4 5 23 5 1 4 5( )ya s c c s s c c s s= + −

23 4 5 23 5za s c s c c= −

1 1 2 2 3 23 4 23 6 23 4 5 23 5 6 1 4 5 ( ( ))

x p c a a c a c d s d c c s s c d s s s= + + + + + +

1 1 2 2 3 23 4 23 6 23 4 5 23 5 6 1 4 5 ( ( ))

y p s a a c a c d s d c c s s c d c s s= + + + + + −

2 2 3 23 4 23 6 23 4 5 23 5 ( )

z p a s a s d c d s c s c c= + − + −

In the equations (6) it is make the following notation: cos( )

i i c θ= , sin( )

ij i j s θ θ= + .

IV. NUMERICAL FORWARD KINEMATIC ANALYSIS OF 6 DOF - LR MATE 200IC ROBOT AND COMPARISONS To carry out the numerical forward kinematic I used Robotic Toolbox ® (version 9.9) for MATLAB® developed by Peter Corke professor at Queensland University of Technology. The Toolbox has always provided many functions that are useful for the study and simulation of classical arm- type robotics, for example such things as kinematics, dynamics, and trajectory generation. The Toolbox provides functions for manipulating and converting between datatypes such as: vectors; homogeneous transformations; roll-pitch-yaw and Euler angles and unit-quaternions which are necessary to represent 3-dimensional position and orientation. These parameters are encapsulated in MATLAB objects, robot objects can be created by the user for any serial-link manipulator. It can operate with symbolic values as well as numeric. The Toolbox is based on a very general method of

representing the kinematics and dynamics of serial-link manipulators [4]. The Robotic Toolbox is an open-source, available for free and its routines are generally written in a straightforward manner which allows for easy understanding [7]. Numerical values for LR Mate 200iC Robot from [5] are:

1 0.075[ ]a m= ,

2 0.300 [ ]a m= ,

3 0.075[ ]a m= ,

4 0.320 [ ]d m= and

6 0.080 [ ]d m= .

In order to create a model of the LR Mate 200iC Robot using Robotic Toolbox , the D-H parameters from Table 1 have been used.

Firstly a vector of ( , , , , )

i i i i i Link d aθ α σ objects was

created and after that it was used SerialLink command to create the model of robot with the name LRMATE200iC (fig.4). The parameters of Link object are the D-H parameters defined in section 2: joint angle ( iθ ), link offset ( id ), link length ( ia ), link twist ( iα ) and a parameter for type of joint

iσ ( 0iσ = for revolute joint and 1iσ = for prismatic joint). The fkine(q) method (fig.5) use for calculation the same algorithm presented in section 2 [4], [7] and is the pose of the robot end-effector as an SE(3) homogeneous transformation ( 4 4× ) for the joint configuration q (1× n ), where q is interpreted as the generalized joint coordinates. In case of LR Mate 200iC Robot (six revolute joints robot) the generalized joint coordinates is represented by a vector (1 6)× : [ ]1 2 3 4 5 6q θ θ θ θ θ θ= (7)

A pose of the robot, based on the kinematic model, can be visualized graphically using plot(q) method (Line 3 in Table 2). A stick figure polyline joins the origins of the link coordinate frames [7]. It was implemented the equation (6) in a Matlab function in order to calculate the forward kinematics based on algorithm presented in section 2.

Fig. 5 Homogeneous transformation of robot in

Robotic Toolbox

Fig. 4 Creating the model of robot in Robotic Toolbox

New Developments in Mechanics and Mechanical Engineering

ISBN: 978-1-61804-288-0 93

In order to validate the model of forward kinematic further tests were conducted. It was imposed three generalized coordinate vector q (line 1 in table 2) to be use. The first pose it is named the rest pose. The result from use of equation (6) is presented in Table 2 (line 2). In the line 3 of Table 2 is presented the representation of the three pose using the plot(q) method from Robotic Toolbox. The homogeneous transformation for the joint configuration resulted from fkine method is presented in line 4 (Table 2). The real pose of LR Mate 200iC Robot for each q is presented in line 5 from Table 2. The line 6 from Table 2 presents the capture image from robot’s teaching pendant (a control box for programming the motions of a robot) display of position joints angle. The final line from Table 2 presents teach pendant display capture

image for the position and orientation (equivalent for kinematic equations). The LR Mate 200iC Robot teach pendant use the following notation: Ji for joint angle ( iθ ) , [ ]X Y Z for position vector p [ ]x y zp p p= and [ ]W P R for rotation angle

around axes [ ]x y z . For compute the rotation matrix of the homogeneous transformation it was the following equation: ( ) ( ) ( )z y xR Rot R Rot P Rot W= ⋅ ⋅ (8)

where ( )zRot R is the rotation matrix around axis z with R angle ( ( )yRot P and ( )xRot W similarly) [6].

Table 2 Results q 0 0 0 0 0

2 π 

   

50 0 4 12 3 2 π π π π

− −     

12 3 12 6 4 6 π π π π π π

− −     

FK

eq . (

6)

R ob

ot P

os es

(R

T B

)

FK

(R T

B )

L R

M at

e 20

0i C

R ob

ot p

os es

A ng

le s

R ob

ot

FK

R ob

ot

New Developments in Mechanics and Mechanical Engineering

ISBN: 978-1-61804-288-0 94

Taking into account all from above it can be deliver the final homogeneous transformation of the robot:

0 1 R p

T  

=    

(9)

The difference between angle from teach pendant display and generalized coordinate vector (line 1 and 6 from Table 2) results as robot software makes some adjustments in order to protect robot from fails: angle of joint 2 is considered in opposite direction and added with 2π ; when robot is moving axis 2 with a value, the same value is subtracted from angle of joint 3 and angle of joint 4 is considered in opposite direction. Note that graphical representation of the kinematic model (line 3) and capture of the robot (line 5) are the same. Taking in consideration remarks from previous paragraph there is almost a perfect match between the three types of results: analytically, numerically and data from robot software (small differences appear on teach pendant display resulted from the precision of the robot ± 0.02[mm])[5].

V. CONCLUSIONS In this paper it was studied the forward kinematics by two different techniques: analytically using a five step algorithm derived from Denavit and Hartenberg notation and numerically using Robotic Toolbox ® for MATLAB® developed by Peter Corke. We make all calculation on LR Mate 200iC Robot from Fanuc Robotics and in order to validate the model of forward kinematic we compare the results with what robot’s software measurated and displayed on teach pendant. Summing up the results, it can be concluded that is almost perfect match between the three types of results: analytically, numerically and data from robot software. Also this paper shows that using Robotic Toolbox ® for MATLAB® is a very useful and fast tool to study the forward kinematics for industrial robot. The information can be use in the future design of the robot to make it faster and more accurate. In our future research we intend to concentrate on Inverse Kinematics, Jacobian and Dynamics of the robot LR MATE 200iC.

ACKNOWLEDGMENT: This paper has been financially supported within the project entitled “Horizon 2020 – Doctoral and Postdoctoral Studies: Promoting the National Interest through Excellence, Competitiveness and Responsibility in the Field of Romanian fundamental and Applied Scientific Research”, contract number POSDRU/159/1.5./140106. This project is co-financed by European Social Fund through Sectoral Operational Programme for Human Resources Development 2007/2013. Investing in people!

REFERENCES [1] Paul, Richard (1981),”Robot manipulators: mathematics, programming,

and control: the computer control of robot manipulators”. MIT Press, Cambridge, MA. ISBN 978-0-262-16082-7.

[2] Denavit, Jacques; Hartenberg, Richard Scheunemann (1955), "A kinematic notation for lower-pair mechanisms based on matrices", Trans ASME J. Appl. Mech 23: 215–221.

[3] Hartenberg, Richard Scheunemann; Denavit, Jacques (1965), Kinematics synthesis of linkage, McGraw-Hill series in mechanical engineering, New York: McGraw-Hill, p. 435.

[4] Corke, Peter (2011) Robotics, Vision and Control: Fundamental Algorithms in MATLAB, ISBN 978-3-642-20144-8,. Springer Tracts in Advanced Robotics, 73. Springer, Berlin.

[5] Fanuc Robotics Europe S.A. (2015, February 12) , LR MATE SERIES, Retrieved from http://www.fanucrobotics.co.uk/

~/media/Global/Files/Downloads/Datasheets/EN/LR_Mate_EN.ashx [6] M. Spong, S. Hutchinson, and M. Vidyasagar, Robot modeling and

control. Wiley, 2006. [7] http://www.petercorke.com/RTB/robot.pdf

New Developments in Mechanics and Mechanical Engineering

ISBN: 978-1-61804-288-0 95