3URFHHGLQJVRI7KH,(((,QWHUQDWLRQDO

&RQIHUHQFHRQ5HDOWLPH&RPSXWLQJDQG5RERWLFV
-XO\2NLQDZD-DSDQ

Analysis of the inverse kinematics for 5 DOF

robot arm using D-H parameters
Apurva Patil Maithilee Kulkarni Ashay Aswale
Department of Department of Electronics and Department of
Mechanical Engineering Telecommunication Engineering Mechanical Engineering

College of Engineering Pune (COEP), Shivajinagar, Pune, India.

Abstract - This paper proposes an al- angles. Inverse kinematics is employed to

gorithm to develop the kinematic model of reach the desired position of the end effector.
a 5 DOF robot arm. The formulation of Solving the system of equations for the joint
the problem is based on finding the D-H angles will move the end effector to a partic-
parameters of the arm. Brute Force it- ular position.
erative method is employed to solve the There are many solutions to solve the inverse
system of non linear equations. The focus kinematics problem, such as geometric, alge-
of the paper is to obtain the accurate so- braic, and numerical hit and trial, iterative,
lutions by reducing the root mean square FABRIC methods [1]. But it is hard to find
error. The result obtained will be imple- a consistent solution using analytical method
mented to grip the objects. The trajec- [2]. Hence often iterative methods are used
tories followed by the end effector for the [3]. One of the iterative methods uses the in-
required workspace coordinates are plot- verse Jacobian matrix approach. But it faces
ted. The methodology used here can be problems of singularity and computational
used in solving the problem for any other complexity [4]. A closed-form joint solution
kinematic chain of up to six DOF. is derived for a 6-DOF humanoid robot arm
Keywords - 5 DOF robot arm; D-H in [5]. The geometric method used in [6] uses
parameters; Inverse kinematics; Iter- geometric perception. The inverse transform
ative method; Trajectories technique is another method used in [7] to
obtain the joint solution of a 6-DOF robot
I. INTRODUCTION arm whereas the method discussed in [8] uses
technique of minimization of error vector. In
This paper presents an approach to control this paper a simple and efficient technique
the position of a 5 DOF robot arm using to develop the kinematic model of a robot
kinematics between rigid links. It is desirable manipulator is discussed.
to control the end effector in the Cartesian
coordinate space but motor system needs the II. TRANSFORMATION USING D-H
reference inputs in the joint space. Hence to PARAMETERS
know the conversion between Cartesian and
joint space, inverse and forward kinematics In any robot arm the position and orienta-
are essential. Forward kinematics gives the tion of the base are known. To locate the
location of the end effector for the given joint end effector the relationship between the base

‹,((( 
 ⎡ ⎤
frame and the end effector frame is found out cθi −sθi 0 ai−1
⎢sθi cαi−1 cθi cαi−1 −sαi−1 −sαi−1 di ⎥
by transformation. The frames are attached i−1
Ti = ⎢
⎣sθi sαi−1 cθi sαi−1 cαi−1 −cαi−1 di ⎦
⎥
to all the links of the robot arm.
Z axis: along the joint axis 0 0 0 1
X axis: along common normal (1)
Y axis: according to the RH frame The final transformation matrix from the
(i−1)
Ti is the homogeneous transformation base to the end effector is obtained as
matrix between frame Ri−1 and Ri which T = 0 T 1 1 T 2 2 T 3 3 T 4 ...n−1 T n (2)
takes into account the rotation of the frame ⎡ ⎤
Ri w.r.t. frame Ri−1 and the translation of nx ox ax px
⎢ny oy ay py ⎥
the origin of frame Ri w.r.t origin of frame T =⎢ ⎥
⎣ n z o z az p z ⎦ (3)
Ri−1 . The transformation matrix (i−1) Ti is
obtained by four parameters ai ,αi ,di and θi 0 0 0 1
which are called as Denavit-Hatenberg (D-H) This matrix depends on the joint angles of
parameters. the links between the base and the end effec-
ai : distance (zi ,zi+1 ) along xi tor.
αi : angle (zi ,zi+1 ) about xi T = f (θi ) where, 1 ≤ i ≤ n (4)
di : distance (xi−1 ,xi ) along zi
At the given configuration the encoder values
θi : angle (xi−1 ,xi ) about zi
of the joint angles are read and the transfor-
To attach the frames to the first and the last
mation matrix T is calculated. The end ef-
links, it is better to try to make a0 = an = α0
fector configuration parameters are obtained
= αn = 0 and to keep the constant parame-
from this matrix. These parameters are used
ters among θ1 , θn , d1 , dn zero. This reduces
to find the trajectories, the velocities and the
the complexity. Thus, the following analysis
acceleration.
makes use of this approach.
i−1
Ti = Rx (αi−1 )Dx (ai−1 )Rz (θi )Dz (di ) III. METHODOLOGY TO SOLVE
where, INVERSE KINEMATICS PROBLEM
⎡ ⎤
cθi −sθi 0 0 Inverse kinematics can be solved by following
⎢sθi cθi 0 0⎥
Rz (θi ) = ⎢⎣0
⎥ two ways:
0 1 0⎦
0 0 0 1 1. Analytical method: The solution is ob-
⎡ ⎤ tained with closed form of equations.
1 0 0 0 2. Numerical method: The solution is
⎢0 1 0 0 ⎥ obtained with an iterative approximation
Dz (di ) = ⎢⎣0 0 1 di ⎦
⎥
method.
0 0 0 1 It is complex to solve the system of non lin-
⎡ ⎤
1 0 0 ai−1 ear equations having trigonometric functions
⎢0 1 0 0 ⎥ analytically. Hence, the analysis carried out
Dx (ai−1 ) = ⎢ ⎣0 0 1 0 ⎦
⎥
uses iterative numerical method.
0 0 0 1 The analysis includes the following steps:
⎡ ⎤
1 0 0 0 1. Construct the line diagram of the arm with
⎢0 cαi−1 −sαi−1 0 ⎥ all the joints and links connecting the joints
Rx (αi−1 ) = ⎢⎣0 sαi−1 cαi−1 di⎦
⎥
2. Assign the frame to each of the joints and
0 0 0 1 find the D-H parameters


3. Construct the transformation matrices be- The values of l1 and l2 are 28 cm and 30 cm
tween the successive 2 joints respectively.
4. Multiply all these matrices to form the The CAD model of 5 DOF arm along with
total transformation matrix (symbolic) be- its different joints is as shown in fig. 2.
tween the base and the end effector
5. Construct the (numeric) transformation
matrix to the target point
6. Form the system of equations by equating
these two matrices.
7. Solve the system of equations using itera-
tive approximation method to get the values
of θ0 , θ1 ,...,θn
8. Find root mean sqaure error for each ob-
tained solution using forward kinematics and
select suitable solution accordingly
This methodology being quite generic can be
used for any other kinematic chain of up to
six DOF.
Figure 2: Arm Design

IV. KINEMATICS SOLUTION OF ARM

The frame attachment to 5 joints of the arm
The schematic representation of kinematics is as shown in fig. 3.
of arm is as shown in fig. 1.

Figure 1: Kinematic diagram of Arm Figure 3: Frame Attachment of Arm


Frame 0 is aligned with frame 1 and frame of the gripper is considered to be aligned with
frame 5.
The D-H parameters found using this coordinate system are as shown in table I.

TABLE I: D-H PARAMETERS OF ROBOT ARM

Joint Frame i ai αi di θi
Base Frame 0 0 0 − −
Shoulder pitch Frame 1 0 −π/2 0 θ1
Shoulder roll Frame 2 0 π/2 0 -θ2 +π/2
Elbow yaw Frame 3 0 π/2 l1 θ3
Elbow roll Frame 4 0 −π/2 0 θ4
Wrist yaw Frame 5 0 0 l2 θ5

V. ANALYSIS OF FORWARD AND px = 0.28cθ1 cθ2 + 0.3sθ4 (sθ1 sθ3 + cθ1 cθ3 sθ2 ) +
INVERSE KINEMATICS 0.3cθ1 cθ4 cθ2
py = 0.28sθ1 cθ2 − 0.3sθ4 (cθ1 sθ3 − cθ3 )sθ2 sθ1 +
0.3cθ4 sθ1 cθ2
pz = −0.28sθ2 − 0.3cθ4 sθ2 + 0.3cθ3 sθ4 cθ2
These DH parameters are substituted in
(8)
transformation matrix and LHS and RHS
of equation(3) are equated using equations
(1) and (2) to obtain: The desired coordinates of the end effector
px , py and pz are substituted in (8). This set
nx = −cθ5 cθ4 (sθ1 sθ3 +cθ1 cθ3 sθ2 )−cθ1 sθ4 cθ2 − of equations is used to find θ1 , θ2 , θ3 and θ4 .
sθ5 (cθ3 sθ1 − cθ1 sθ2 sθ3 ) The values of θ1 , θ2 , θ3 and θ4 are found using
ny = cθ5 cθ4 (cθ1 sθ3 − sθ1 cθ3 sθ2 ) + sθ1 sθ4 cθ2 + iterartive methd. The problem is to solve set
sθ5 (cθ3 cθ1 + sθ1 sθ2 sθ3 ) of three equations for four unknowns θ1 , θ2 ,
nz = cθ5 (−sθ2 sθ4 − cθ2 cθ3 cθ4 ) + θ3 and θ4 .
sθ3 sθ5 cθ2 The solution of position coordinates is ob-
(5) tained using brute force iterative method in
embedded c and the algorithm followed is as
ox = sθ5 (cθ4 (sθ1 sθ3 +cθ1 cθ3 sθ2 )−cθ1 sθ4 cθ2 )− shown in fig. 4
4. The complexity of the algo-
cθ (cθ sθ − cθ sθ sθ ) rithm is N .
5 3 1 1 2 3
oy = cθ5 (cθ1 cθ3 +sθ2 sθ1 sθ3 )−sθ5 (cθ4 (cθ1 sθ3 − The variables funx, funy or funz represent
cθ3 sθ2 sθ1 ) + sθ1 sθ4 cθ2 ) difference between calculated value of px, py
oz = cθ5 sθ3 cθ2 + sθ5 (sθ2 sθ4 + cθ3 cθ4 cθ2 ) or pz and the desired (actual) value of px, py
or pz. Thus, their values should be as close
(6)
to zero as possible.
The root mean square error for each solu-
ax = sθ4 (sθ1 sθ3 + cθ1 cθ3 sθ2 ) + cθ1 cθ4 cθ2
tion found is calculated and the appropriate
ay = cθ4 sθ1 cθ2 − sθ4 (cθ1 sθ3 − cθ3 sθ2 )sθ1
solution is selected.
az = −cθ4 sθ2 + cθ3 sθ4 cθ2
(7)


 Figure 5: Graph 1

Figure 6: Graph 2

Figure 4: Flowchart of Algorithm

VI. RESULT AND ANALYSIS

The algorithm is tested for over 20 sample

test cases and is found to be working effi- Figure 7: Graph 3
ciently.
Initially, θi =0 ∀ 1≤i≤ 5 and hence, the de- These solution trajectories are found in joint
fault position of the arm is (58,0,0). The space. Here, target coordinates are known.
three sample trajectories of end effector Inverse kinematics is solved and coordinates
shown in fig. 5, 6 and 7 are for desired coor- of each joint are found. By joining these
dinates of end effector as points to initial points, the trajectory is cre-
(px , py , pz ) = (0,58,0), (px , py , pz ) = (28,0,30) ated.
and (px , py , pz ) = (50,29,0).


 VII. CONCLUSION [3] M. Mistry, J. Nakanishi, G. Cheng, and S.
Schaal, Inverse kinematics with floating base
The kinematic model of a robot arm having 5 and constraints for full body humanoid robot
DOF has been developed in this paper. The control, in Proc. of the IEEE-RAS Intl. Conf.
kinematic analysis of the arm is carried out on Humanoid Robots, 2008, pp.2227.
considering D-H parameters. A consistent [4] J. Wang and Y. Li, Inverse kinematics anal-
joint solution for the arm is found using the ysis for the arm of a mobile humanoid robot
inverse transform. The arm can be moved to based on the closed-loop algorithm, in Proc. of
the desired position by global planning using the IEEE Information and Automation (ICIA),
two shoulder and two elbow actuators. The 2009, pp.516521.
local planning can be done by the fifth actu- [5] Y. Cui, P. Shi, and J. Rua, Kinematics anal-
ator fitted at the wrist which can be used to ysis and simulation of a 6 − dof humanoid robot
change the orientation of the arm. Finally, manipulator, in Proc. of the IEEE Intl. Asia
the trajectories followed by the end effector Conf. on Informatics in Control, Automation
to attain some desired positions are plotted. and Robotics (CAR), vol. 2, Mar 2010, pp.
246249.
ACKNOWLEDGMENT [6] K. S. Fu, R. C. Gonzalez, and C. S. G. Lee,
Robotics: Control, Sensing, Vision, and Intelli-
Special thanks to our mentor and faculty ad- gence, McGraw-Hill, 1987.
visor Prof. Dr. S.S. Ohol, Department of [7] R. P. Paul, B. E. Shimano, and G. Mayer,
Mechanical Engineering and Prof. Dr. S. Kinematic control equations for simple manipu-
P. Mahajan, Department of Electronics and lators, in IEEE Transactions on Systems, Man
Telecommunication Engineering, COEP. and Cybernetics, vol. 11, 1981, pp. 449455.
[8] Juyi Park, Jung-Min Kim, Hee-Hwan Park,
Jin-Wook Kim, Gye-Hyung Kang, and Soo-Ho
REFERENCES
Kim, An Iterative Algorithm for Inverse Kine-
[1] Andeas Aristidou and joan Lasenby, FABRIC matics of 5-DOF Manipulator with Offset Wrist,
: A fast iterative solver for the inverse kinematic World Academy of Science, Engineering and
problem, ELSEVIER 2011. Technology International Journal of Mechanical,
[2] M. A. Ali, H. A. Park, and C. S. G. Lee, Aerospace, Industrial, Mechatronic and Manu-
Closed-form Inverse Kinematic Joint Solution facturing Engineering Vol : 6, No : 12, 2012.
for Humanoid Robots, pp. 704709, 2010.