View
227
Download
0
Tags:
Embed Size (px)
Citation preview
1
0y
1z
1y
1x
0x
0z
0o
1o
2z
3z
2x
3x
1
2
3
Forward Kinematics
3321321
3
00 ),,,,,( PlllTP
1l
2l
3l)()( 1 i
i
iii qTqA
321
332211
3
0 )()()(
AAA
qAqAqAT
2
DH Representation
1000
0
,,,,
i
i
i
xaxdzzi
dcs
sascccs
casscsc
RotTransTransRotA
ii
iiiiii
iiiiii
iiii
i
id
i
ia
i
i
i
i
joint of angle
link ofoffset
link of twist
link oflength
iii dq or variablefor the stands
3
DH Representation - Frames
• (Convention) Frame i is attached to link i. The inertial frame is Frame 0 and Earth is link 0. Joint i joins links i-1 and i.
• (Another convention) The joint i+1 rotates about axis z i
• (DH1) The axis xi is perpendicular to the axis zi-1
• (DH2) The axis xi intersects the axis zi-1
• DH convention imposes two constrains thus enabling the use of only four parameters instead of six.
4
0y
1z
1y
1x
0x
0z
0o
1oa
1
0d
1
1
iiii xaxdzzi RotTransTransRotA ,,,,
3332
232221
131211
010101
010101
010101
1
0
0...
...
...
rr
rrr
rrr
kkkjki
jkjjji
ikijii
R),(),( 2111 scrr
),(),( 3332 scrr
,,
1
0 and Compare xz RRRDH1
d
5
0y
1z
1y
1x
0x
0z
0o
1oa
1
0d
1
1
iiii xaxdzzi RotTransTransRotA ,,,,
d
as
ac
aRidkd
10
1
0
) (i.e., and
ofn combinatiolinear a is
001
1
0
kzRi
dDH2
d
6
Assign frames at the two joints and at the end-
point based on DH convention
Write A1, A2, and A3
1
2
1l
2l
Link ai αi di
1
2
i
•Frame i is attached to link i. •The inertial frame is Frame 0 and Earth is link 0. •Joint i joins links i-1 and i.•The joint i+1 rotates about axis zi
iiii xaxdzzi RotTransTransRotA ,,,,
7
1000
100
0
0
1
1
1
1111
111
l
slcs
clsc
A
1000
0100
0
0
122111212
122111212
21
2
0
slslcs
clclsc
AAT
1000
100
0
0
1
2
2
2222
222
l
slcs
clsc
A
1
2
1l
2l
8
Three link cylindrical robot
Link ai αi di i
1
2
3
1000
0
,,,,
i
i
i
xaxdzzi
dcs
sascccs
casscsc
RotTransTransRotA
ii
iiiiii
iiiiii
iiii
2d
3d
1l
1
9
1000
100
00
00
1
111
11
l
cs
sc
A
1000
0
,,,,
i
i
i
xaxdzzi
dcs
sascccs
casscsc
RotTransTransRotA
ii
iiiiii
iiiiii
iiii
1000
110
0100
0001
2
2 dA
1000
100
0010
0001
3
3 dA
1000
010
0
0
21
3
3
321
3
0111
111
dl
dccs
dssc
AAAT
2d
3d
1l
1
12
Inverse Kinematics
1000333231
232221
131211
6
0
z
y
x
drrr
drrr
drrr
T
621
6
0
6
0 q,,q,q find d and Given R
13
1
2
1l
2l
1000
0100
0
0
122111212
122111212
2
0
slslcs
clclsc
T
p
112211
12211
slsl
clcl
p
p
p
p
z
y
x
Dll
llpp yx
21
2
2
2
1
22
2 2cos
)1,tan( 2
2 DDA
),tan(),tan( 222211 slcllAppA yx
14
Kinematic Decoupling
• Last three joints intersecting at a point (spherical wrist)– Inverse position kinematics– Inverse orientation kinematics
• Let R )q,,q,(q 621
6
0 R
d )q,,q,(q 621
6
0 d
15
cd0
6
0d
6, za
0z
4z
6oo
Rkddd
Rkdooc
60
66
6d
effector)-end theoflength theis ( 6d
1
0
0
k
1. Find q1, q2, q3 such that is located at o.
2. With q1, q2, q3 obtain
3. Solve for q1, q2, q3 using the expression for from forward kinematic equations and its numerical value obtained from
cd0
RRR T)( 3
0
6
3
6
3R
3
0R
16
0y
0x
0z
p
0x
0y
),( yx pp1
),tan(1 yx ppA
r
0z),( sr
22
yx ppr
1lps z 2
3
2a
3aDaa
aasr 32
2
3
2
2
22
3 2cos
)1,tan( 2
3 DDA
),tan(),tan( 333322 sacaaAsrA
1l
18
Velocity Kinematics
1000
0100
0
0
122111212
122111212
2
0
slslcs
clclsc
T
012211
12211
slsl
clcl
p
p
p
p
z
y
x
1
2
1l
2l
p
0z
1z
2z
0x
1x
2x
100
0
0
1212
1212
2
0 cs
sc
R
2
0
2
0
2
0 ][ RSR
)( 2
0
2
0 ddtd
v
Angular velocity of frame 2 wrt frame 0
velocity of p wrt frame 0
19
2
0
2
021122112
21122112
2
0 ][
000
0)()(
0)()(
RSsc
cs
R
11
00
00
;0
0
000
00)(
0)(0
100
0
0
000
0)()(
0)()(
][
2
1
21
2
021
21
1212
1212
21122112
21122112
2
0
JJ
cs
sc
sc
cs
S
20
2
1
2
112212211
12212211
21122111
21122111
00
0
)(
)(
v
z
y
x
Jclclcl
slslsl
clcl
slsl
p
p
p
dtd
p
0012212211
12212211
clclcl
slslsl
J v
21
n
n
nn RRR 1
1
0
3
2
2
0
2
1
1
0
1
00
1 ii
i
i kq
prismaticjoint ith if 0
revolutejoint ith if 1
i
so that Note 1
01 kRz i-
i
nnn
n qzqzqzqz 13232121010
Angular Velocity
23
Linear Velocity
n
ii
i
nnn q
qd
dv1
000
prismaticjoint ith if
revolutejoint ith if ][
1
1110
i
n
ii
i
n
i
i
n
z
dzS
qd
qd
)4,3:1()4,3:1( 1
001
inn
i TTd
24
Velocity Kinematics - 2 DOF
1000
0100
0
0
122111212
122111212
2
0
slslcs
clclsc
T
1
2
1l
2l
p
0z
1z
2z
0x
1x
2x
1000
0100
0
0
111
111
1
1
1
slcs
clsc
A
1000
0100
0
0
222
222
2
2
2
slcs
clsc
A
vJJddzz ,,,,, :for sexpression Write 2
1
2
010
25
Velocity Kinematics - 2 DOF
000
;
0122
122
11
11
12211
12211
2
112211
12211
2
0 sl
cl
sl
cl
slsl
clcl
dslsl
clcl
d
0z
1
2
1l
2l
p
1z
2z
0x
1x
2x
1
0
0
;
1
0
0
00 zz
;
00
)()( 12212211
12212211
2
11
2
00
clclcl
slslsl
dzSdzSJ v
11
00
00
10
zzJ
26
Singularities
T
n
T
zyxzyx
qqqq
vvvX
dqqJdXqqJX
],,,[
],,,,,[
))( (also )(
21
The values of q for which the rank of J decreases are called the singularities or singular configurations.
27
Singularities 2-DOF
;
00 2
112212211
12212211
clclcl
slslsl
v
v
v
z
y
x
Velocity can be given in only x and y direction.
0))(()( 1221221112212211 slclclclslsl
when theta2 is zero. In that configuration arbitrary velocity in x and y direction cannot be imparted. One example is velocity in line with the manipulator when theta2 is zero.
28
Importance of Singularities
• certain direction of motion may be unattainable.• bounded gripper velocities to unbounded joint
velocities.• bounded gripper forces and torques may correspond
to unbounded joint torques.• usually points on the boundary of the workspace.• unreachable points in workspace under small
perturbation of link parameters such as length, offset, etc.
• non-unique solution to inverse kinematics problem.
29
Inverse kinematics and Jacobian
T
n
T
zyxzyx
qqqq
vvvX
XqJqdqqJdXqqJX
],,,[
],,,,,[
))( and )( (also )(
21
1
For desired end-point position and orientation as a function of time (the usual robot operation) X-dot can be evaluated and by inverting the Jacobian and so by integrating from a known initial value of q, other values of q can be obtained for given positions and orientations.
This method is great but for singularities of the Jacobian.
30
Dynamics
• Differential equation relating input torques and forces to the positions (angles) and their derivatives.
• Like force = mass times acceleration.
)(),()( qgqqqcqqD
anglesjoint of vector theis
uesinput torq of vector theis
q
31
Euler-Lagrange Equations
• Equations of motion for unconstrained system of particles is straightforward (F = m x a).
• For a constrained system, in addition to external forces, there exist constrained forces which need to be considered for writing dynamic equations of motion.
• To obtain dynamic equations of motion using Euler-Lagrange procedure we don’t have to find the constrained forces explicitly.
32
Holonomic ConstraintsF
(x,y)
m
l222 lyx
mjtqq nj ,,1,0),,,(
sconstraint holonomicfor expression General
1
33
Nonholonomic Constraints
)(sin)(
)(cos)(
ttry
ttrx
mjtqqqqf nnj ,,1,0),,,,,,( 11
x
y
General expression for nonholonomic constraints is:
Nonholonomic constraints contain velocity terms which cannot be integrated out.
A rear powered front steering vehicle
34
krr ,,1
Consider a system of k particles, with corresponding coordinates,
kiqqrr
nii
n
,,1),,,(
and ,,
1
1
Often due to constraints or otherwise the position of k particles can be written in terms of n generalised coordinates (n < k),
In this course we consider only holonomic constraints and for those constraints one can always find in principle n (n < k) independent generalised coordinates.
35
Virtual Displacements
ir
kidtt
rdq
q
rdr i
n
jj
j
ii ,,2,1,
1
Define virtual displacements from above by setting dt=0.
kiqq
rr
n
jj
j
ii ,,2,1,
1
In our case dqj are independent and satisfy all the constraints. If they additional constraints have to be added to dqj to finally arrive at a statement of virtual displacements which have only independent dqj, we can replace dqj with jq
36
Virtual Work
forces ngconstraini theare
forces external theare )(a
j
j
f
f
k
jj
Ta
jj
k
jj
T
j rffrFW1
)(
1
)(
Let Fi be total force on every particle, then virtual work is defined as:
Constraining forces do no work when a virtual displacement takes place (as is the case with holonomic constraints), so in equilibrium
0)(1
k
jj
T
j rfW
37
D’Alembert’s Principle
D’Alembert’s principle states that, if one introduces a fictitious additional force, the negative of the rate of change of particle i, then each particle will be in equilibrium.
dt
drmprpfW j
j
k
jj
T
jj
,0)(1
38
Generalised Forces
is called the i-th generalised force. The equations of motion become:
.,,2,1,
where
1i
11 11
niq
rf
qqq
rfrf
k
j i
jT
j
i
n
ii
k
j
n
ii
i
jT
j
k
jj
T
j
.0))((11
i
k
j i
jT
ji
n
ii q
q
rrm
39
F
(x,y)
m
l
222 lyx
cos
sin
coordinate dGeneralise
;sin
cos
1,1
1
1
1
F
F
F
FF
q
l
l
y
xr
nk
y
x0))((11
i
k
j i
jT
ji
n
ii q
q
rrm
40
F
(x,y)
m
l
222 lyx
cos
sin
coordinate dGeneralise
;sin
cos
1,1
1
1
1
F
F
F
FF
q
l
l
y
xr
nk
y
x
0)(
0))((
1
11
yym
xxm
rrm i
k
j i
jT
ji
n
ii
FlFlFl
l
lF
q
rf T
j i
jT
j
22
1
2
1
sincos
sin
cos
2222
2222
))(cossin()cos(
))(cossin()sin(
llx
y
llx
x
Fml
:ismotion ofequation The
41
Euler-Lagrange Equations of Motion
k
j i
jT
ji
i
jT
ji
k
j i
jT
ji
q
r
dtd
rmq
rrm
dtd
q
rrm
1
1
)()(
)(
i
j
l
n
l li
j
i
j
i
j
i
j
i
n
i
jT
jj
q
vq
r
q
r
dtd
q
r
q
v
rrv
1
2
11
and
Then
)(
42
k
j i
jT
ji
i
jT
ji
k
j i
jT
ji q
vvm
q
vvm
dtd
q
rrm
11
)(
ii
k
j i
jT
ji
k
jj
T
jj qK
q
Kdtd
q
rrmvvm
11
)(21
K
be K toenergy kinetic theDefine
.,2,1,0
0))((111
niqK
q
Kdtd
qqK
q
Kdtd
rrm
i
ii
n
iii
ii
i
k
j i
jT
ji
n
ii
43
K-VL
niqL
q
Ldtd
qV
i
ii
i
i
i
Where
.,2,1,
Then
such that
forces),or torques(external tau and energy) (potential V
exist theresuppose Finally,
44
F
(x,y)
m
l
y
xyxmK21
.,2,1, niqL
q
Ldtd
i
ii
Write the dynamic equations of motions for this system.
45
F
(x,y)
m
l
Fl
mly
xyxmK
2
2
21
21
.,2,1, niqL
q
Ldtd
i
ii
Flml
2
:ismotion of
equation dynamic The
46
Expression for Kinetic Energy
B
T
B
T
B
dmzyxvzyxv
dxdydzzyxzyxvzyxvK
mdxdydzzyx
),,(),,(21
),,(),,(),,(21
),,(
BBody
0)(or 1
1,
1,
1
:asgiven is ),,( mass of Centre
B
c
B
c
B
c
B
c
B
c
ccc
dmrrrdmm
r
ydmm
zydmm
yxdmm
x
zyx
47
Attach a coordinate frame to the body at its centre of mass, then velocity of a point r is given by:
mass of centre theofocity linear vel theis
mass of centre sbody' the toattached
frame theoflocity angular ve theis
where
)(
c
c
v
rSvv
4321
)()(21
KKKK
dmrSvrSvK c
T
B
c
48
c
T
cc
T
B
c vvmdmvvK21
21
1
)0 (since 0)(21
)(21
2 c
B
T
c
T
B
c rdmrSvdmrSvK
0)(21
3 dmvrSK c
T
B
)()(21
)()(21
)()(21
4
T
TT
B
T
B
JSSTr
dmSrrSTr
dmrSrSK
BBB
BBB
BBB
T
B
dmzyzdmxzdm
yzdmdmyxydm
xzdmxydmdmx
dmrrJ
2
2
2
Where
49
BBB
BBB
BBB
dmyxyzdmxzdm
yzdmdmzxxydm
xzdmxydmdmzy
I
)(
)(
)(
Where
22
22
22
0
0
0
)(
xy
xz
yz
S
IJSSTrK TT
21
)()(21
4
50
00000
4
0
21
21
21
IRIR
IK
R
TTT
T
T
Frame for I and Omega
The expression for the kinetic energy is the same whether we write it in body reference frame or the inertial frame but it is much easier to write I in body reference frame since it doesn’t change as the body rotates but its value in the inertial frame is always changing.
So we write the angular velocity and the inertia matrix in the body reference frame.
51
Jacobian and velocity
frame) referencebody in is ( )()(
,)(
i
T
ii
vci
qqJqR
qqJv
i
ci
qqJqRIqRqJqJqJmqKn
i
T
iii
T
v
T
vi
T
iicici1
)()()()()()()(21
qqDqK T )()(21
D(q) is a symmetric positive definite matrix and is known as the inertia matrix.
53
Two Link Manipulator
0z
1
2
11, lm
22 , lm
p
1z
2z
0x
1x
2x
Links are symmetric, centre of mass at half the length.
21,,, Find21
cc vv