Professional Documents
Culture Documents
no.4
vol.1
march./2011
no.4
vol.1
march./2011
example, cutting speed of a wire or a milling machine affects the surface finish
condition as well as the tool life. The travel speed of a spray-painting robot
governs the uniformity of the film thickness. Velocity of a weld gun should be
maintained constant for equal heat penetration along the weld seam. For
these purposes, joint rates of a robot are commonly computed from a required
tool velocity through the inversion of the Jacobian matrix. The Jacobian is a
6x5 matrix for a five-axis robot. It has no inverse solution because it is not a
square matrix. Usually, get a least square solution from the Jacobian matrix by
pseudo-inverse method. Pseudo-inverse of the Jacobian matrix provides a
possibility to solve for approximate solutions. There is no exact velocity
solution for five-axis robot.[2]
Pieper and Roth [3] showed that any 6DOF manipulator hose wrist axes
intersect at a common point has a closed form inverse solution to system,
owing to decoupling of the inverse position and inverse orientation. Paul and
Mayer [4] presented the basis for all advanced manipulator control which is
relationship between the Cartesian coordinates of the end-effector and
manipulator joint coordinate. Dieh, Z. [5] proposed a closed form solution
algorithm for the solving the inverse position and orientation problem for the
robot by imposing a castrating condition to the problem and projection the tool
frame on subspace of the robot. Sugimoto and Duffy [6] added two virtual axes
to a five-axis robot. The robot then became a seven-axis closed loop
mechanism so that it can be solved. Tsai and Morgan [7] used continuation
and dual number methods respectively, to solve the position of five-axis
robots. Hunt [8] applied a constraint to the tool velocity and added a dummy
axis to the five-axis robot so that the matrix has inverse.
In this paper, the mathematical model and kinematical analysis of the TR
4000 educational robot manipulator is studied. So providing method to derived
5x5 Jacobian matrix based on a five parameter to give the exact velocity and
acceleration solution.
2-Kinematics:
TR 4000 robot arm has five directions of motion (DOF) plus a grip
movement. It is also similar to human arm from the number of joints point of
view. These joints provide shoulder rotation, shoulder back and forth motion,
elbow motion, wrist up and down motion, wrist rotation and gripper motion. A
graphical view of all the joints was displayed in Fig.-1. TR 4000 robot has five
rotational joints and a moving grip. Joint 1 represents the base and its axis of
motion is Z0. This joint provides a rotational 1 angular motion around Z0 axis
in X0Y0 plane. Joint 2 is identified as the shoulder and its axis is perpendicular
to Joint 1 axis. It provides an angular motion 2 in X1Y1 plane. Z axes of Joint 3
(Elbow) and Joint 4 (Wrist) are parallel to Z1 axis, They provide 3 and 4
angular motions in X2Y2 and X3Y3 planes respectively. Joint 5 is identified as
the grip. Its Z4 axis is vertical to Z3 axis and it provides 5 angular motion in
X4Y4 plane. Denavit-Hartenberg (D-H), representation is used to model the
joints of TR 4000 Robot arm in Fig.-1. All the joints are assigned by using the
2
no.4
vol.1
march./2011
Y1
2
Z1
X1
Y2
a2
31
Z2
X2
Y3
a3
Z3
a4
Y4
Z4
X3
X4
Z0
1
Y0
X0
i
ai
Joint i
1
1
90
0
2
a2
2
0
3
a3
3
0
4
a4
4
-90
5
5
90
0
6
6
0
0
Table-1:D-H parameters for TR 4000 robot arm.
di
0
0
0
0
0
0
no.4
vol.1
march./2011
In most practical case, such as parts handling and assembling and spray
painting. one degree of freedom in orientation may be unnecessary and so
one joint of wrist construction can be omitted. We can treat five-degree-offreedom robot manipulators but we only need to fix this joint variable of wrist
as zero or some suitable data.
The homogenous transformation matrix , i 1Ti , which describes the position and
orientation frame i relative to frame i-1 can be formed by using:
Ci
S
i 1
Ti (q ) i
0
SiCi
CiCi
Si Si
Ci Si
Si
0
Ci
0
C1
S
0
T1 1
0
S1
aiCi
ai Si
di
...(1)
Substituting the parameters in Table-1 to obtain on transformation matrices A1
to A6:
0
0 C1
1
...(2)
C 2
S
1
T2 2
0
S2
C 3
S
2
T3 3
0
S3
C 4
S
3
T4 4
0
S4
0
1
C4
0
C2
0
0
...(3)
C3
0
0
...(4)
...(5)
0
0
0
0 a2C2
0 a 2 S 2
1
0
0
1
0 a3C 3
0 a3 S 3
1
0
0
1
a4C4
a 4 S 4
0
C 5
S
4
T5 5
0
0 S5
0 C5
1
0
0
..(6)
1
0
5
T6
0
no.4
vol.1
march./2011
0
0
0
0 0 0
1 0 0
0 1 0
0 0 1
..(7)
A: Forward kinematics
Calculating the position and orientation of the end effectors with given
joint angles is called Forward Kinematics analysis. Forward Kinematics
equations are generated from the transformation matrixes and the forward
kinematics solution of the arm is the product of these six matrices identified as
0
T6 (with respect to base). The first three columns in the matrices represent
the orientation of the end effectors, whereas the last column represents the
position of the end effectors.
0
T6 0T1*1T2 *2 T3*3T4 *4 T5 *5 T6
R
T6
03
0
nx
P6 n y
1 nz
ox
ax
oy
ay
oz
az
Px
Py
Pz
no.4
vol.1
march./2011
ay = S1C234 S5 - C1 C5
(15)
az= S234 S5
(16)
Px = C1(C23a3 + C2a2+C234 a4)
(17)
Py= S1(C23a3 + C2a2+C234 a4)
...(18)
Pz =S23a3 + S2a2+S234 a4
(19)
B: Inverse Kinematics:
The inverse position and orientation problem demands the
transformation from task space, 0XT , to joint space coordinates, ,
= f -1(0XT)
..(20)
In general, the inverse kinematics problem can be solved either by an
algebraic, an iterative, or geometric approach. The closed form solution is very
difficult and suffers from the fact that the solution does not give a clear
indication on how to select the correct solution from several possible solutions
for a particular arm configuration. The iteration solution of ten equations
requires more computation, and it does not guarantee convergence to the
correct solution, especially in the singular and degenerate cases. The
geometric approach presents a better approach when the manipulator is
simple but it is not suitable for computer served robots. The closed form
solutions are preferable for two reasons. The first, the forward kinematics
equations must be solved at a rapid rate and the second, kinematics equations
in general have multiple solutions.
Inverse Kinematics analysis determines the joint angles for desired
position and orientation in Cartesian space. This is more difficult problem than
forward kinematics. The Transformation matrix 0T6 will be used to calculate
inverse kinematics equations. To determine the joint angles, 0T6 matrix
equation is multiplied by 0T1-1 on both sides sequentially.
0 1 0
...(21)
T1 * T6 0T11*1T2 *2 T3 *3T4 *4 T5 *5 T6
n x C1 n y S1
nz
n x S1 n y C1
C 234 C 5
S C
234 5
S5
o x C1 o y S1
oz
no.4
vol.1
march./2011
a x C1 a y S1
az
Px C1 Py S1
Pz
o x S1 o y C1 a x C1 a y S1 Px S1 Py C1
0
0
1
0
C5
0
0
0
1
(22)
Both matrix elements are equated to each other and the resultant i
values are extracted. Equating (3,4) elements of both matrices gave in
Eq.(22):
Px S1 Py C1 0
...(23)
1 a tan 2( Py , Px )
or 1 1 180
...(24)
Equating (1,4) and (2,4) elements of both matrices at Eq.(22) gives:
Px C1 Py S1 a 4 C 234 a3 C 23 a 2 C 2
...(25)
Pz a 4 S 234 a3 S 23 a 2 S 2
...(26)
Rearranging and squaring Eq.(25) and Eq.(26) and adding them
( Px C1 Py S1 a 4 C 234 ) 2 ( Pz a 4 S 234 ) 2 a32 a 22 2 * a 2 * a3
Hence,
...(27)
...(28)
3 A tan 2( S 3 , C 3 )
...(29)
Referring Eqs.( 25) and ( 26 )
2 A tan 2( S 2 , C 2 )
..(33)
Equating (3,1) and (3,2) in Eq(22),
S5 C1n y S1nx
..(34)
C5 C1a x S1a y
..(35)
5 A tan 2( S 5 , C 5 )
..(36)
...(30)
...(31)
..(32)
no.4
vol.1
march./2011
Fig-2:
GUI
for
both
..(37)
X J ( ) J ( )
..(38)
Where J() denotes to the (m x n) Jacobian matrix and J ( ) denotes to
the time derivative of the Jacobian matrix. We can solve the joint velocities i
and joint acceleration i by:
J 1 ( ) X
..(39)
J 1 ( )( X J ( ) )
..(40)
Where J-1() denotes to the inverse kinematics. In general, the (6x5)
Jacobian matrix is non-square. In which case the inverse is undefined. In this
8
no.4
vol.1
march./2011
(41)
sign( I x )
2 2
Iz
tan 1
I x2 I y2
(42)
(43)
Then a 5_5 Jacobian matrix can be derived from
no.4
dh h h h h h 2
3 J i
dt 1 2 3 4 5
4
5
Px
1
Py
1
P
[J ] z
1
1
1
Px
2
Py
Px
3
Py
Px
4
Py
2
Pz
2
3
Pz
3
4
Pz
4
vol.1
march./2011
(44)
Px
5
Py
5
Pz
5
5
5
(45)
The Jacobian matrix is a transformation matrix from the joint space to
the spatial line location space. If the i-th axis of the robot is a prismatic joint,
then the i-th column in Eq. (44), i should be replaced by the joint off-set, di,
and the corresponding joint rate, i (in angular term), should be replaced by i
(in translational term).
From homogenous transformation of the end-effector (0T6), the position
and orientation of the end effector are:
Px C1 (a3 C 23 a 2 C 2 d 6 C 234 S 5 ) d 6 S1C 5
P Py S1 (a3 C 23 a 2 C 2 d 6 C 234 S 5 ) d 6 C1C 5
Pz
a3 S 23 a 2 S 2 d 6 S 234 S 5
And
S 234 S 5
...(47)
The spherical angles are:
S C S C1C 5
tan 1 1 234 5
C1C 234 S 5 S1C 5
...(48)
10
(46)
S 234 S 5
tan 1
S2 S C2
5
234 5
(49)
no.4
vol.1
march./2011
(50)
And
I x C 234 S 5
I I y S 234 S 5
I z C 5
...(51)
The spherical angles are:
S S
1 tan 1 234 5
C 234 S 5
..(52)
C5
1 tan 1
S5
..(53)
The given velocity of the tool is assigned in the fixed reference frame. It
should be transferred to the frame 1 also. The contribution of joint 1 to the tool
velocity should be deleted from the original tool velocity.
2
dh1 h1 h1 h1 h1 3
dt 2 3 4 5 4
5
(54)
Px' 0
'
Py 0
= Pz' 1Px1
' 0
1C234
(55)
11
no.4
vol.1
march./2011
T
Where the Px' Py' Pz' ' ' is the tool velocity transferred to the frame 1.
Rearranging Eq.(55 )
Px' 0
'
Py
Pz' Px1
'
0
C
234
Px1
2
Py1
Px1
3
Py1
Px1
4
Py1
2
Px1
2
1
2
1
2
3
Px1
3
1
3
1
3
4
Px1
4
1
4
1
4
0
(a4S234
a3S23a2S2)
0
(a4C
a3C
a2C
234
23
2)
J(a4C
a3C
a2C
0
234
23
2)
0
1
C
0
234
Px1
5
Py1
1
5
2
Px1
3 J
5
1 4
5 5
1
5
(56)
(a4S234
a3S23)
a4S234
(a4C
a3C
)
234
23
0
a4C
234
0
0
0
1
0
1
0
0
1
...(57)
This is the 5X5 Jacobian of the robot with respect to the frame 1, and it
is no difficulty to inverse the Jacobian matrix analytically.
The joint velocities are given by:
1 Pz1 / Px1
(58)
) /(a a S a a S )
2 (a2C23 Px1 a3 S 23 Py1 a3a4 S 4
1
3 4 4
3 2 3
) /(a a S a a S )
3 (a2C23 Px1 a2C2 Px1 a3 S 23 Py1 a2 S 2 Py1 S34
1
3 4 4
3 2 3
) /(a a S a a S )
4 (a2C2 Px1 a2 S 2 Py1 S34
1
3 4 4
3 2 3
(59)
(60)
...(61)
1
5 C34 Pz1 / Px1
...(62)
...(63)
) /( a a S a a S )
2 (a2C23 A a2 S 23 B a3a4 S 4
1
3 4 4
3 2 3
) /( a a S a a S )
3 ((a3C23 a2C2 ) A (a3 S 23 a2 S 2 ) B S34
1
3 4 4
3 2 3
) /( a a S a a S )
4 (a2C2 A a2 S 2 B S34
1
3 4 4
3 2 3
(66)
1 S 2341
5 C234 D / Px1
(67)
Where
12
(64)
(65)
no.4
vol.1
march./2011
P (a C a C ) a C
AP
x1
x1 2
4 234
3 23
3
4 234 4
P (a S a S ) a S
BP
y1
y1 2
234
3 23
234 4
P
DP
z1
y1 1
3-Conclusions:
Finding closed form solution to the inverse kinematics problem is
desirable since because it is required less computational time and provides
accurate multiple solutions. In this paper, a closed form solution to the inverse
kinematics of a 5-DOF robot is proposed. A software program was also
developed to show the robot motion with respect to its mathematical analysis.
Almost used pseudo inverse method to derive the 6x5 Jacobian matrix.
The solution based on six freedoms inverse velocity analysis is just an
approximation with a least-square error. For a five axis robot, the Jacobian is
not a square matrix, and it has no inverse. In this paper, presents method to
compute the joint velocity and acceleration of fives axis robot. This method
based on the spherical angle of the tool to derive 5x5 Jacobian matrix for
which the exact solutions are obtainable.
4-Reffrences:-
13