Analytical Inverse Kinematics Algorithm of 5 Dof Robot Arm
description
Transcript of 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
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
-
2Journal of education of college no.4 vol.1 march./2011
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
-
3Journal of education of college no.4 vol.1 march./2011
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
-
4Journal of education of college no.4 vol.1 march./2011
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)
-
5Journal of education of college no.4 vol.1 march./2011
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
-
6Journal of education of college no.4 vol.1 march./2011
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)
-
7Journal of education of college no.4 vol.1 march./2011
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)
-
8Journal of education of college no.4 vol.1 march./2011
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
-
9Journal of education of college no.4 vol.1 march./2011
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
Journal of education of college no.4 vol.1 march./2011
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
Journal of education of college no.4 vol.1 march./2011
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
Journal of education of college no.4 vol.1 march./2011
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.