Upload
pierce-paul
View
229
Download
4
Embed Size (px)
Citation preview
Kinematic relations
6
5
4
3
2
1
z
y
x
X
Joint SpaceTask Space
θ =IK(X)
Location of the tool can be specified using a joint space or a cartesian space description
X=FK(θ)
Velocity relations
Joint Space Task Space
• Relation between joint velocity and cartesian velocity.• JACOBIAN matrix J(θ)
6
5
4
3
2
1
z
y
x
z
y
x
)(JX
XJ )(1
Jacobian• Suppose a position and orientation vector of a
manipulator is a function of 6 joint variables: (from forward kinematics)
X = h(q)
z
y
x
X 16
6
5
4
3
2
1
)(
q
q
q
q
q
q
h
166216
6215
6214
6213
6212
6211
),,,(
),,,(
),,,(
),,,(
),,,(
),,,(
qqqh
qqqh
qqqh
qqqh
qqqh
qqqh
Jacobian MatrixForward kinematics
)( 116 nqhX
qdq
qdh
dt
dq
dq
qdhqh
dt
dX n )()(
)( 116
z
y
x
z
y
x
1
2
1
6
)(
nn
n
q
q
q
dq
qdh
1616 nnqJX
dq
qdhJ
)(
Jacobian Matrix
z
y
x
z
y
x
1
2
1
6
)(
nn
n
q
q
q
dq
qdh
nn
n
n
n
q
h
q
h
q
h
q
h
q
h
q
hq
h
q
h
q
h
dq
qdhJ
6
6
2
6
1
6
2
2
2
1
2
1
2
1
1
1
6
)(
Jacobian is a function of q, it is not a constant!
Jacobian Matrix
Vz
y
x
X
z
y
x
16 nnqJX
z
y
x
V
Linear velocity
z
y
x
Angular velocity
The Jacbian Equation
1
2
1
1
nn
n
q
q
q
q
Example• 2-DOF planar robot arm
– Given l1, l2 , Find: Jacobian2
1
(x , y)
l2
l1
),(
),(
)sin(sin
)cos(cos
212
211
21211
21211
h
h
ll
ll
y
x
)cos()cos(cos
)sin()sin(sin
21221211
21221211
2
2
1
2
2
1
1
1
lll
lllhh
hh
J
2
1
Jy
xY
Singularities• The inverse of the jacobian matrix cannot be
calculated when
det [J(θ)] = 0
• Singular points are such values of θ that cause the determinant of the Jacobian to be zero
• Find the singularity configuration of the 2-DOF planar robot arm
)cos()cos(cos
)sin()sin(sin
21221211
21221211
lll
lllJ
2
1
Jy
xX
2
1
(x , y)
l2
l1
x
Y
=0
V
determinant(J)=0 Not full rank
02 Det(J)=0
Jacobian Matrix• Pseudoinverse
– Let A be an mxn matrix, and let be the pseudoinverse of A. If A is of full rank, then can be computed as:
AA
nmAAA
nmA
nmAAA
ATT
TT
1
1
1
][
][
Jacobian Matrix– Example: Find X s.t.
2
3
011
201x
24
51
41
9
1
21
15
02
10
11
][1
1TT AAAA
16
13
5
9
1bAx
Matlab Command: pinv(A) to calculate A+
Jacobian Matrix• Inverse Jacobian
• Singularity– rank(J)<n : Jacobian Matrix is less than full rank– Jacobian is non-invertable– Boundary Singularities: occur when the tool tip is on the surface
of the work envelop.– Interior Singularities: occur inside the work envelope when two
or more of the axes of the robot form a straight line, i.e., collinear
666261
262221
161211
JJJ
JJJ
JJJ
qJX
6
5
4
3
2
1
q
q
q
q
q
q
XJq 1
5q
1q
Singularity
• At Singularities:– the manipulator end effector cant move in certain
directions.– Bounded End-Effector velocities may correspond
to unbounded joint velocities.– Bounded joint torques may correspond to
unbounded End-Effector forces and torques.
Jacobian Matrix
• If
• Then the cross product
,x x
y y
z z
a b
A a B b
a b
( )y z z y
x y z x z z x
x y z x y y x
i j k a b a b
A B a a a a b a b
b b b a b a b
Remember DH parmeter
• The transformation matrix T
i i i i i i i
i i i i i i i
i i i
c -c s s s a c
s c c -s c a s
0 s c d
0 0 0 1
iA
ii AAAT .....210
19
Where (θ1 + θ2 ) denoted by θ12 and
i i i i i i i
i i i i i i i
i i i
c -c s s s a c
s c c -s c a s
0 s c d
0 0 0 1
iA
0 1
0
0
1
Z Z
1 1 1 1 2 1 2
0 1 1 1 2 1 1 2 1 2
0 cos cos cos( )
0 , sin , sin sin( )
0 0 0
a a a
O O a O a a
Jacobian Matrix2-DOF planar robot arm
Given l1, l2 , Find: Jacobian
• Here, n=2
0 2 0 1 2 11 2
0 1
( ) ( ),
z o o z o oJ J
z z
2
1
(x , y)
l2
l1
Jacobian Matrix
0 2 01
0
( )z o oJ
z
1 1 2 1 2
0 2 0 1 1 2 1 2
1 1 2 1 2 1 1 2 1 2
1 1 2 1 2
1 1 2 1 2
0 cos cos( )
( ) 0 sin sin( )
1 0
0 0 1
cos cos( ) sin sin( ) 0
sin sin( )
cos cos( )
0
a a
Z o o a a
i j k
a a a a
a a
a a
Jacobian Matrix
1 2 12
1
( )z o oJ
z
2 1 2
1 2 1 2 1 2
2 1 2 2 1 2
2 1 2
2 1 2
0 cos( )
( ) 0 sin( )
1 0
0 0 1
cos( ) sin( ) 0
sin( )
cos( )
0
a
Z o o a
i j k
a a
a
a
Jacobian Matrix
2 1 2
2 1 2
2
sin( )
cos( )
0
0
0
1
a
a
J
1 1 2 1 2
1 1 2 1 2
1
sin sin( )
cos cos( )
0
0
0
1
a a
a a
J
1 2J J J
The required Jacobian matrix J
Stanford Manipulator
i i i i i i i
i i i i i i i
i i i
c -c s s s a c
s c c -s c a s
0 s c d
0 0 0 1
iA
The DH parameters are:
Stanford Manipulator
10 1T A
1 2 1 1 2 2 1
1 2 1 1 2 2 120 1 2
2 20 0
0 0 0 1
c c s c s d s
s c c s s d cT A A
s c
0
0
0
1
z
1
1 1
0
s
z c
1 2
2 1 2
2
c s
z s s
c
1 2 1 1 2 3 1 2 2 1
1 2 1 1 2 3 1 2 2 130 1 2 3
2 2 3 20
0 0 0 1
c c s c s d c s d s
s c c s s d s s d cT A A A
s c d c
1 2
3 1 2
2
c s
z s s
c
3 1 2 2 1
3 3 1 2 2 1
3 2
d c s d s
O d s s d c
d c
0 1
0
0
0
O O
2 1
2 2 1
0
d s
O d c
Stanford Manipulator 4
0 1 2 3 4T A A A A
50 1 2 3 4 5T A A A A A
60 1 2 3 4 5 6T A A A A A A
1 2 4 1 4
4 1 2 4 1 4
2 4
c c s s c
z s c s c c
s s
T4 = [ c1c2c4-s1s4, -c1s2, -c1c2s4-s1*c4, c1s2d3-sin1d2][ s1c2c4+c1s4, -s1s2, -s1c2s4+c1c4, s1s2d3+c1*d2][-s2c4, -c2, s2s4, c2*d3][ 0, 0, 0, 1]
3 1 2 2 1
4 3 1 2 2 1
3 2
d c s d s
O d s s d c
d c
Stanford Manipulator T5 = [ (c1c2c4-s1s4)c5-c1s2s5, c1c2s4+s1c4, (c1c2c4-s1s4)s5+c1s2c5, c1s2d3-s1d2][ (s1c2c4+c1s4)c5-s1s2s5, s1c2s4-c1c4, (s1c2c4+c1s4)s5+s1s2c5, s1s2d3+c1d2][ -s2c4c5-c2s5, -s2s4, -s2c4s5+c2c5, c2d3] [ 0, 0, 0, 1]
1 2 4 5 1 4 5 1 2 5
5 1 2 4 5 1 4 5 1 2 5
2 4 5 2 5
c c c s s s s c s c
z s c c s c s s s s c
s c s c c
1 2 4 5 1 4 5 1 2 5
5 1 2 4 5 1 4 5 1 2 5
2 4 5 2 5
c c c s s s s c s c
z s c c s c s s s s c
s c s c c
Stanford Manipulator T5 = [ (c1c2c4-s1s4)c5-c1s2s5, c1c2s4+s1c4, (c1c2c4-s1s4)s5+c1s2c5, c1s2d3-s1d2][ (s1c2c4+c1s4)c5-s1s2s5, s1c2s4-c1c4, (s1c2c4+c1s4)s5+s1s2c5, s1s2d3+c1d2][ -s2c4c5-c2s5, -s2s4, -s2c4s5+c2c5, c2d3] [ 0, 0, 0, 1]
1 2 4 5 1 4 5 1 2 5
5 1 2 4 5 1 4 5 1 2 5
2 4 5 2 5
c c c s s s s c s c
z s c c s c s s s s c
s c s c c
3 1 2 2 1
5 3 1 2 2 1
3 2
d c s d s
O d s s d c
d c
Stanford Manipulator
6
d6s5c1c2c4 d6s5s1s4 d6c1s2c5 c1s2d3 s1d2
d6s5s1c2c4 d6s5c1s4 d6s1s2c5 s1s2d3 c1d2
d6s2c4s5 d6c2c5 c2d3
O
T6 = [ c6c5c1c2c4-c6c5s1s4-c6c1s2s5+s6c1c2s4+s6s1c4, -c5c1c2c4+s6c5s1s4+s6c1s2s5+c6c1c2s4+c6s1c4, s5c1c2c4-s5s1s4+c1s2c5, d6s5c1c2c4-d6s5s1s4+d6c1s2c5+c1s2d3-s1d2][ c6c5s1c2c4+c6c5c1s4-c6s1s2s5+s6s1c2s4-s6c1c4, -s6c5s1c2c4-s6c5c1s4+s6s1s2s5+c6s1c2s4-c6c1c4, s5s1c2c4+s5c1s4+s1s2c5, d6s5s1c2c4+d6s5c1s4+d6s1s2c5+s1s2d3+c1d2][ -c6s2c4c5-c6c2s5-s2s4s6, s6s2c4c5+s6c2s5-s2s4c6, -s2c4s5+c2c5, -d6s2c4s5+d6c2c5+c2d3][ 0, 0, 0, 1]
Stanford Manipulator
0 6 0 1 6 11 2
0 1
( ) ( ),
z o o z o oJ J
z z
Joints 1,2 are revolute
23 0
zJ
Joint 3 is prismatic
3 6 3 5 6 54 6 44 5 6
3 54
( ) ( )( ), ,
z o o z o oz o oJ J J
z zz
The required Jacobian matrix J
1 2 3 4 5 6J J J J J J J
Inverse Velocity– The relation between the joint and end-effector velocities:
where j (m×n). If J is a square matrix (m=n), the joint velocities:
– If m<n, let pseudoinverse J+ where
( )X J q q
1( )q J q X
1[ ]T TJ J JJ
( )q J q X
Acceleration– The relation between the joint and end-effector velocities:
– Differentiating this equation yields an expression for the acceleration:
– Given of the end-effector acceleration, the joint acceleration
( )X J q q
( ) [ ( )]d
X J q q J q qdt
Xq
( ) [ ( )]d
J q q X J q qdt
1( )[ ( )] ]d
q J q X J q qdt