chapter 3. differential motions and...
TRANSCRIPT
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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= +
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
=
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
ˆev
ˆe
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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 차이 주의!
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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)
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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,
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
3-11
Inverse Instantaneous Kinematics
1
1
2
3
4
5
6
1( ,
q
q
q
q
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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)
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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”
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
3-22
1ˆ ˆ
e ip p −− 1
ˆ ˆ ˆ( )i e ip p −= −
1ˆ
i iz −=
i-th Revolute joint
i-th Prismatic joint
ˆep
1
ˆˆ
i i iz −=
1ˆ
ip −
i-th link
ˆev
ˆe ii-th joint
end-effector
각속도( )가
속도에기여하는 양
ii-th joint
end-effector
각속도( )가
각속도에기여하는 양
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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 ?
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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
Robotics (School of AME, KAU)
로봇공학, Chapter 3
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점)