EPC431 Robotics and Automation

Kinematics of Robot Manipulator-Part 4

Class Schedule
Monday 12.00 - 13.00
Tuesday 10.00 - 11.00
Thursday 10.00 - 11.00

Venue :
DK10
MEK 1
DK5
Dr. Elmi Abu Bakar
Room: 0.68A
Instrumentation, Electronics and Avionics Laboratory
School of Aerospace Engineering
Tel.: 04-5995954/5926 H/p: 016-4939839
Email: meelmi@usm.my
http://elearning.usm.my

Outlines
Kinematics
Preliminary
Worldframe,jointframe,endeffectorframe
RotationMatrix,compositerotationmatrix
HomogeneousMatrix
Directkinematics
DenavitHartenberg Representation
Examples
Inversekinematics

Endeffector
End effector the last coordinate system of figure
Located in joint N.
But usually, we want to specify it in base coordinates.

11/3/2016

EPC431 Robotics and Automation

Endeffector
Atransformation fromthelinkNtothebase:

11/3/2016

EPC431 Robotics and Automation

Endeffector
Wecanalsoexpressitas
threerotations (aroundeachofthecoordinateaxes)
followedbyatranslation

Howcanwe
establisha
relationwith
theother
expression?
11/3/2016

EPC431 Robotics and Automation

Endeffector
Location
Theorigin ofacoordinateframe relativeto
somebasecoordinateframeisspecified
bythetranslation:
z
z

y
x

R
11/3/2016

x
EPC431 Robotics and Automation

Endeffector
Orientation
Any3Dorientation relativetosomebase
coordinateframe canbespecifiedby:
threerotations,onearoundeachofthe
coordinateaxes.
z
z

y
x

y
W
11/3/2016

EPC431 Robotics and Automation

Endeffector
Orientation
Theserotationsarenamed roll,pitch,yaw

11/3/2016

We do them in this order : around x, y, z.

EPC431 Robotics and Automation

Endeffector
Orientation
Theyaw transformationcanbeexpressed:

11/3/2016

EPC431 Robotics and Automation

Endeffector
Orientation
Thepitch transformationcanbeexpressed:

11/3/2016

EPC431 Robotics and Automation

10

Endeffector
Orientation
The roll transformationcanbeexpressed:

11/3/2016

EPC431 Robotics and Automation

11

Endeffector
Orientation
Theroll,pitch and yawtransformationis
thenexpressed:

11/3/2016

EPC431 Robotics and Automation

12

Endeffector
Finally,the transformation fromacoordinate
frame tothebaseframe isexpressed:
translation

11/3/2016

orientation

EPC431 Robotics and Automation

13

Endeffector
Rememberthatcanbeexpressedasa
4x4matrixwithelementsmij functionsofjoint
parameters1,...,N.
Letsomitthemforreadability:

11/3/2016

EPC431 Robotics and Automation

14

Endeffector
Asthetwotransformations
anddothesamething:

11/3/2016

EPC431 Robotics and Automation

15

Endeffector

Weobtaindirectlythetranslationvector:

11/3/2016

EPC431 Robotics and Automation

16

Endeffector

Wecanobtaintheyawangle:
Because :

arctan is -periodic.
Lets use our function arctan2 to get the right
angle.
11/3/2016

EPC431 Robotics and Automation

17

Endeffector

Knowingtheyawangle,wecanobtainthe
pitchangle :
Because :

Again, lets use our function arctan2 :

11/3/2016

EPC431 Robotics and Automation

18

Endeffector

Wecanobtaintherollangle:
Because :

Again, lets use our function arctan2 :

11/3/2016

EPC431 Robotics and Automation

19

Endeffector
Letsdefinethestatevector

Forward(Direct) kinematics
Given joint variables

q (q1 , q2 , q3 , q4 , q5 , q6 , qn )
End-effector position and orientation, Formula?

Y ( x, y, z , O, A, T )
11/3/2016

x
EPC431 Robotics and Automation

20

Endeffector
Aspreviouslyshown,

The state vector is composed of elements of this

matrix. Its also a function of joint parameters :

11/3/2016

EPC431 Robotics and Automation

21

InverseKinematics
Givenadesiredposition(P)&orientation(R)of
theendeffector

z
Findthejointvariableswhichcanbringthe
robotthedesiredconfiguration

q (q1 , q2 , qn )

y
Inverse Kinematics
From Position to Angles

22
11/3/2016

EPC431 Robotics and Automation

InverseKinematics
Moredifficult
Systematicclosedformsolutionin
generalisnotavailable
Solutionnotunique
Redundantrobot
Elbowup/elbowdown
configuration
Robotdependent

11/3/2016

EPC431 Robotics and Automation

(x , y)

23

InverseKinematics
Transformation Matrix
nx
n
T y
nz

sx
sy
sz
0

ax
ay
az
0

px
p y
T01T12T23T34T45T56
pz

1

2
3

4
5

6

Special cases make the closed-form arm solution possible:

1.

Three adjacent joint axes intersecting (PUMA, Stanford)

2.

Three adjacent joint axes parallel to one another (MINIMOVER)

24

ASimpleExample
Revolute and
Prismatic
Joints
Combined

Finding :

y
arctan( )
x

More Specifically:

(x , y)

y
arctan 2( )
x

arctan2() specifies that its in the

first quadrant

1
X

11/3/2016

Finding S:

S (x 2 y 2 )
EPC431 Robotics and Automation

25

InverseKinematicsofaTwoLinkManipulator
(x , y)

Find:

l2

l1

Given: l1, l2 , x , y
1, 2

Redundancy:
A unique solution to this problem
does not exist. Notice, that using the
givens two solutions are possible.
Sometimes no solution is possible.
(x , y)

11/3/2016

EPC431 Robotics and Automation

26

The Geometric Solution

2

l2

(x , y)

Using the Law of Cosines:

c 2 a 2 b 2 2ab cos C
2

( x 2 y 2 ) l1 l2 2l1l2 cos(180 2 )
cos(180 2 ) cos( 2 )
l1

x 2 y 2 l1 l2
cos( 2 )
2l1l2

Using the Law of Cosines:

sin B sin C

b
c
sin 1 sin(180 2 )
sin( 2 )

2
2
l2
x y
x 2 y2
1 1
y
arctan 2
x

11/3/2016

x 2 y 2 l12 l2 2

2 arccos

2
l
l
1
2

Redundant since 2 could be in the

first or fourth quadrant.

Redundancy caused since 2 has two possible

values

l sin( )
y
2
1 arcsin 2
arctan 2
x 2 y2
x

EPC431 Robotics and Automation

27

The Algebraic Solution

2

l2

(x , y)

Construct this equation first

c 1 cos 1
c 1 2 cos( 2 1 )
(1) x l1 c 1 l 2 c 1 2

l1

(2) y l1 s 1 l 2 sin

(3) 1 2

(1) 2 ( 2) 2 x 2 y 2

1 2

l1 c1 l2 (c1 2 ) 2 2l1l2 c1 (c1 2 ) l1 s1 l2 (sin 1 2 ) 2 2l1l2 s1 (sin 1 2 )

l1 l2 2l1l2 c1 (c1 2 ) s1 (sin 1 2 )
2

l1 l2 2l1l2 c 2

Only Unknown

x 2 y 2 l12 l2 2

2 arccos

2
l
l
1
2

11/3/2016

Note:
cos(ab) (cosa)(cosb) (sina)(sinb)

sin(ab) (cosa)(sinb) (cosb)(sina)

EPC431 Robotics and Automation

28

Note:

x l1 c 1 l 2 c 1 2
l1 c 1 l 2 c 1 c 2 l 2 s 1 s 2
c 1 ( l1 l 2 c 2 ) s 1 ( l 2 s 2 )
y l1 s 1 l 2 sin

1 2

l1 s 1 l 2 s 1 c 2 l 2 s 2 c 1
c 1 ( l 2 s 2 ) s 1 ( l1 l 2 c 2 )
c1

x s1 ( l 2 s 2 )
( l1 l 2 c 2 )

x s1 ( l 2 s 2 )
( l 2 s 2 ) s 1 ( l1 l 2 c 2 )
y
( l1 l 2 c 2 )

cos(ab) (cosa)(cosb) (sina)(sinb)

sin(ab) (cosa)(sinb) (cosb)(sina)

We know what 2 is from the previous

slide. We need to solve for 1 . Now
we have two equations and two
unknowns (sin 1 and cos 1 )

Substituting for c1 and simplifying

many times

1
x l 2 s 2 s 1 ( l1 2 l 2 2 2 l1 l 2 c 2 )
( l1 l 2 c 2 )

s1

y ( l1 l 2 c 2 ) x l 2 s 2

11/3/2016

x2 y2

Notice this is the law of cosines

and can be replaced by x2+ y2

y ( l1 l 2 c 2 ) x l 2 s 2
1 arcsin
x2 y2

EPC431 Robotics and Automation

29