FORWARD AND INVERSE KINEMATIC SOLUTION FOR FUNUC M-10iA ROBOT

BY

SOLOMON EZEIRUAKU

AND

UCHE MARK EZEOBI

A PROJECT ON

Robotic Engineering II (ME 582)

1.0 INTRODUCTION

M-10iA Fanuc robot is a six degree of freedom, high performance industrial robot. This is a small but mighty robot only weighs 130kg but provides 10kg pay-load with highest wrist moments and inertia in its class. It can be mounted on the floor, wall, or ceiling at any angle. The third axis can be flipped over allowing the robot to work behind itself thereby creating a large working space. It is an ideal solution for machine tending, material handling, picking/packing, assembly, material removal, testing and sampling, and dispensing.

1.1 TECHNICAL SPECIFICATION OF THE ROBOT

Degree of Freedom = 6 Type of joint = All rotary Payload = 10kg at the wrist Repeatability = ±0.08 mm

1.2 FRAME ASSIGNMENT

We systematically established a coordinate system to each link using Denavit-Hartenberg method. The Zi-axis of the entire joint is fixed in the direction of rotation as followed by the right-hand rule for rotation. Here the rotation about the Z-axis will be the joint variable. Then Xi axis is fixed considering Zi-1 and Zi axes. And finally Yi axis is fixed to complete the right handed orthonormal coordinate frame. Thus frame assignment of 6-DOF robot was completed as shown in Figure 1. The link parameters is shown in table 1 and the transformations between adjacent coordinate frames is represented using 4x4 homogenous coordinate transformation matrix.

i αi-1 ai-1 di Ѳi

1 0 0 0 Ѳ1

2 -90 a1 0 Ѳ2

Figure 1. Frame assignment of M-10iA Fanuc Manipulator

3 0 a2 0 Ѳ3

4 -90 a3 d4 Ѳ4

5 90 0 0 Ѳ5

6 -90 0 0 Ѳ6

Transformation matrix for link 1 is

0T1 =[c 1 −s1 0 0

s1 c1 0 0

0 0 1 00 0 0 1

](1.1)

Transformation matrix for link 2 is

1T2 =[ c 2 −s2 0 a1

0 0 1 0−s2 −c2 0 0

0 0 0 1]

(1.2)

Transformation matrix for link 3 is

2T3 =[c3 −s3 0 a2

s3 c3 0 0

0 0 1 00 0 0 1

](1.3)

Transformation matrix for link 4 is

Table 1: Link parameters of the M-10iA Fanuc Robot

3T4 =[ c4 −s4 0 a3

0 0 1 d4

−s4 −c4 0 0

0 0 0 1]

(1.4)

Transformation matrix for link 5 is

4T5 =[ c5 −s5 0 0

0 0 −1 0−s5 −c5 0 0

0 0 0 1]

(1.5)

Transformation matrix for link 6 is

5T6 =[ c6 −s6 0 0

0 0 1 0−s6 −c6 0 0

0 0 0 1]

(1.6)

2.0 FORWARD KINEMATCS

We solved for Forward kinematic solution of the robot to determine the position and orientation of the end-effector given the values for joint variables of the robot. That is computation of position and orientation of the end effector relative to the base. In this case, our joint variables are the angles between the links. The homogenous matrix 0T6 which specifies the location of the end effector with respect to the base is gotten by matrix multiplications below.

To form our 0T6 matrix, we multiple out all the individual matrices of each link:

4T6 = 4T5 . 5T6 =[c5c6 −c5c6 −s5 0s6 c6 0 0

s5 c6 −s5s6 c5 0

0 0 0 1]

(2.1)

Then, we have:

3T6 = 3T4 . 4T6 =[ c4 c5 c6−s4 s6 −c6s4−c4c5 s6 −c 4s5 a3

s5c6 −s5 s6 c5 d4

−c4 s6−c5c6 s4 c5 s4 s6−c4 c6 s4 s5 0

0 0 0 1]

(2.2)

1T3 = 1T2 . 2T3 =[ c2c3−s2 s3 −c2 s3−c3s2 0 a2c2+a1

0 0 1 0−c4 s3−c3s2 s2s3−c2c3 0 −a2 s2

0 0 0 1]

(2.3)

Using sum of angle formulas:

C23 = c2c3 – s2s3

S23 = c2s3 + s2c3

Equation (2.3) becomes;

1T3 =[ c23 −s23 0 a2c2+a1

0 0 1 0−s23 −c23 0 −a2s2

0 0 0 1]

(2.4)

Then we have

1T6 = 1T3 . 3T6 =[

1 r111 r12

1 r131 px

1 r 211 r22

1 r 231 py

1 r311 r32

1 r 331 px

0 0 0 1]

(2.5)

1r11 = −c23 [s4 s6−c4 c5 c6]−s23 c6 s5

1r21 = −c4 s6−c5c6 s4

1r31 = s23 [s4 s6−c4 c5c6 ]−c23 c6 s5

1r12 = s23 s5 s6−c23 [c6 s4+c4c5 s6 ]

1r22 = c5 s4 s6−c 4c6

1r13 = −s23 c5−c23c4 s5

1r23 = s4s5

1r33 = s23 c4 s5−c23c5

1px = a1+a3 c23−d4 s23+a2c2

1py = 0

1pz = −d 4c23−a3s23−a2s2 (2.6)

The product of all the six link transforms is:

0T6 = 0T1 . 1T6 =[r11 r12 r13 pxr21 r22 r23 p yr31 r32 r33 px0 0 0 1

](2.7)

r11 = s1 [ s4 s6−c4c5c6 ]−c1 [c23 (s4 s6−c4c5c6 )+s23 c6 s5 ]

r21 = −c1 [c4 s6+c5c6 s4 ]−s1 [c23 (s4 s6−c4 c5c6 )+s23 c6s5 ]

r31 = s23 [s4 s6−c4 c5c6 ]−c23 c6 s5

r12 = s1 [c 4c6−c5 s5 s6]−c1 [c23 (c6 s4+c4c5 s6 )−s23 s5 s6 ]

r22 = −s1 [c23 (c6 s4+c4 c5s6)−s23 s5 s6 ]−c1 [c4 c6−c5 s4 s6]

r32 = s23 [c6 s4+c4c5 s6 ]+c23 s5s6

r13 = c1 [ s23 c5+c23 c4 s5]−s1s4 s5

r23 = c1 s4 s1−s1 [s23 c5+c23 c4 s5 ]

r33 = s23 c4 s5−c23c5

px = c1 [a1+a3c23−d4 s23+a2 c2 ]

py = s1 [a1+a3c23−d 4s23+a2c2]

pz = −d 4c23−a3s23−a2s2 (2.8)

3.0 INVERSE KINEMATICS

Inverse kinematic solution is concerned with determining values for the joint variables that achieve a desired position and orientation for the end-effector of the robot. That is giving the position and orientation of the end effector relative to the base frame, we are required to compute all the possible sets of joint angles and link geometries which could be used to attain the given position and orientation of the end effector.

[0 T 1 (θ1 )]−160 T=2

1 T 32 T 4

3 T 54 T 6

5 T (3.1)

[ c 1 s1 0 0

−s1 c1 0 0

0 0 1 00 0 0 1

] .

[r11 r12 r13 pxr21 r22 r23 p yr31 r32 r33 px0 0 0 1

] = 6

1 T (3.2)

From (2, 4) element;

−s 1 p x+c 1 p y=0p x=pcos φp y=p sinφ

p=√ px2+ py2φ=A tan 2( p y , p x )

−s 1 pcosθ+c 1 p sinθ=0−s 1 pc φ+c1 sφ=0

sin (φ−θ 1 )=0

cos (φ−θ1 )=±√1−0=±1

φ−θ 1=A tan2 (0,1)

θ1=A tan2 ( py , px )−A tan2 (0 ,±1 ) (3.3)

[ 1T2 ]−1 [ 0 T

1 ]−1 0 T 6 =2 T6 (3.4)

[ c 2 0 −s2 −a 1 c2

−s2 0 −c2 a 1 s2

0 1 0 00 0 0 1

] .

[ c 1 s1 0 0

−s1 c1 0 0

0 0 1 00 0 0 1

] .

[r11 r12 r13 pxr21 r22 r23 p yr31 r32 r33 px0 0 0 1

] = 2T6 (3.5)

We obtained 2T6 below:

2T6 =[ c6 [ s3 s5−c 3 c4 ]−c3 s4 s6 s6 [c3 s4s6−c3 c4c5 ]−c3c6 s4 −c5s3−c3c4 s5 a2+a3c3−d4 s3

c6 [c3 s5−c 4 c5 s3 ]−s3s4 s6 −s6 [c3 s5+c 4 c5s3 ]−c6 s3 s4 c3c5−c4 s3s5 d 4c3+a3 s3−c4 s6−c5 c6 s4 c5s4 s6−c4 c6 s4 s5 0

0 0 0 1]

[ c 1 c2 c2 s1 −s2 −a1c2

−c1s2 −s1s2 −c2 a1s2

−s1 c1 0 0

0 0 0 1] .

[r11 r12 r13 pxr21 r22 r23 p yr31 r32 r33 px0 0 0 1

] = 2T6 (3.6)

From (3, 4 element) of equation (3.6), we obtain

−s1 px+c1 p y=0 (3.7)

From (1, 4 element)

c1c2 px+c2 s1 p y−s2 pz−a1c2=a2+a 3 c3−d 4 s3 (3.8)

From (2, 4 element)

−c1 s2 px−s1s2 py−c2 pz+a1 s 2=d 4c3+a3 s3 (3.9)

If we square equations (3.7) and (3.8) and add the resulting equation, we obtain

(c1 p x )2+(s 1 py )2+ p z2 +a 12 +2c1s1 px p y−2c1 a1 px−2 s1a1 p y=a2

2+a32+d4

2−2a2a3 c3+2a2d 4 s3(3.10)

If we square equation (3.7) and add the resulting equation to equation (3.10), we obtain

px2+ py

2 pz2+a1

2−2a1 px c1−2a1 py s1−a22+a3

2+d 42=2a2a3c3−2a2d4 s3

Let K =

px2+ p y

2+ pz2+a1

2−2a1 px c1−2a1 py s1−a22−a3

2−d42

2a2 (3.11)

Then,

K=a3c3−d 4 s3 (3.12)

θ3=A tan 2 (a3 , d 4)−A tan 2 (K ,±√a32+d4

2−K 2) (3.13)

[0T3]-1[0T6] = [3T6] (3.14)

[0T3]-1=[ c1 c23 c23 s1 −s23 −a1c23−a2 c3

−s23 c1 −s23 c1 −c23 a1s23+a2 s3−s1 c1 0 0

0 0 0 1]

[ c1 c23 c23 s1 −s23 −a1c23−a2 c3

−s23 c1 −s23 c1 −c23 a1s23+a2 s3−s1 c1 0 0

0 0 0 1][r11 r12 r13 p xr21 r22 r23 pyr31 r32 r33 pz0 0 0 1

]=3 T 6

(3.15)

Where 3T6 is given by equation (2.2). Equating both the (1, 4) element and the (2, 4) elements from equation 5, we obtain the following,

c1c23Px+s1c23P y−s23 P z−a1c23−a2c3=a3 (3.16)

−c1 s23 Px−s1 s23 Py−c23P z+a1 s23+a2s3=d4 (3.17)

Solving equation (3.16) and (3.17) simultaneously for s23 and c23 results to the following:

s23=

−P z (a3+a2c3)+(d4−a2s3 )(c1Px+s1Py−a1)

Pz2+(c1Px+s1P y−a1 )

2

(3.18)

c23=

P z (d4−a2 s3 )+( a3+a2c3 )(c1Px+s1Py−a1)

Pz2+(c1Px+s1P y−a1 )

2

(3.19)

From equation (3.17) and (3.18) above,

θ23=A tan 2[−P z (a3+a2c3 )+( d4−a2s3 )(c1Px+s1P y−a1 ) ,P z (d4−a2s3 )+(a3+a2c3 )(c1Px+s1P y−a1 ) ] (3.20)

These gives us four values forθ23 , according to four possible values for θ1 and θ3 . Then using

the following equation for possible solutions, θ2 will be computed.

θ2 =θ23 - θ3 (3.21)

From [¿ 0T 3 ]−1. 0 T 6=3 T 6 (3.22)

Equating both the (1, 3) elements and the (3, 3) elements from the two sides of equation (3.22), we get:

r13 c1c23+r23 c23 s1−r33 s23=−c4 s5

−r13 s1+r23 c1=−s4 s5

θ4=A tan 2 (−r13 s1+r 23c1 ,−r13c1c23−r 23 c23 s1+r33 s23 ) ;→ s5≠0 (3.24)

To find Ѳ5 ;

[¿ 40T (θ4) ]−1 [0 T 6 ]=54T (θ5)6

5T (θ6 ) (3.25)

[¿ 40T (θ4) ]−1=[ s1 s4+c1c 4c23 −c1s4+c23 c4 s1 −s23 c4 −a1 c23c4−a3c4−a2c 4c 3

c4 s1−c1s4 c 23 −c1c4−s1 s4c23 s23 s4 a1c23 s4+a3 s4+a2c3 s4

−s23c1 −s 23 s1 −c23 −d4+a1s23+a2 s30 0 0 1

](3.26)

4T6 = [c5c6 −c5c6 −s5 0s6 c6 0 0

s5 c6 −s5s6 c5 0

0 0 0 1]

(3.27)

Equating both (1, 3) elements and the (3, 3) elements from the two sides of equation (3.27), we get

r13 (c1c4 c23+s1s4 )+r 23 (−c 1 s4+c 23 c4 s1 )−r 33 (s23c4 )=−s 5 (3.29)

r13 (−c1 s23 )+r 23 (−s 1 s23)+r33 (−c23 )=c 5 (3.30)

From equation (3.25) and (3.26);

θ5=A tan2 (s5 , c5) (3.31)

[¿ 50T ]−1

60

T=65T (θ6 ) (3.32)

Equating (3, 1) elements and (1, 1) elements of equation (3.32), we have

s6=−r 13 (c1 c23 s4−s1c4 )−r 21 ( s 1 c23 s4+c 1 c4 )+r31 (s23 s4 )c6=r11 [ c5 s1s4−s23 c 1 s5+c23c1c4 c5 ]+r 21 [c23 c4 c5 s1−c1c5 s4−s23 s 1 s5 ]−r31 [s23 c4c5+c23 s5 ]θ6=A tan 2 (s6 , c6 ) (3.33)

FORWARD AND INVERSE KINEMATIC SOLUTIONS WITH MAT LAB CODE

The forward kinetics of our robot was obtained using theta = [10 20 30 40 50 60]. The output of our robot is shown below:

T_0_6 =[ -0 .3344 0 .0315 -0 .9419 13 . 6517-0 .9424 -0 .0200 0 .3339 2.4072-0 .0084 0 .9993 0 . 0364 -30 . 3073

0 0 0 1]

The next step was to verify that our inverse kinematics would produce the same angle that was used to obtain the forward kinematics. Putting T_0_6 into our inverse kinematics matlab code yielded the following:

Theta =[10.0000 20.0000 30 . 0000 40 .00000 50 .0000 60 .0000−170 .0000 169. 9055 −160 .1453 −149 .9414 100 . 5592 94 . 3944 ]

We noticed that due to an offset “a1” in our robot kinematics, we could only obtain 2 sets of theta. This sets of theta was obtained using for loop to remove the unwanted results thus, our results are consistent.