Analytical Inverse Kinematics Algorithm of 5 Dof Robot Arm

1Journal of education of college no.4 vol.1 march./2011

Analytical Inverse kinematics AlgorithmOf A 5-DOF Robot Arm

Mustafa Jabbar HayawiComputer science Dept.Educational collegeThi-Qar University

[email protected]

The kinematics problem is defined as the transformation from theCartesian space to the joint space and vice versa. Both forward and inversekinematics solutions for the TR 4000 educational robot arm are presented.The closed form solution of the inverse kinematics problem is of utmostimportance in controlling robotic manipulator. In this paper, a closed formsolution to the inverse kinematics of a 5-DOF robot is presented to overcomethe relatively slow iterative numerical solution. A software program developedto compute the forward and inverse kinematics of TR4000 robot arm.

Inverse velocity problem for five axis robots is investigated. Theconventional method for a five axis robot is to pseudo inverse the 6x5Jacobian matrix. Pseudo inverse of the Jacobian matrix provides a possibilityto solve for approximate solutions. The spherical angles solution is provided toderive a 5x5 Jacobian matrix.Keywords: forward kinematics, inverse kinematics, inverse velocity, Jacobianmatrix.

Introduction:-1The motion of a rigid body in three-dimensional space (3-D) has sixdegrees of freedom. For incompletely specified spatial motion, less than sixfreedoms are required. A robot with four actuated axes can guide a line in 3-Dspace and a five-axis machine can guide a line segment for a specified pose.Industrial applications of these types are wire cutting, grinding and polishing,arc welding, spray painting, pin insertion, etc. Many industrial robots designedfor such jobs have only four or five axes.The kinematics solution of any robot manipulator consists of two subproblems forward and inverse kinematics. Forward kinematics will determinewhere the robots manipulator hand will be if all joints are known whereasinverse kinematics will calculate what each joint variable must be if the desiredposition and orientation of end-effector is determined. Hence Forwardkinematics is defined as transformation from joint space to Cartesian spacewhereas Inverse kinematics is defined as transformation from Cartesian spaceto joint space.[1]In many line guidance applications, velocity control of a robot is animportant issue. Speed of the robot tool governs the quality of the job. For


example, cutting speed of a wire or a milling machine affects the surface finishcondition as well as the tool life. The travel speed of a spray-painting robotgoverns the uniformity of the film thickness. Velocity of a weld gun should bemaintained constant for equal heat penetration along the weld seam. Forthese purposes, joint rates of a robot are commonly computed from a requiredtool velocity through the inversion of the Jacobian matrix. The Jacobian is a6x5 matrix for a five-axis robot. It has no inverse solution because it is not asquare matrix. Usually, get a least square solution from the Jacobian matrix bypseudo-inverse method. Pseudo-inverse of the Jacobian matrix provides apossibility to solve for approximate solutions. There is no exact velocitysolution for five-axis robot.[2]Pieper and Roth [3] showed that any 6DOF manipulator hose wrist axesintersect at a common point has a closed form inverse solution to system,owing to decoupling of the inverse position and inverse orientation. Paul andMayer [4] presented the basis for all advanced manipulator control which isrelationship between the Cartesian coordinates of the end-effector andmanipulator joint coordinate. Dieh, Z. [5] proposed a closed form solutionalgorithm for the solving the inverse position and orientation problem for therobot by imposing a castrating condition to the problem and projection the toolframe on subspace of the robot. Sugimoto and Duffy [6] added two virtual axesto a five-axis robot. The robot then became a seven-axis closed loopmechanism so that it can be solved. Tsai and Morgan [7] used continuationand dual number methods respectively, to solve the position of five-axisrobots. Hunt [8] applied a constraint to the tool velocity and added a dummyaxis to the five-axis robot so that the matrix has inverse.In this paper, the mathematical model and kinematical analysis of the TR4000 educational robot manipulator is studied. So providing method to derived5x5 Jacobian matrix based on a five parameter to give the exact velocity andacceleration solution.Kinematics:-2

TR 4000 robot arm has five directions of motion (DOF) plus a gripmovement. It is also similar to human arm from the number of joints point ofview. These joints provide shoulder rotation, shoulder back and forth motion,elbow motion, wrist up and down motion, wrist rotation and gripper motion. Agraphical view of all the joints was displayed in Fig.-1. TR 4000 robot has fiverotational joints and a moving grip. Joint 1 represents the base and its axis ofmotion is Z0. This joint provides a rotational 1 angular motion around Z0 axisin X0Y0 plane. Joint 2 is identified as the shoulder and its axis is perpendicularto 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 4angular motions in X2Y2 and X3Y3 planes respectively. Joint 5 is identified asthe grip. Its Z4 axis is vertical to Z3 axis and it provides 5 angular motion inX4Y4 plane. Denavit-Hartenberg (D-H), representation is used to model thejoints of TR 4000 Robot arm in Fig.-1. All the joints are assigned by using the


principles of D-H convention. The D-H parameters are routinely noted di(motional distance along Z0 axis), ai (distance between joints along X axis), i(angle around the Z axis on XY plane), i (angle between 2 adjacent Z axes) .These parameters describe the location of a robot link-frame i (a joint) from apreceding link-frame i-1 (previous joint) through the sequence of translationsand rotations.(Table-1) .

Fig.-1: Modeling of TR 4000 robotic arm

Joint i i ia id1 1 90 0 02 2 0 2a 03 3 0 3a 04 4 -90 4a 05 5 90 0 06 6 0 0 0

Table-1:D-H parameters for TR 4000 robot arm.Now we will consider five-degree-of-freedom robot manipulators whichlose one degree of freedom in the wrist configuration, so it cannot reach theany place in space arbitrary. Simply a five-degree-of-freedom robotmanipulator can be imagined as a six degree-of-freedom robot manipulator inwhich one joint variable is fixed as zero or some suitable value. One degree offreedom lost may appear in arm part or in wrist part of the robot manipulator.The solution of inverse kinematics, in principle, can be obtained by applicationof algorithm existing for six-degree-of-freedom robot manipulator. The onlynecessary additional step is to determine which joint can represent the onedegree of freedom lost and it is not so difficult task to do so.

X0

Y0Z0

Y1 Z1

X1

Y2 Z2X2

Y3 Z3

X3

Y4

Z4

X4

a2 a3 a4

1

2 13 4

5


In most practical case, such as parts handling and assembling and spraypainting. one degree of freedom in orientation may be unnecessary and soone joint of wrist construction can be omitted. We can treat five-degree-of-freedom robot manipulators but we only need to fix this joint variable of wristas zero or some suitable data.The homogenous transformation matrix , ii T1 , which describes the position andorientation frame i relative to frame i-1 can be formed by using:

10000)(

1iii

iiiiiii

iiiiiii

ii

dCSSaSCCCSCaSSCSC

qT

...(1)Substituting the parameters in Table-1 to obtain on transformation matrices A1to A6:

100000100000

11

11

10 CS

SCT

...(2)

10000100

00

2222

2222

21 SaCS

CaSCT

...(3)

10000100

00

3333

3333

32 SaCS

CaSCT

...(4)

10000010

00

4444

4444

43 SaCS

CaSCT

...(5)


100000100000

55

55

54 CS

SCT

..(6)

1000010000100001

65T

..(7)A: Forward kinematicsCalculating the position and orientation of the end effectors with givenjoint angles is called Forward Kinematics analysis. Forward Kinematicsequations are generated from the transformation matrixes and the forwardkinematics solution of the arm is the product of these six matrices identified as0T6 (with respect to base). The first three columns in the matrices representthe orientation of the end effectors, whereas the last column represents theposition of the end effectors.

10001036

06

0

zzzz

yyyy

xxxx

T PaonPaonPaon

PR

The orientation and position of the end effectors can be calculated interms of joint angles shown in (8) to (19).nx = C1C234 C5 -S1S5(8)ny = S1C234 C5 +C1 S5(9)nz= S234C5...(10)ox= - C1S234...(11)oy= - S1 S234..(12)oz= C234...(13)ax = C1C234S5 + S1 C5...(14)

65

54

43

32

21

10

60 ***** TTTTTTT


ay = S1C234S5 - C1 C5(15)az= S234S5(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 thetransformation from task space, 0XT , to joint space coordinates, , = f -1(0XT)..(20)In general, the inverse kinematics problem can be solved either by analgebraic, an iterative, or geometric approach. The closed form solution is verydifficult and suffers from the fact that the solution does not give a clearindication on how to select the correct solution from several possible solutionsfor a particular arm configuration. The iteration solution of ten equationsrequires more computation, and it does not guarantee convergence to thecorrect solution, especially in the singular and degenerate cases. Thegeometric approach presents a better approach when the manipulator issimple but it is not suitable for computer served robots. The closed formsolutions are preferable for two reasons. The first, the forward kinematicsequations must be solved at a rapid rate and the second, kinematics equationsin general have multiple solutions.Inverse Kinematics analysis determines the joint angles for desiredposition and orientation in Cartesian space. This is more difficult problem thanforward kinematics. The Transformation matrix 0T6 will be used to calculateinverse kinematics equations. To determine the joint angles, 0T6 matrixequation is multiplied by 0T1-1 on both sides sequentially.

65

54

43

32

211

10

601

10 ****** TTTTTTTT ...(21)


100000

1000

55

22233234452342345234

22233234452342345234

11111111

11111111

CSSaSaSaSSCCSCaCaCaSCSCC

CPSPSaCaCoSoCnSnPaon

SPCPSaCaSoCoSnCn

yxyxyxyx

zzzz

yxyxyxyx

(22)

Both matrix elements are equated to each other and the resultant ivalues are extracted. Equating (3,4) elements of both matrices gave inEq.(22):

011 CPSP yx...(23)

180),(2tan 111 orPPa xy...(24)Equating (1,4) and (2,4) elements of both matrices at Eq.(22) gives:

22233234411 CaCaCaSPCP yx ...(25)

222332344 SaSaSaPz ...(26)Rearranging and squaring Eq.(25) and Eq.(26) and adding them

3222

23

22344

2234411 **2)()( aaaaSaPCaSPCP zyx ...(27)

Hence,32

22

23

22344

22344113 **2/))()(( aaaaSaPCaSPCPC zyx ...(28)

),(2tan 333 CSA...(29)Referring Eqs.( 25) and ( 26 )

23

23

22334234113342342332 )/()())(( aSaaCaCSPCPaSaSPaaCS yxz ...(30)

23

23

22334234334234112332 )/()())(( aSaaCaSPaSaCSPCPaaCC zyx ...(31)

),(2tan 222 CSA ..(32)Assume 234=k, then:

232344 ..(33)

Equating (3,1) and (3,2) in Eq(22),xy nSnCS 115

..(34)yx aSaCC 115

..(35)),(2tan 555 CSA

..(36)


An analysis introduced to reduce the multiple solutions in inversekinematics part. The values of ai* are specified. A set of i are chosen{45,45,30,-45,30} to calculate the position and orientation of the end effector0T6. A computer program do by MATLAB program used graphical userinterface (GUI) to calculate both forward and inverse kinematics solution of therobot arm.

Fig-2:GUIfor

both

forward and inverse kinematics of TR 4000 Robot* a2=a3=12;a4=14.25ccm.

Inverse Velocity and Acceleration:-CThe forward kinematics model of a robot manipulator gives the velocityand acceleration of the end effector XandX in terms of the joint velocities andacceleration and . It is written as:

)J(X ..(37)

)()( JJX ..(38)Where J() denotes to the (m x n) Jacobian matrix and )(J denotes tothe time derivative of the Jacobian matrix. We can solve the joint velocities iand joint acceleration i by:

XJ )(1 ..(39)

))()((1 JXJ ..(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


work, we used spherical angle representation line to derive a (5x5) Jacobianmatrix. The location of a tool is commonly assigned by six parameters. Theseare the position (Px, Py, Pz) of the tool and the direction (Ix, Iy, Iz) of the tool.However, a line segment can be assigned with only five parameters. The toolorientation can then be represented by two spherical angles. They are theazimuth angle and the latitudinal angle . Then the line can be representedby five parameters (Px, Py, Pz, , ), as shown in Fig. -3.The angles and should be able to cover all the directions in the 3-Dspace. The relationship between the spherical angles and the directioncosines are given by:

)(22tan1

xx

y IsignII

(41)

221tan

yx

zII

I

(42)

Fig-3: Line representation in a spherical angle

If the tool is to be guided by a five-axis robot, these five parameters arefunctions of the joint variables (1, 2, 3 4, 5). Let

.][ Tzyx PPPh (43)Then a 5_5 Jacobian matrix can be derived from

10

Journal of education of college no.4 vol.1 march./2011

iJhhhhhdtdh

54

32

1

54321(44)

54321

54321

54321

54321

54321

][

zzzzz

yyyyy

xxxxx

PPPPP

PPPPP

PPPPP

J

(45)The Jacobian matrix is a transformation matrix from the joint space tothe 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 positionand orientation of the end effector are:

5234622233

51652346222331

51652346222331)()(

SSdSaSaCCdSCdCaCaSCSdSCdCaCaC

PPP

Pz

y

x(46)

And

5234

5152341

5152341

33

23

13

SSCCSCSCSSCC

RRR

III

Iz

y

x

...(47)The spherical angles are:

515234151523411tan CSSCC

CCSCS

...(48)

11


2

552234

52341tan CSSSS

(49)The partial derivatives of and will make a very complex Jacobianexpression. We may temporarily set the reference frame at joint 1. Let P1 andI1 are the tool position and orientation with respect to the frame 1. From Eq.(22), we obtain:

222332344

222332344

222332344

1

1

1

1SaSaSaSaSaSaCaCaCa

PPP

Pz

y

x

(50)And

5

5234

5234

CSSSC

III

Iz

y

x

...(51)The spherical angles are:

523452341

1 tan SCSS

..(52)

551

1 tan SC

..(53)The given velocity of the tool is assigned in the fixed reference frame. Itshould be transferred to the frame 1 also. The contribution of joint 1 to the toolvelocity should be deleted from the original tool velocity.

54

3

2

51

41

31

211

hhhhdtdh

(54)

=

2341

11'

'

'

0

00

'C

PPPP

xz

y

x

(55)

12


Where the Tzyx PPP ''''' is the tool velocity transferred to the frame 1.Rearranging Eq.(55 )

J

C

PPPPP

PPPP

PPPP

PPP

xxxxx

yyyy

xxxx

z

y

x

5

4

3

2

1

51

41

31

21

234

51

41

31

21

51

41

31

21

1

5

1

4

1

3

1

2

151

41

31

21

'

'

'

0

0

0

'(56)

1000011100000)(0)()(00)()(0

234

222332344

23442332344222332344

23442332344222332344

C

CaCaCaCaCaCaCaCaCaSaSaSaSaSaSa

J ...(57)

This is the 5X5 Jacobian of the robot with respect to the frame 1, and itis no difficulty to inverse the Jacobian matrix analytically.The joint velocities are given by:

111 / xz PP (58)

)/()( 3234431443123312322 SaaSaaSaaPSaPCa yx (59))/()( 323443134122123312212323 SaaSaaSPSaPSaPCaPCa yyxx (60)

)/()( 3234431341221224 SaaSaaSPSaPCa yx ...(61)111345 / xz PPC

...(62)And the joint accelerations are given by:

11111 /)( xyz PPP ...(63)

)/()( 32344314432322322 SaaSaaSaaBSaACa (64))/())()(( 32344313422233222333 SaaSaaSBSaSaACaCa (65)

)/()( 32344313422224 SaaSaaSBSaACa (66)

1234112345 / SPDC x (67)Where

13


4234432332344211 )( CaCaCaPPA xx 4234432332344211 )( SaSaSaPPB yy

111 yz PPD

Conclusions:-3Finding closed form solution to the inverse kinematics problem isdesirable since because it is required less computational time and providesaccurate multiple solutions. In this paper, a closed form solution to the inversekinematics of a 5-DOF robot is proposed. A software program was alsodeveloped 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 anapproximation with a least-square error. For a five axis robot, the Jacobian isnot a square matrix, and it has no inverse. In this paper, presents method tocompute the joint velocity and acceleration of fives axis robot. This methodbased on the spherical angle of the tool to derive 5x5 Jacobian matrix forwhich the exact solutions are obtainable.

-s:Reffrence-41. R.Mansear,Structural kinematics of six revolute axis robot manipulators.Mech.Mach.Theoty,Vol.31,No.05,2004.2. S.Surman Formulating swept profiles for five axis tool motion. Int.J.Mach Tools Manual2005.3.D.Piper and B.Roth The kinematics of manipulators under computer control. Proc.2ndInt.Cong.on the Theory of Mach,2002.4.R.Paul and P.Mayer Robot manipulators: Mathematics programming, Int.J. of RoboticsResearch,1987.5. Z.DiehClosed-form inverse kinematics of a two axis wrist industrial are welding robot.J.Engineering and development,AL-Mustansirya Univ.,Baghdad,1999.6. K. Duffy Analysis of five-degree-of-freedom robot arms. ASME J Mech Trans AutomatDes 2005.7. W.Tsai and T. Morgan Solving the kinematics of the most of general six and five degree-of-freedom manipulators by continuation methods. ASME J Mech Trans Automat Des2007.8. K.Hunt Robot kinematics a compact analytic inverse solution for velocities. ASME JMech Trans Automat Des 2004.9.M.SpongRobot dynamics and controls.Singapore:John Wiley,1989.10. J.Ming Inverse velocity analysis for line guidance five axis robot,Robotics andComputer-Integrated Manufacturing,2009.

Analytical Inverse Kinematics Algorithm of 5 Dof Robot Arm

Documents

Transcript of Analytical Inverse Kinematics Algorithm of 5 Dof Robot Arm