kinematika manipulator robot -...

1

Tugino, ST [email protected]

Kinematika Manipulator Robot

1

Jurusan Teknik Elektro STTNASYogyakarta

Pendahuluan Robot Manipulators

Konfigurasi Robot S ifik i R b tSpesifikasi Robot

Jumlah Axis, DOF(Degree Of Freedom)Precision, Repeatability

KinematicsPreliminary

World frame, joint frame, end-effector frameRotation Matrix, composite rotation matrixHomogeneous Matrix

Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 2

Homogeneous MatrixDirect kinematics

Denavit-Hartenberg RepresentationExamples

Inverse kinematics

2

Manipulators

Lengan Robot , Robot industriRigid bodies (links) connectedRigid bodies (links) connected by jointsJoints: revolute or prismaticDrive: electric or hydraulic End-effector (tool) mounted on a flange or plate secured to the wrist joint of robot


j

Manipulators R(revolute) or P( prismatic)

Robot Configuration:

Cartesian: PPP Cylindrical: RPP Spherical: RRP


SCARA: RRP(Selective Compliance Assembly Robot Arm)

Articulated: RRR

Hand coordinate:n: normal vector; s: sliding vector;

a: approach vector, normal to the

tool mounting plate

3

Robots Cartesian

Cartesian robotArm moves in 3 linearArm moves in 3 linear axes. X,y,z axes

z


xy


4

Manipulators

Motion Control MethodsP i t t i t t lPoint to point control

a sequence of discrete pointsspot welding, pick-and-place, loading & unloading

Continuous path controlfollow a prescribed path, controlled-path motionSpray painting Arc welding Gluing


Spray painting, Arc welding, Gluing

Manipulators

Robot SpecificationsNumber of AxesNumber of Axes

Major axes, (1-3) => Position the wristMinor axes, (4-6) => Orient the toolRedundant, (7-n) => reaching around obstacles, avoiding


undesirable configurationDegree of Freedom (DOF)WorkspacePayload (load capacity)Precision v.s. Repeatability

Which one is more important?

5

Kinematik

0ykinematics Forward

x

0y

1x1y

kinematics Inverse

sincos

0

0

lylx

==

θθ

θ

l


0x)/(cos 0

1 lx−=θ


6


A Simple Example

Revolute and Prismatic Joints Finding Υ:

y

Y

S

Combined

(x , y)

)xyarctan(θ =

More Specifically:

)xy(2arctanθ = arctan2() specifies that it’s in the

first quadrant


Υ1

X

SFinding S:

)y(xS 22+=

7

Υ2

(x , y)

Inverse Kinematics of a Two Link Manipulator

Given: l1, l2 , x , y

Find: Υ1, Υ2

Υ1

l2

l1

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)

Preliminary

Robot Reference FramesW ld fWorld frameJoint frameTool frame

x

yz

z

y T


x

y

W R

PT

8

PreliminaryCoordinate Transformation

Reference coordinate frame OXYZ zOXYZBody-attached frame O’uvw

zyx kji zyxxyz pppP ++=r

y

zP

vw

Point represented in OXYZ:T

zyxxyz pppP ],,[=


wvu kji wvuuvw pppP ++=r

zyx j zyxxyz pppx

uO, O’

zwyvxu pppppp ===

Point represented in O’uvw:

Two frames coincide ==>

PreliminaryProperties: Dot Product Let and be arbitrary vectors in and be 3R θx y

Mutually perpendicular Unit vectors

Properties of orthonormal coordinate frame

0=⋅ jivv

1||v

the angle from to , thenθcosyxyx =⋅

x y


00

0

=⋅

=⋅

jkki

ji

vv

vv

1||1||1||

=

=

=

kji

v

v

9

Preliminary

Coordinate TransformationRotation only zRotation only

wvu kji wvuuvw pppP ++=r y

zP

zyx kji zyxxyz pppP ++=r

u

vw


xuvwxyz RPP = u

How to relate the coordinate in these two frames?

PreliminaryBasic Rotation

, , and represent the projections of xp Pyp zponto OX, OY, OZ axes, respectively

Since

x y z

wvux pppPp wxvxuxx kijiiii ⋅+⋅+⋅=⋅=

wvu kji wvu pppP ++=


wvuy pppPp wyvyuyy kjjjijj ⋅+⋅+⋅=⋅=

wvuz pppPp wzvzuzz kkjkikk ⋅+⋅+⋅=⋅=

10

wvux pppPp wxvxuxx kijiiii ⋅+⋅+⋅=⋅=

wvuy pppPp wyvyuyy kjjjijj ⋅+⋅+⋅=⋅=

wvuz pppPp wzvzuzz kkjkikk ⋅+⋅+⋅=⋅=

⎥⎤

⎢⎡⎥⎤

⎢⎡ ⋅⋅⋅

⎥⎤

⎢⎡ ux pp wxvxux kijiii


⎥⎥⎥

⎦⎢⎢⎢

⎣⎥⎥⎥

⎦⎢⎢⎢

⎣ ⋅⋅⋅⋅⋅⋅=

⎥⎥⎥

⎦⎢⎢⎢

⎣ w

v

z

y

pp

pp

wzvzuz

wyvyuy

kkjkikkjjjij

PreliminaryBasic Rotation Matrix

⎥⎤

⎢⎡⎥⎤

⎢⎡ ⋅⋅⋅

⎥⎤

⎢⎡ ux pp wxvxux kijiii

Rotation about x-axis with⎥⎥⎥

⎦⎢⎢⎢

⎣⎥⎥⎥

⎦⎢⎢⎢

⎣ ⋅⋅⋅⋅⋅⋅=

⎥⎥⎥

⎦⎢⎢⎢

⎣ w

v

z

y

pp

pp

wzvzuz

wyvyuy

kkjkikkjjjij

zw

θ

Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 20x

y

vP

u

θ⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−=θθθθθ

CSSCxRot

00

001),(

11

Preliminary

Is it True? Rotation about x axis with θ

θθθθ

cossin0sincos0001

w

v

u

z

y

x

ppp

ppp

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

z

v

wP

θ


θθ

θθ

cossin

sincos

wvz

wvy

ux

ppp

ppppp

+=

−==

x

yu

θ

Basic Rotation MatricesRotation about x-axis with

⎥⎥⎤

⎢⎢⎡

−= θθθ SCxRot 0001

),(

θ

Rotation about y-axis with

R t ti b t i ith

⎥⎥⎦⎢

⎢⎣ θθ CS0

0

0100

),(⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−=

θθ

θθθ

CS

SCyRot

θ


Rotation about z-axis with

uvwxyz RPP =⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

10000

),( θθθθ

θ CSSC

zRot

θ

12

PreliminaryBasic Rotation Matrix

⎥⎤

⎢⎡ ⋅⋅⋅ wxvxux kijiii

Obtain the coordinate of from the coordinate of

uvwxyz RPP =⎥⎥⎥

⎦⎢⎢⎢

⎣ ⋅⋅⋅⋅⋅⋅=

wzvzuz

wyvyuy

kkjkikkjjjijR

QPP =

uvwP

xyzP

⎥⎤

⎢⎡⎥⎤

⎢⎡ ⋅⋅⋅

⎥⎤

⎢⎡ xu pp zuyuxu kijiii

Dot products are commutative!


xyzuvw QPP =

TRRQ == −1

31 IRRRRQR T === − <== 3X3 identity matrix

⎥⎥⎥

⎦⎢⎢⎢

⎣⎥⎥⎥

⎦⎢⎢⎢

⎣ ⋅⋅⋅⋅⋅⋅=

⎥⎥⎥

⎦⎢⎢⎢

⎣ z

y

w

v

pp

pp

zwywxw

zvyvxv

kkjkikkjjjij

Example 2A point is attached to a rotating frame, the frame rotates 60 degree about the OZ axis of the reference frame.Find the coordinates of the point relative to the reference

)2,3,4(=uvwa

Find the coordinates of the point relative to the reference frame after the rotation.

⎤⎡−⎤⎡⎤⎡ −

=

598.040866.05.0

)60,( uvwxyz azRota


⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

2964.4598.0

234

10005.0866.00866.05.0

13

Example 3A point is the coordinate w.r.t. the reference coordinate system, find the corresponding point w.r.t. the rotated OU-V-W coordinate

)2,3,4(=xyza

uvwapoint w.r.t. the rotated OU V W coordinate system if it has been rotated 60 degree about OZ axis.

uvwa

⎤⎡⎤⎡⎤⎡

=

598440866050

)60,( xyzT

uvw azRota


⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−=

2964.1

598.4

234

10005.0866.00866.05.0

Composite Rotation MatrixA sequence of finite rotations

matrix multiplications do not commuteprules:

if rotating coordinate O-U-V-W is rotating about principal axis of OXYZ frame, then Pre-multiply the previous (resultant) rotation matrix with an appropriate basic rotation matrixif rotating coordinate OUVW is rotating about its


if rotating coordinate OUVW is rotating about its own principal axes, then post-multiply the previous (resultant) rotation matrix with an appropriate basic rotation matrix

14

Example 4Find the rotation matrix for the following operations:

axis OUabout Rotation axisOW about Rotation

axis OYabout Rotation

αθφ

⎥⎥⎤

⎢⎢⎡

−+−

=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

=

αθαθθαφαθφαθφαφθφ

ααααθθ

θθ

φφ

φφαθφ

SCCCSCSSSCCSCSSCC

CSSCCS

SCuRotwRotIyRotR

00

001

10000

C0S-010

S0C),(),(),( 3

Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 27Post-multiply if rotate about the OUVW axes Pre-multiply if rotate about the OXYZ axes

...Answer ⎥⎥⎦⎢

⎢⎣ −+− αθφαφαφαθφθφ SSSCCSCCSSCS

Example 5Translation along Z-axis with h:

⎥⎤

⎢⎡

00100001

⎥⎤

⎢⎡

⎥⎤

⎢⎡⎥⎤

⎢⎡

⎥⎤

⎢⎡

00100001 ppx uu

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

=

1000100

0010),(

hhzTrans

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

+=

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

=

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣ 111000100

0010

1hp

ppp

hzy

w

v

w

v

y

z P

w

zP


x

y

u

vw

O, O’hx

y

u

vw

O, O’

15

Example 6Rotation about the X-axis by

⎥⎤

⎢⎡ 0001

⎥⎤

⎢⎡⎥⎤

⎢⎡

⎥⎤

⎢⎡ 0001 upx

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

−=

10000000

),(θθθθ

θCSSC

xRot

z

v

wP

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

−=

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣ 110000000

1w

v

pp

CSSC

zy

θθθθ

Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 29x

y

v

u

Homogeneous Transformation

Composite Homogeneous Transformation MatrixMatrixRules:

Transformation (rotation/translation) w.r.t (X,Y,Z) (OLD FRAME), using pre-multiplicationTransformation (rotation/translation) w r t


Transformation (rotation/translation) w.r.t (U,V,W) (NEW FRAME), using post-multiplication

16

Example 7Find the homogeneous transformation matrix (T) for the following operation:

axis OZabout ofRotation axis OZ along d ofn Translatioaxis OX along a ofn Translatio

axis OXabout Rotation

θ

α

ITTTTT


:Answer44,,,, ×= ITTTTT xaxdzz αθ

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

100000000001

100001000010

001

1000100

00100001

100001000000

ααααθθ

θθ

CSSC

a

dCSSC

Homogeneous Representation

A frame in space (Geometric Interpretation)

z),,( zyx pppP

⎤⎡

y

z

⎥⎥⎤

⎢⎢⎡

yyyy

xxxx

pasnpasn

F

ns

a

⎥⎦

⎤⎢⎣

⎡= ××

101333 PR

F(X’)

(y’)(z’)


x⎥⎥⎥

⎦⎢⎢⎢

⎣

=

1000zzzz

yyyy

pasnF

Principal axis n w.r.t. the reference coordinate system

17

Homogeneous Transformation

Translation s

a

y

z

ns

a ns

⎥⎤

⎢⎡ +

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

×

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

10001000100010001

xxxxx

zzzz

yyyy

xxxx

z

y

x

new

dpasn

pasnpasnpasn

ddd

F


⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

++

=

1000

zzzzz

yyyyy

dpasndpasn

oldzyxnew FdddTransF ×= ),,(

Orientation RepresentationEuler Angles Representation ( , , )

Many different typesφ θ ψ

Description of Euler angle representations

Euler Angle I Euler Angle II Roll-Pitch-Yaw

Sequence about OZ axis about OZ axis about OX axis

of about OU axis about OV axis about OY axis

φ φθ θ θ

ψ


of about OU axis about OV axis about OY axis

Rotations about OW axis about OW axis about OZ axisφθ θ θψ ψ

18

Orientation RepresentationEuler Angle I

⎟⎞

⎜⎛

⎟⎞

⎜⎛ − 0010sincos φφ

⎟⎟⎞

⎜⎜⎛ −

=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

0cossin0sincos

,cossin0sincos0001

,1000cossin0sincos

'

ϕϕϕϕ

θθθθφφ

φφ

θφ uz

R

RR


⎟⎟

⎠⎜⎜

⎝

=1000cossin'' ϕϕϕwR

Euler Angle IResultant eulerian rotation matrix:

⎟⎟⎟⎟⎟⎞

⎜⎜⎜⎜⎜⎛

−−

−−

−

=

θφϕφϕφ

θϕθϕφ

ϕφθϕφ

ϕφϕθφ

sincossinsincossin

sinsincoscossin

sincoscossinsin

coscos''' wuz RRRR


⎟⎟⎟⎟⎟

⎠⎜⎜⎜⎜⎜

⎝

−+−

+

θθϕθϕ

θφθϕφ

ϕφθϕφ

ϕφ

cossincossinsin

sincoscoscoscos

sinsincossincos

cossin

19

Euler Angle II, Animated

zw'=

yv'

φ

θ =v"

w"

v"'ϕ

"'

w"'=


xu'

u"

u"'

Note the opposite (clockwise) sense of the third rotation, φ.

zw'=

Euler Angle I, Animated

yv'

φv "

w"v'"

ϕ

w'"=


xu'

θ=u"

u'"

20

Orientation RepresentationMatrix with Euler Angle II

⎞⎛ φφ iii

⎟⎟⎟⎟⎟⎟⎟⎞

⎜⎜⎜⎜⎜⎜⎜⎛

−+

−−

+−

θϕθϕφ

ϕφθϕφ

ϕφ

θφθϕφ

ϕφθϕφ

ϕφ

sinsincoscossin

coscoscoscossin

sincos

sincoscoscossin

cossincoscoscos

sinsin


⎟⎟⎟

⎠⎜⎜⎜

⎝ −θ

θϕθϕcos

sinsinsincos

Quiz: How to get this matrix ?

Orientation RepresentationDescription of Roll Pitch Yaw

Z

Yϕθ

φ


X

θ

Quiz: How to get rotation matrix ?

21

Terimakasih

x

yz

x

yz

x

yzz

y


x

Coordinate Transformations• position vector of P in {B} is transformed to position vector of Pto position vector of P in {A}

• description of {B} as seen from an observer in {A}


Rotation of {B} with respect to {A}

Translation of the origin of {B} with respect to origin of {A}

22

Coordinate TransformationsTwo Special Cases

1 T l ti l

'oAPBB

APA rrRr +=1. Translation only

Axes of {B} and {A} are parallel

2. Rotation only

1=BAR


yOrigins of {B} and {A} are coincident

0' =oAr

Homogeneous Representation• Coordinate transformation from {B} to {A}

'oAPBB

APA rrRr +=

• Homogeneous transformation matrix

⎥⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡

× 1101 31

' PBoAB

APA rrRr


g

⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡= ××

× 10101333

31

' PRrRT

oAB

A

BA

Position vector

Rotation matrix

Scaling

23

Homogeneous TransformationSpecial cases1. Translation

2. Rotation

⎥⎦

⎤⎢⎣

⎡=

×

×

10 31

'33

oA

BA rIT


⎥⎦

⎤⎢⎣

⎡=

×

×

100

31

13BA

BA RT

The Geometric Solution

l

l2Υ2

(x , y) Using the Law of Cosines:

+

−=−−−+=+

−+=

2222

212

22

122

222

)cos(θ)θ180cos()θ180cos(2)(

cos2

ll

llllyx

Cabbac

22

2

l1

Υ1

α

⎟⎟⎠

⎞⎜⎜⎝

⎛ −−+=

−−+=

21

22

21

22

21

21

2arccosθ

2)cos(θ

llllyx

llllyx

2

2

Using the Law of Cosines:

=sinsin

cC

bB Redundant since θ2 could be in the

first or fourth quadrant.

Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 46⎟⎠⎞

⎜⎝⎛=

+=

+=

+

−=

xy2arctanα

αθθ

yx)sin(θ

yx)θsin(180θsin

11

222

222

2

1

l

cb

⎟⎠⎞

⎜⎝⎛+

⎟⎟

⎠

⎞

⎜⎜

⎝

⎛

+=

xy2arctan

yx)sin(θarcsinθ

2222

1l

Redundancy caused since θ2 has two possible values

24

The Algebraic Solution

l2Υ2

Υ(x , y)

1221

11

ccx(1))θcos( θc

cos θc

++=

=

+

ll

( ) ( )+++++=

=+=+

++++ 211212

212

22

12

1211212

212

22

12

1

2222

)(sins2)(sins)(cc2)(cc

yx)2((1)

llllllll

l1

Υ1

21

21211

21211

θθθ(3)sinsy(2)ccx(1)

+=+=+=

+

+

llll


( ) ( )( )

⎟⎟⎠

⎞⎜⎜⎝

⎛ −−+=∴

++=

+++= ++

++++

21

22

21

22

2

2212

22

1

211211212

22

1

21121212112112121211

2yxarccosθ

c2

)(sins)(cc2

)()()()(

llll

llll

llllOnly Unknown

))(sin(cos))(sin(cos)sin(

))(sin(sin))(cos(cos)cos(:

abbaba

bababaNote

+−

+−

−+

+−

=

=

))(sin(cos))(sin(cos)sin(

))(sin(sin))(cos(cos)cos(:

abbaba

bababaNote

+−

+−

−+

+−

=

=

sinsy

)()c(c ccc

ccx

21211

2212211

21221211

21211

ll

slsllsslll

ll

+=

−+=−+=

+=

+

+

We know what θ2 is from the previous

)c(s)s(c cscss

2211221

12221211

llllll++=

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

221122221

221

221

2211

)c(s)s()c()(xy

)c()(xc

+++

+=

++

=

lllll

slsll

sls

Substituting for c1 and simplifying many times


( )

2222221

1

2212

22

1122221

yxx)c(ys

)c2(sx)c(

1

+

−+=

++++

=

slll

llllslll

Notice this is the law of cosines and can be replaced by x2+ y2

⎟⎟⎠

⎞⎜⎜⎝

⎛

+

−+= 22

222211 yx

x)c(yarcsinθ slll

25

Homogeneous TransformationComposite Homogeneous Transformation Matrix

1z2z

2y

0x

0z

0y1

0 A2

1 A

1x

1y 2x

?


21

10

20 AAA =

?i

i A1− Transformation matrix for adjacent coordinate frames

Chain product of successive coordinate transformation matrices

Example 8For the figure shown below, find the 4x4 homogeneous transformation matrices and for i=1, 2, 3, 4, 5

⎥⎤

⎢⎡ xxxx pasn

ii A1−

iA0

c ⎥⎤

⎢⎡− 0001

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

1000zzzz

yyyy

xxxx

pasnpasnp

F

a

b

c

d

e

2z

3y3x

3z

4z

4y4x

5x5y

5z⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

−−+−

=

1000010100

10

dace

A

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

−−−

=0001

100010

21 da

b

A


0x 0y

0z1x

1y

1z2x

2y

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡+−−

=

10000100

001010

20 ce

b

A

⎥⎦

⎢⎣ 1000

Can you find the answer by observation based on the geometric interpretation of homogeneous transformation matrix?

26

Orientation Representation

⎥⎦

⎤⎢⎣

⎡= ××

101333 PR

F

Rotation matrix representation needs 9 elements to completely describe the orientation of a rotating rigid body. Any easy way?

⎦⎣ 10


Euler Angles Representation

kinematika manipulator robot -...

Documents