Chapter 3. Differential Motions and Velocitiesmercury.hau.ac.kr/sjkwon/Lecture/robotics/Robotics-chap3.pdf · 2019. 9. 24. · Robotics (School of AME, KAU) 로봇공학, Chapter

Robotics (School of AME, KAU)

로봇공학, Chapter 3

3-1

Chapter 3. Differential Motions and Velocities

속도 차원의 로봇 기구학

로봇공학 (Robotics)

◆ Differential motions of Frames and Robot joints

◆ Robot(manipulator) Jacobian

◆ Inverse differential Kinematics



3-2

Differential Kinematics

▪ Purpose of the differential kinematics:

• To derive velocity relationships between robot joints and end-

effector(robot hand)

▪ Differential motion is a “small movement”.

▪ Note that robot link’s movement is measured relative to a

current frame attached to the previous link.1( ~ )nq q

0x0y

0z

1x 1y

1z

2x 2y

2z

1q( )nx n

( )ny o

( )nz a

0

1( , , )n nT q q

2q3q

nq1 2( , , , )

( , , , , , )

( , , , , , )

n

n n n x y z

n n n

Jacobian

q q q

x y z

x y z

Joints velocity

End-effector velocity

r

o



3-3

3.2 Differential Relationships

• A two-degree-of-freedom planar mechanism

Velocity diagram

( ) ( )

/

/ /

/ /

1 1 1 1 2 1 2 1 2 1 2

1 1 2

ˆ

ˆ ˆ ˆ

ˆˆ ˆ ˆ0,

ˆˆ ˆ ˆ ˆ ˆ

s c ( ) s( ) c( )

ˆ ˆˆ ˆ, ( ˆ)

A o A o

o A o OA A O

B A B A A AB B A

O AB

e

eA

v v v

v v r

v v v v r

l i j l i j

k k

v

= +

= =

= + = + =

= − + + + − + + +

= = + =

• Kinematics of rigid bodies

ˆ ˆˆ i jB x yv p p= +



3-4

Differential Relationships

( ) ( )/

1 1 1 1 2 1 2 1 2 1 2

1 1 2 12 2 12 1

1 1 2 12 2 12 2

s c ( ) s( ) c( )

B A B A

Bx

By

x

v v v

l i j l i j

v l s l s l s

v l c l c l c

p x

= +

= − + + + − + + +

− − − → =

+

•

=

2-link manipulato

1) Velocity

2) Position

r 의예

( )

( )

1 1 2 1 2 1 2

1 1 2 1 2 1 2

1 1 1 2 12 1 2 1 1 2 12 2 12

1 1 2 12 2 121 1 1 2 12 1 2

cos cos 1 12

sin sin 1 12

( )

( )

B

y B

B B

BB

l l l c l c

p y l l l s l s

dx l s d l c d d dx l s l s l s

dy l c l c l cdy l c d l c d d

= + + = +

= = + + = +

= − − + − − − → → =

+= + +

Jacobian matrix

1

2

d

d

1

2

1l

2l

1

A

( , )B x y

Differential motionof end-effector(B)

Differential motionof joints



3-5

3.3 Robot Jacobian

1 1 1 2

2 2 1 2

1 2

1 2

)

( , , , )

( , , , )( , , , )

( , , , )

i j

n

n

i i n

m m n

Y x

Y f x x x

Y f x x xY f x x x

Y f x x x

•

=

==

=

A set of equations, (function of a set of variables

1 2

1 11 1

1

2 22 1

1

1

1

( , , , )i

n

n

n

n

n

m mm n

n

Y

x x x

f fY x x

x x

f fY x x

x x

f fY x x

x x

•

= + +

= + +

= + +

Differential change (motion) of w.r.t the differenetial

change of

End-effectorPosition and orientation

Joint angles

0

0 0 0 1

x x x x

y y y y

n

z z z z

n o a p

n o a pT

n o a p

=

ii j

j

fY x

x

=



3-6

Robot(Manipulator) Jacobian

1 1 1

1 2

1 1

2 2 2

2 2

1 2

1 2

n

n

m n

m m m

n

f f f

x x xY x

f f fY x

x x x

Y xf f f

x x x

Manipulator Jacobian

=

( ) ( )1 2, , , , , , ,

6

x y z nx y z q q q

Manipula o

m n

t

m

r

=

=

differential motion of differential motion the hand-frame of robot joints

(6-DOF) matrix

Jacobian

#n = of joints

êv

ê



3-7

Robot Jacobian

xy

zn

o

a

1q2q

1

2

x

n y

z

Jacobian

x

q y

q z

q

⎯⎯⎯⎯

= =

→

Differential motion

of robot joints :

Differential translation& rotation of end-eff

(Vel

ector

oc w.ity r.) t. rq p

J eference frame

( ) →p = J q p = J q q

x

y

z

differential rotation of hand:

around the (x,y,z)-axes Text와 notation 차이 주의!



3-8

3.6 Differential Motions of a Frame

( , , )

sin ,cos 1, 0 1

1 0 0 1 0 1 0

( , ) 0 1 , ( , ) 0 1 0 , (z, ) 1 0

0 1 0 1 0 0 1

x y

y z

x x y z z

x y

dx dy dz

R x R y R

−

= − = = −

•

•

Differential translations

(Note)

Differential Rotations

( , ) ( , ) ( , ) ( ,

ˆ

)x y y xR x R y R y R x

q

•

•

=

In differential motions, it can be assumed that

:

Differential Rotations about a general axis

: Composed of three differentia

l motion abo

교환법칙성립

( , ) ( , ) ( , ) ( , )

( , ) ( , ) ( , ) ( , ) ( , ) ( , )

1

1

1

x y z

y x z z y x

z y

z x

y x

R q d R x R y R z

R y R x R z R z R y R x

=

= =

−

= − −

ut the three in any oaxes

rder

Higher order terms can be neglected(xy, -xyz, xz, yz)



3-9

Differential Motions of a Frame

1

1( , , ) ( , )

1

0 0 0 1

:

:

z y

z x

y x

dx

dyTrans dx dy dz R d

T

dz

T

dT

k

−

− = −

•

Let original frame

the c

Differential Transformation of a Fra

hange of after differential transf

me

ormatio

( , , ) ( , )

( , , ) ( , )

0

0

0

0 0 0 0

z y

z x

y x

T dT Trans dx dy dz R k d T

dT Trans dx dy dz R k d I T

T

dx

dy

dz

+ =

→ = −

=

−

− =

•−

then,

differntial operator:

n,



3-10

3.9 Differential Motions of a Robot (end-effector)

6 6

x

y

z

x

y

z

•

=

Forward kinematics in velocity level (

of the differential motions (6-DO Linear relationshi F manipulat )p ors

정기구학)

Robot

Jacobian ( )

1

2

3

4

5

6

q

q

q

q

q

q

p = J q

Differential motion ofrobot jointsDifferential motion

of the hand frame

Function of robot’s configuration(D-H parameters)and of its instantaneous locationand orientation



3-11

Inverse Instantaneous Kinematics

1

1

2

3

4

5

6

1( ,

q

q

q

q

qq

q

−

•

→

•

=

Inverse kinematics in velocity level (

:when is square.

In case of 6-DOF manipulators (n=6)

- Joint velocities:

역기구학)

p = q q = p

J J J

J 1

6 66, )

( )

x

y

z

x

y

z

i i

v

v

v

q t

q

q t d

−

=

- Then, joint angles :

End-effector velocity(must be given in path planning stage!)

1( )T T+ −→

In case the Jacobian is not square

Pseudo-inverse:

q = p J J=

J

J J p



3-12

( )f

•

→ →

•

비선형관계

선형관계

:

Position kinematics of manipulators (chap. 2)

inverse kinematics

Velocity kinematics of manipulaors (chap. 3)

:

q = ?

p = q

p = J

1

( )i iq t q dt

−

→

→

→ =

inverse differential kinematics

joint velocoity

joint angles :

q = J p

q



3-13

Path Planning

Robot trajectory (A → B)

: Position profile & velocity profile

( ), ( ), ( )

( ), ( ), ( )

e e ex t y t z

t t

t

t 기준좌표계에대한 자

좌표계

위치궤적

속도궤

원점의위

적

치:

좌표계

세

1) Position trajectory ( )

2) Velocity trajector

[position] end-effector

[translational velocit

[orientatio

y] end-effec

y

to

n

r

] :

)

(

( ), ( ), ( )

( ),

( )

( ), ( )

, ( ), ( )

e e e

x y z

x t y t z t

t t

t t t

t

원점의선속도

에대한 각속도:

자세의변화율

[angular velocity] (Orient

:

or (x,y,z)

ation rate):

-axis

1 1,q q

2 2,q q

3 3,q q

A

B

xy

z

,

• End-effector trajectory about the reference frame

at every sampling time



3-14

Inverse (differential) Kinematics

1 1,q q

2 2,q q

3 3,q q

A

B

Robot trajectory (A → B)

( )( )

, ,

, ,

(x,y,z)

RPY rate ?

x y z

• 기준좌표계 에대한 각속도 와

의관계는

1

1 12

x

n y

z

x

q y

q z

q

− −

→ =

p = q q p J =J J

2) Inverse differential kinematics:주어진 end-effector의 선속도/각속도 궤적에 대하여각 joint의 각속도를 계산→ 로봇 궤적 제어에 이용

1) Inverse kinematics:주어진 end-effector의 위치/자세 궤적에 대하여매 샘플링 시간마다 각 joint 의 각도를 계산→ 로봇 궤적 제어에 이용

xy

z

Basic concept of

Resolved motionrate control (RMRC)



3-15

Joint Control

Single jointdynamics

K(s)

Joint controller(ex. PID)

( )Command

dq t ( )q t( )t( )e t ( )u t

( )d t

+

−

++

▪ Single loop



3-16

Joint Trajectory Control

▪ n-dof robot manipulator의 경우 (multi-loop)

Joint controllers(ex. PID)

Joint spacetrajectory

Joint output

Robot dynamics

(n개 연립미방으로모델링)

K1(s)1

Command

dq1q1e 1u+

−

Kn(s)ndq ne+−

nqnu

2dqK2(s)

2u2q

Joint sensor(encoder)

Desired end-effectorTrajectory

(Cartesian space)

Inverse kinematicssolution

Pathplanning

Forw

ard

kin

em

atics

Actual end-effectortrajectory



3-17

3.10 Calculation of the Jacobian

1 1 2 2 3 3 1 1

0 0 0 1

( ) ( ) ( ) ( ) ( )

x x x x

y y y y

z z z z

R

H n n n n

n o a p

n o a p

n o a pT A A A A A− −

•

= =

position of the end-effect

From the forward kinematics of the robot (n-DOF case)

we ge or (hant the

d)

1 2

1 2 6 1 2

1 2

1 2 6

6

1

1 2

1 2

( , , ) ~

x x x x x xx n

n

x

y y y y

y n y

z

x

z z zz n

y z n

p p p p p pp q q q

q q q q q qp

p p p pp q q q p

q q q qp

p p pp q q q

q q

p p p

q

= + + +

= + + + → = + + +

=

function

s of

=

1

2

1 2

1 2

( )

y y

n

nz z z

n

n

p

q

p p q

q q

qp p p

q q q

3 matrixPosition Jacob

ian

J

→ Pev = J q



3-18

Position/Orientation Jacobian

1

2

3

1

1

2

3

1

(3 )

(3 )

Position Jacobian

OrientationJacobian

n

n

x

y

z

n

n

q

qx

qy

zq

q

q

q

q

q

n

n

q

−

−

=

=

P

O

,

J

J

1

2

3

1

1 2

1 2

(3 )

(3 )

,x

ny

z n

P P P Pn

O O O

qx

qy

q

q

nz

q

n

−

− − − − −

=

=

−

P

O

J J J JJ =

J J

J

J

J

On

J

(Ref.) Sciaviscco & Sicilliano, “Modeling and control of robot manipulators”

→ p = J q

Pe

e O

J

J

v = q

= q



3-19

Position/Orientation Jacobian 계산을 위한 일반식

1 2

1 2

1 1

1

1

•

ˆ ˆˆ ( )

ˆ

•

ˆ

0

P P P Pn

O O O On

Pi i e i

Oi i

Pi i

Oi

J J J JJ

J J J J

J z p p

J z

J z

J

− −

−

−

= =

− =

=

For i-th revolute joints

For i-th prismatic joints

(Ref.) Asada & Slotine, “Robot analysis and control”



3-20

Position Jacobian

( ) ( ) ( )

( ) ( )

1 2 1 3 2 1

1 0 2 1 1 1 1

0 1 1

[1]

ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ

ˆ ˆ ˆ ˆ ˆ

ˆ( )

e e e n e n

e e n n e n

e

v p p p p p p p

z p z p p z p p

z p z

−

− −

= + − + − + + −

= + − + + −

= +

e

( ) Tra

(1) Revolute jo

nslational velocity of end-effector

ints

선속도

( ) ( )

( ) ( )

( )

0 1 1 1

1 2 1 1

1

1

1

ˆ ˆ ˆ ˆ

ˆ ˆ

:

ˆ ˆ ˆ

e n e n n

x

i e i

ey

n

e n n

z

e

P

p p z p p

v

v p

p p

z z p p pz p

v

−

−

−

−

−

− + + −

→ =

−

−

−

: i-th joint e

(2) Prismatic jo

nd-effector

i

각속도가 속도에기여하는 양

J

1

1 0 2 1 1 0 1 1ˆ

x

n n y n

z nP

v d

v d z d z d z v z z z

v d

− −

= + + + → =

e

nts

J



3-21

Orientation Jacobian

1 2 3

1 0 2 1 1

1 0 2 1 1

0

( [0 0 1]

( [0 0 1]

ˆn

n n

n

T

i

T

i in

z z z z

z z

z

z Rz

−

−

= + + + +

= + + +

= + + +

= =

=

e

w.r.t. c

[2]( ) Angular velocity of end-effector

urren

t {i} frame

(1) Revolute joi ts

)

w

n

각속도

0

0 1

0 0 0

1

2 1 1 2 1

2 1 3 2 1

1 1

ˆ ˆ ˆ ˆ

n n

x

y

z n n

n n

O

z z z z

z R z

z R z R z R

R

z

z R z

− −

−= + + + +

→ = =

.r.t. fixed reference fram

(2) Prismatic join

ts

N

e)

:

J

ˆ 0 0i OiJ = → =

o contribution to the end-effector angular velocity



3-22

1ˆ ˆ

e ip p −− 1

ˆ ˆ ˆ( )i e ip p −= −

1ˆ

i iz −=

i-th Revolute joint

i-th Prismatic joint

êp

1

ˆˆ

i i iz −=

1ˆ

ip −

i-th link

êv

ê ii-th joint

end-effector

각속도( )가

속도에기여하는 양

ii-th joint

end-effector

각속도( )가

각속도에기여하는 양



3-23

Position/Orientation Jacobian

0

0 1 1

1 1 1 2 2 3 3 1 1

1 1

0 0 0 1

( ) ( ) ( ) ( )0 1

, ,

x x

y y

z z

x

y y

x

zz

i i

i i

x

y

i

i i y

z

x

z

n o

n o

n o

a

a

a

p

p

p

R pT A q A q A q A q

p z

a

p

p

a

p a

− −

− − −

− −

= = =

→ = =

{i-1} frame z-axis unit vector [0 0 1]’의 기준좌표계에 대한

direction cosines

기준좌표계에 대한{i-1} frame 원점의 위치

0

1 1 1

0

0

1

x

y

z

i i

x

y

z

ip z R

p

p

a

a

ap

− − −

• = • = =

1 1i ip z− −• How to get and ?



3-24

RPY rate에 대한 Jacobian

( )

( )sin

, ,

, ,

ˆ

0

0

cos cos

cos cos sin

s

1 0

in

x y z

x y z

x

y

z

x

y

z

i j k

s c c

c c s

s

•

+ + = + +

−

= − +

=

=

+

= −

(x,y,z)

RPY rate ?

기준좌표계 에대한 각속도 와

의관계는

e =

1

1

1

( ) ( )

( )

x

RPY RPY y

z

RPY RPY

P

O

ORP

P

PY R Y

T T

T

T

q q

q

−

−

−

→ =

→

• =

=

Analytic Jacobian:

A

JJ

J J

JJJ =

(Ref.) Sciavicco and Siciliano, Modeling and control of robot manipulators



3-25

Analytic Jacobian

1

2

3

4

5

6

x

y

z

x

y

z

x

y

z

P

O

P

RPY

v

v

v

v

v

v

=

=

or

A

J J

J

J J

J

p = q

p = q

1

2

3

4

5

6

1

1

x

y

z n

n

→

= →

(1)

or

(2)

O O

RPY RPY

e

e

= qJ =

q =

J

J J



3-26

[H.W. #2] (125점)

▪ Example 3.1 ~3.5, 3.9~3.16 (13 probs.)

(5점 x 13 = 65점)

▪ Determine Jacobian matrix of the PUMA type robot

(Example 2.25) (30점)

▪ Determine Jacobian matrix of the stanford arm

(Example 2.26) (30점)

Documents

Chapter 3. Differential Motions and Velocitiesmercury.hau.ac.kr/sjkwon/Lecture/robotics/Robotics-chap3.pdf · 2019. 9. 24. · Robotics (School of AME, KAU) 로봇공학, Chapter