Professional Documents
Culture Documents
Robot Công nghiệp (Trường Đại học Bách Khoa - Đại học Đà Nẵng)
c
978-1-7281-1895-6/19/$31.00 2019 IEEE 1393
The robot manipulator uses the first three joints to position ªCTi STiCD i STi SD i aiCTi º
the structure and the next three are used to orient the end- « ST CTiCD i CTi SD i ai STi »» (1)
effector. The kinematic model shows the relationship between i 1
Ti « i
the joint angles and the pose (position and orientation) of the « 0 SD i CD i di »
« »
end-effector. ¬ 0 0 0 1 ¼
Finding this matrix for each row of Table I, multiple
transformation matrices can be found as shown below.
ªC1 0 S1 0º ª S2 C2 0 a2 S2 º
«S 0 »» « C S
« 1 0 C1 0 a2C2 »»
0
T1 1
T2 « 2 2
« 0 1 0 d1 » « 0 0 1 0 »
« » « »
¬0 0 0 1¼ ¬ 0 0 0 1 ¼
ªC3 0 S3 a3C3 º ªC4 0 S4 0º
«S a3S3 »» «S 0 C4 0 »»
2
T3 « 3 0 C3 3
T4 « 4
« 0 1 0 0 » «0 1 0 d4 »
« » « »
¬0 0 0 1 ¼ ¬0 0 0 1¼
ªC5 0 S5 0º ª C 6 S6 0 0º
«S « S 0 0 »» (2)
4 « 5 0 C5 0»» 5 « 6 C6
T5 T6
«0 1 0 0» « 0 0 1 d6 »
« » « »
¬0 0 0 1¼ ¬ 0 0 0 1¼
These individual matrices are combined to determine the
Fig. 2. Frame assignment overall transformation matrix which describes the end frame
with respect to the base frame. By multiplying the matrices 0T1
TABLE I. DH PARAMETERS OF IRB 1200 ROBOT through 5T6 yields 0T6 containing the following twelve equations
which define the position and orientation of the end effector.
Joint/ θn+1 dn+1 an+1 αn+1 Working
Link (degree) (mm) (mm) (degr Range ªn x ox ax px º
ee) (degree) «n
1 θ1 d1 0 -90 +170 to -170 0 0 « y oy ay p y »»
T6 T1 .1 T2 .2 T3 .3 T4 .4 T5 .5 T6
2 θ2-900 0 a2 0 +135 to -100 «nz oz az pz »
3 θ3 0 a3 -90 +70 to -200 « »
¬0 0 0 1¼
4 θ4 d4 0 +90 +270 to -270
5 θ5 0 0 -90 +130 to -130 nx C1 S 23C 4 C5 C6 S1 S 4 C5 C6 C1C 23S 5 C6 C1 S 23S 4 S 6 S1C 4 S 6
6 θ6-1800 d6 0 0 +360 to -360 ox C1 S 23C 4 C5 S 6 S1 S 4 C5 S 6 C1C 23S 5 S 6 C1 S 23S 4 C6 S1C 4 C6
d1=399; a2=350; a3=42; d4=351; d6=82
ax C1 S 23C 4 S 5 S1 S 4 S 5 C1C 23C5
III. MODELLING OF FORWARD KINEMATICS px C1 S 23C 4 S 5 d 6 S1 S 4 S 5 d 6 C1C 23C5 d 6 C1C 23d 4 a3C1 S 23 C1 a 2 S 2
ny S1 S 23C 4 C5 C 6 C1 S 4 C5 C 6 S1C 23 S 5 C 6 S1 S 23 S 4 S 6 C1C 4 S 6
The forward kinematic model is used to find the pose of the
oy S1 S 23C 4 C 5 S 6 C1 S 4 C5 S 6 S1C 23 S 5 S 6 S1 S 23 S 4 C 6 C1C 4 C 6
manipulator, given the joint variables. It is a relatively simple
problem once the DH parameters are known. DH approach is the ay S1 S 23C 4 S 5 C1 S 4 S 5 S1C 23C5
standard way for robot representation and its motion modelling py S1 S 23C 4 S 5 d 6 C1 S 4 S 5 d 6 S1C 23C5 d 6 S1C 23d 4 a3 S1 S 23 S1 a 2 S 2
that can be used for any robot configuration, regardless of its nz C4C23C5C6 S23S5C6 C23S4 S6
complexity. First, assign a frame to each joint starting from
oz C4C23C5 S6 S23S5 S6 C23S4C6 (3)
reference frame {0} fixed to the base and transform this frame
to the next. Fig. 2 shows reference frames assigned for the robot az S5C4C23 S23C5
manipulator for the analysis of DH parameters. The DH pz S5C4C23d6 S23C5d6 S23d 4 a3C23 a2C2 d1
parameters determined and the joint range of IRB 1200 robot are where
presented in Table I. The represented frames and associated DH C1 Cos(T1 ); S1 Sin(T1 ); C23 Cos(T 2 T 3 ); S 23 Sin(T 2 T3 )
parameters are found using Craig's convention [2].
Having derived the direct kinematics of the IRB 1200 as in
Based on the transformation of reference frames to each joint equation (3), it's now possible to obtain the end-effector
and DH parameters, individual homogenous transformation position and orientation from the individual joint angles [θ1, θ2,
matrices relating the frames attached to successive links are θ3, θ4, θ5, θ6]. These equations specify how to compute the
found. By using the definition of the link transformation matrix,
position and orientation of frame {6} relative to frame {0} of
the matrix i 1Ti describes the homogenous transformation from
the robot. These are the key equations for all kinematic analysis
the coordinates system ‘i’ to ‘i-1’ which is given by,
of this robot manipulator. The working area of the robot To solve an equation of this type, trigonometric substitution
manipulator plotted in Y-Z plane is shown in Fig. 3. has to be done as shown below.
d4 GCosI ;
(10)
a3 GSinI
§a 2 ·
where G d 4 a3
tan 1 ¨¨ 3 and I ¸¸
© d4 ¹
Hence SI C3 S 3 CI Sin(I T 3 ) K (11)
G
2
K
Therefore, Cos(I T 3 ) r 1 (12)
2
G
From (11) and (12),
§ ·
tan 1 ¨ ¸
K (13)
I T3 ¨ ¸
2 2 2
© r a3 d 4 K ¹
Hence,
§ ·
I tan 1 ¨¨ ¸
Fig. 3. Workspace of ABB IRB 1200 K
T3
2 2 2 ¸
IV. MODELLING OF INVERSE KINEMATICS © r a3 d 4 K ¹ (14)
This section addresses the problem of arm solution for IRB §a · § ·
¸¸ tan 1 ¨ ¸
K
1200 robot manipulator. To adjust the position and orientation T3 tan 1 ¨¨ 3 ¨ ¸
2 2
of the end-effector of a robot so as to reach its target object, the © d4 ¹ © r a3 d 4 K
2
¹
inverse kinematic solution is most needed. For a given pose of From (14), two solutions for T3 will be obtained. Pre-
the end-effector, the joint angles that bring the end-effector in
0 1
multiplying T6 in n , o , a and position format with T3 , a
0
the specified pose is defined by Inverse kinematic model. In
other words, given 0T6 as sixteen numeric values, Inverse matrix with elements as a function of only known variables will
kinematics solution is the corresponding joint angles θ1, θ2, θ3, be obtained.
θ4, θ5 and θ6[14]. In this section, the inverse kinematic solution
of IRB 1200 robot is worked out purely algebraically. From the ªn x ox ax px º
forward kinematic equations in (3), « p y »» 3
0 1 «n y oy ay (15)
'
px d6 ax C1C23d 4 a3C1 S 23 C1a2 S 2 (4)
T3 . T6
px «n z oz az pz »
' « »
py py d6ay S1C 23d 4 a3 S1 S 23 S1a2 S 2 ¬0 0 0 1¼
(5)
0 0
' T3 T1 .1 T2 . 2 T3
py S1 (C23d 4 a31 S 23 a2 S 2 )
(6) a 3 C1 S 23 a 2 C1 S 2 º
'
C1 (C23d 4 a31 S 23 a2 S 2 ) ªC1 S 23 S1 C1C 23
px «S S
Hence, joint angle ߠଵ is given by, « 1 23 C1 S1C 23 a 3 S1 S 23 a 2 S1 S 2 »» (16)
§ p '·
(7)
« C 23 0 S 23 a 3 C 23 a 2 C 2 d 1 »
T1 tan 1 ¨ y ' ¸ « »
¨p ¸
© x ¹ ¬ 0 0 0 1 ¼
Also, ª R3
0 0
P3 º ª R3
0
R3 .0 P3 º .
0
T T
0 1
' If T3 « » , then 0T3 « »
pz pz d6 az S 23d 4 a3C23 a2 C2 d1 (8)
'
¬ 0 1 ¼ ¬ 0 1 ¼
' '
Squaring and adding p z , p y , p z d1 and rearranging 0 1
T3 is obtained as,
' ' ' 2 2 2
Hence
( px )2 ( p y ) 2 ( pz d1 )2 d 4 a3 a2
a3C3 d 4 S3 (9) ª C1 S 23 S1 S 23 C 23 a 3 a 2 C 3 d1C 23 º
2a2 « S »
1 « 1 C1 0 0 » (17)
a3C3 d 4 S3 K 0
T3
' 2 ' 2 ' 2
( p x ) ( p y ) ( p z d1 ) d 4 a3 a 2
2 2 2 «C1C 23 S1C 23 S 23 a 2 S 3 d1 S 23 »
where K « »
2a 2 ¬ 0 0 0 1 ¼
0 1
To find
3
T6 , T3 is multiplied with
3
T6 represented in n , o , a and position format to get the following matrix.
ªn x ox ax px º ª C1 S 23 S1 S 23 C 23 a3 a 2 C 3 d1C 23 º ª n x ox ax px º
« p y »» « S C1 0 0 » «n p y »»
1 «n y oy ay « 1 ».« y oy ay
3
T6 0
T3 (18)
«nz oz az pz » «C1C 23 S1C 23 S 23 a 2 S 3 d1 S 23 » «n z oz az pz »
« » « »« »
¬0 0 0 1¼ ¬ 0 0 0 1 ¼¬0 0 0 1¼
ªC1S23nx S1S23n y C23nz C1S23ox S1S23oy C23oz C1S23ax S1S23a y C23az C1S23 px S1S23 p y C23 pz a3 a2C3 d1C23 º
« S1nx C1ny S1ox C1oy S1ax C1a y S1 px C1 p y »
3
T6 « »
«C1C23nx S1C23ny S23nz C1C23ox S1C23oy S23oz C1C23ax S1C23a y S23az C1C23 px S1C23 p y S23 pz a2 S3 d1S23 »
« »
¬ 0 0 0 1 ¼
3 3
Also, T6 is calculated by multiplying T4 , 4T5 and 5T6 given in (2).
ª C 4 C 5 C 6 S 4 S 6 C 4 C 5 S 6 S 4 6 C4 S5 C4 S5 d 6 º
« S C C C S S 4 C5 S 6 C 4 C6 S4 S5 S 4 S 5 d 6 »» (19)
3
T6 3 T4 . 4 T5 .5 T6 « 4 5 6 4 6
« S 5C6 S5 S6 C5 d 6 C5 d 4 »
« »
¬ 0 0 0 1 ¼
By equating the (3,3) elements from (18) and (19),
C5 C1C23ax S1C23a y S 23az
(20)
Similarly, by equating the (3,4) elements from (18) and (19),
d 6C5 d 4 C1C23 px S1C23 p y S 23 pz a2 S3 d1S 23
(21)
Substituting (20) in (21),
d 6 (C1C23ax S1C23a y S 23az ) d 4 C1C23 px S1C23 p y S 23 pz a2 S3 d1S 23
(22)
Rearranging,
C23 (C1 p x S1 p y d 6C1a x d 6 S1a y ) S 23 ( p z d1 d 6 a z ) d 4 a2 S 3 ;
(23)
Equation (23) can be written as, § ·
T 23 D tan 1 ¨¨ ¸
C 23 M 1 S 23 M 2 M (24) M
2 2 2 ¸
where © r M1 M 2 M ¹ (28)
M 1 C1 p x S1 p y d 6 C1 a x d 6 S1 a y § ·
§M ·
¸¸ tan 1 ¨ ¸
M
M2 p z d1 d 6 a z T 23 tan 1 ¨¨ 1 ¨ ¸
2 2
M a 2 S 3 d 4 © M2 ¹ © r M1 M 2 M
2
¹
M1 C1 p x S1 p y d 6 C1 a x d 6 S1 a y
Making trigonometric substitution to M 1 and M 2 ,
M 1 USinD ; where, M 2 p z d1 d 6 a z
(25)
M 2 UCosD M a 2 S 3 d 4
2 2 §M · Equation (28) computes the four values of T 23 according to
where U M 1 M 2 and D tan 1 ¨¨ 1 ¸¸
©M2 ¹ the four possible combinations for T 1 and T 3 . Then four
Substituting (25) in (24), possible solutions for T2 can be computed as,
M
C 23SD S 23CD Sin(D T 23 ) T2 T 23 T 3
U (29)
2 where the appropriate solution for T 3 is used when forming the
and Cos(D T 23 ) M (26)
r 1 difference. Equating (1,3) and (2,3) elements from (18) and (19),
U2
C4 S5 S1ax C1a y
§ · (30)
tan 1 ¨¨ ¸ (27)
M
D T 23 ¸ S 4 S5 C1S 23ax S1S 23a y C23az
2 2 2 (31)
© r M1 M 2 M ¹ Dividing (30) from (31), we get,
S4 C1 S 23a x S1 S 23a y C 23a z In Fig. 4, 10 standard positions of the IRB 1200 robot
manipulator are shown and its corresponding joint angle and
C4 S1a x C1a y
(32) wrist centre data are given in Table II.
T z 0 , we can solve for T 4
As long as 5 as,
§ C1 S 23a x S1 S 23a y C 23a z ·
T4 tan 1 ¨ ¸ (33)
¨ S1 a x C1 a y ¸
© ¹
When T 5 0 , the manipulator is in singular position. This
condition is obtained by finding the Jacobian Matrix of robot
and equate its determinant to zero. Considering (3,3) element in
(19) and equating to the corresponding element in (18),
Cos(T 5 ) C1C23a x S1C23a y S 23a z (34)
Hence, T 5 can be solved as
T5 Cos 1 (C1C23a x S1C23a y S 23a z )
(35)
Similarly equating (3,1) and (3,2) elements of the matrix in
(19) with corresponding elements in (18), we get,
S5C6 C1C23nx S1C23n y S 23nz Fig. 4 Positions at wrist center of IRB 1200 Robot.
(36)
S5 S 6 C1C23ox S1C23o y S 23oz TABLE II. WRIST CENTRE POINT AND ANGLE OF AXES 2 AND 3 FOR
(37) VARIOUS POSITIONS
As long as T 5 z 0 , T 6 can be solved as Position Wrist at Centre(mm) Joint angles (degrees)
§ (C1C 23o x S1C 23o y S 23o z ) · X Z T2 T3
T6 tan 1 ¨ ¸ (38)
¨ CC n S C n S n ¸ Pos0 351 791 0 0
© 1 23 x 1 23 y 23 z ¹ Pos1 0 1102 0 -83
Hence, four inverse kinematic solutions are obtained and Pos2 160 434 0 +70
four more solutions can be obtained by flipping the Pos3 703 398 +90 -83
manipulator's wrist. The flipped solution is given by, Pos4 497 -99 +135 -83
Pos5 -133 55 -100 -200
T 4 ' T 4 180 0 , Pos6 -62 550 -100 +70
Pos7 -703 400 -90 -83
T5' T 5 , (39)
Pos8 -693 278 -100 -83
Pos9 358 488 +135 -200
T 6 ' T 6 180 0
A. Analytical Solution
Out of these eight solutions, the solution which violates the
joint angle limitations can be neglected and from the remaining, The homogeneous transformation matrix for each pose is
the one which is adjacent to present manipulator configuration calculated from the forward kinematic equations. The
can be chosen as the final solution. homogenous transformation matrix can be converted into
position data as well as the orientation data. The position data
V. VALIDATION indicates the position of the robot tool flange with respect to the
The following steps are performed to validate and world coordinate points and are given in mm. The orientation
demonstrate the kinematic model developed. data extracted from the rotation matrix and expressed in terms
of Euler angles. The position in Cartesian co-ordinates and
1. Pose selection: Select ten standard poses of the robot for
orientation in terms of Euler angles are given in Table III (data
validation. for only 4 poses are included).
2. Analytical solution: The mathematical equations to find the
kinematic solution of the robot is programmed in The inverse kinematic solution corresponding to each pose
MATLAB R2018a and the results for the selected poses is also given in Table III. From the eight joint angle solutions
are stored. obtained, some are not considered due to joint angle limitations.
3. Software validation: The selected poses are simulated in The exact solution corresponding to the transformation matrix is
RobotStudio software and compared the pose and joint marked in red colour. It is found that one solution will be exactly
angle results with the analytical solution. the same as the one which corresponds to the transformation
4. Experimental validation: The robot manipulator is jogged matrix.
to the selected poses and compared the pose and joint angle
results with the analytical solution.
B. Validation using RobotStudio coordinates of the position and orientation at Pos0. For that,
RobotStudio is a PC application for modelling, offline create target and use ‘Teach target' command which will store
programming and simulation of robot cells. RobotStudio allows the current location of the robot in terms of position (X, Y, Z) in
working with an off-line controller, which is a virtual IRC5 mm and orientation in degrees [15]. From this, the position and
controller running locally on the PC. This offline controller is orientation corresponding to each pose can be obtained (See Fig.
often called as the virtual controller that is identical to the robot's 5). The same procedure can be repeated for rest of the poses and
real controller. RobotStudio also allows working with the real compare the results stored in ABB RobotStudio with the
physical IRC5 controller, which is the real controller. ABB mathematically calculated co-ordinates given in Table III.
RobotStudio programs can be directly loaded on the robot's real
controller through LAN connection [13].
The virtual model of the IRB 1200 7Kg robot is loaded into
the application and jog the robot in different modes to check the
robot movement. Create a joint target with joint angles Fig. 5. Pos0 in RobotStudio
corresponding to Pos0 and the robot at the joint target can be
visualised in the simulation window. Next step is to measure the
C. Experimental Validation runs the robots in the real world, a realistic simulation can be
The experimental setup includes IRB 1200 robot with IRC5 performed. Hence RobotStudio results help in the faultless
controller which is shown in Fig. 6. The robot is jogged to the validation of kinematic analysis of all ABB robots. The
selected poses using the Flex Pendant and the pose data kinematic analysis method used in this paper can be extended to
corresponding to each set of joint angles is noted. The results any robots of similar structure. The industrial robot manipulators
obtained are compared with the analytical solutions given in are required to have proper knowledge of its joint variables as
Table III and found the same. Hence the kinematic model is well as the understanding of kinematic parameters in many
demonstrated and validated experimentally. fields of applications such as material handling, pick-n-place,
planetary and undersea explorations, space manipulation, and
hazardous field etc. Moreover, the kinematic analysis is also
important in the medical field robotics for rehabilitation and
surgery.
REFERENCES
[1] R. S. Hartenberg and J. Denavit, “A kinematic notation for lower pair
mechanisms based on matrices,” Journal of Applied Mechanics, vol. 77,
pp. 215–221, June 1955.
[2] John J Craig, Introduction to Robotics, Mechanics and Control, 2nd
Edition, (Addison Wesley, Boston, MA, USA) 1989, 68-144.
[3] Sebastian Głowiński, Tomasz Krzyżyński, “An inverse kinematic
algorithm for the human leg, “ Journal of theoretical and Applied
Mechanics, vol.54 No.1, pp. 53-61, 2016.
[4] Y.Davidor, “A genetic algorithm applied to robot trajectory generation,
in L. Davis,” Handbook of Genetic Algorithms, Van Nostrand Reinhold,
New York,pp144-165, 1991.
[5] Vijyant Agarwal, “Trajectory planning of redundant manipulator using
Fig.6 Experimental Setup fuzzy clustering method, “ Int. J. Adv. Manuf. Technol., vol. 61, pp.727–
744, 2012.
The orientation values obtained from the demonstration [6] Y.Feng, W.Yao-nan,Y Yi-min, “Inverse kinematics solution for Robot
manipulator based on Neural Network under Joint Subspace,”
software and from the hardware is exactly the same as that of the International Journal of Computer Communication,vol.7.No.3.pp.459-
analytical solution. The error in position coordinates while 472, 1991.
comparing with different validation methods used above are [7] Srinivasan Alavandar, M. J. Nigam, “Neuro-Fuzzy based Approach for
summarised in Table IV. Since all the poses considered here are Inverse Kinematics Solution of Industrial Robot Manipulators,” Int. J. of
in x-z coordinates, the y coordinate is always zero and not computers, Communication and Control, Vol.3.No.3, pp.224-234, 2008.
mentioned in the table. [8] Adrian-Vasile Duka, “ANFIS based Solution to the inverse Kinematics
of a 3DOF Planar Manipulator,” Procedia Technology, vol.19, pp.526-
533, 2015.
TABLE IV. POSITION ERRORS
[9] D. Constantin, M. Lupoae, C. Baciu, D.Buliga,”Forward kinematic
Poses Analytical solution Position Errors analysis of an industrial robot,” Proc International Conference on
RobotStudio Hardware Mechanical Engineering, pp. 90-95, 2015.
Pos0 x 433 0 0.2 [10] S. Kucuk, Z. Bingul, “The Inverse kinematics solutions of industrial robot
z 791 0.1 -0.4 manipulators,” Proc IEEE Conference on Mechatronics, pp.274-279,
Pos1 x 11.1 -0.02 0.1 2004.
z 1183.9 0.09 0.1 [11] D. Manocha, J. F. Canny, “Efficient inverse kinematics for general 6R
Pos2 x 187.6 -0.04 -0.1 manipulators,” IEEE Transactions on Robotics and Automation,
z 356.5 0.08 0 vol.10,pp.648-657,1994.
Pos3 x 784.9 -0.01 0.1
[12] Ramish Syed Baqar Hussain, Farah Kanwal, “Design of 3-DoF Robotic
z 387.9 0.12 0 Arm,” Proc IEEE Int conf on Innovative computing technology, 2016.
[13] https://library.e.abb.com/public/683ae78194674b809d0362308ebcd5f3/
VI. CONCLUSION 3HAC046982%20PS%20IRB%201200-en.pdf. Accessed on April, 2019.
The forward and inverse kinematic modelling for ABB IRB [14] S. B. Niku, Introduction to Robotics: Analysis, Systems, Applications
1200 Robot Manipulator was performed analytically. The (Prentice Hall) 2001,29-88.
analytical solution is compared with the results obtained from [15] https://library.e.abb.com/public/244a8a5c10ef8875c1257b4b0052193c/3
the RobotStudio software. Since RobotStudio is built on the HAC032104-001_revD_en.pdf. Accessed on April,2019.
ABB Virtual Controller, an exact copy of the real software that