View
0
Download
0
Category
Preview:
Citation preview
n Ø Ø Ø
n Ø Ø
n
n Ø
n Ø ( ) Ø 2 404 Ø E-mail: hariyama@tohoku.ac.jp Ø 7153 ( 022-795-7153)
=
n Ø
n Ø
+
+
: →
XYZ:
X1Y1Z1:
6 : : x y z : α β γ (X1Y1Z1→XYZ )
6 → ( ) 6
6 ) →3 2 6
5 6 7
?
n n n
n Ø
n Ø
2 (q1 q2)
( )
(xd yd)
( )
( )
n Ø
n Ø
τ(t)
(xd(t) yd(t))
( )
( ) : → :
( )
(q1 q2)
(xd yd) ?
(q1 q2)
(xd yd) ?
−π ≤ q1 < +π−π ≤ q2 < +π
−qlower + nπ ≤ q < qupper + nπ
qlower ∈ 0,π[ ] qupper ∈ 0,π )⎡⎣n ( )
0 ≤ cos−1 x ≤ π
−π2≤ sin−1 x ≤ π
2
− π2≤ tan−1 x ≤ π
2
cos θ
0 cos-1x π
q2 = π −α
q1 2 1
q1 = tan−1 y
x⎛⎝⎜
⎞⎠⎟ − β
q2 = −(π −α )
q1 = tan−1 y
x⎛⎝⎜
⎞⎠⎟ + β
3
• 2 3 = 2 1 2
4 → : Denavit-Hartenberg
2
q2 = π −α
q1 = tan−1 y
x⎛⎝⎜
⎞⎠⎟ + β
n Ø Ø
6 → 6
Q = q1,q2,q3,q4,q5,q6( )tP = x, y, z( )t
Θ = α,β,γ( )t
PΘ⎡
⎣⎢
⎤
⎦⎥ =
xyzαβγ
⎡
⎣
⎢⎢⎢⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥⎥⎥⎥
=
fx (q1,q2,q3,q4,q5,q6 )fy (q1,q2,q3,q4,q5,q6 )
fz (q1,q2,q3,q4,q5,q6 )fα (q1,q2,q3,q4,q5,q6 )fβ (q1,q2,q3,q4,q5,q6 )
fγ (q1,q2,q3,q4,q5,q6 )
⎡
⎣
⎢⎢⎢⎢⎢⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥⎥⎥⎥⎥⎥
= f(Q)
f* : Q
t
dxdt
= ∂fx∂q1
dq1dt
+ ∂fx∂q2
dq2dt
+ ∂fx∂q3
dq3dt
+ ∂fx∂q4
dq4dt
+ ∂fx∂q5
dq5dt
+ ∂fx∂q6
dq6dt
x )
f (q1,q2,q3,,qn )2
qi f qi
∂f∂qi
f (q1,q2 ) = q1 + 2q2 f (q1,q2 ) = q1 ⋅q2∂f∂q1
=
∂f∂q2
=
∂f∂q1
=
∂f∂q2
=
f (q1,q2 ) = cos(q1 + q2 )∂f∂q1
=
∂f∂q2
=
∂f∂q1
=
∂f∂q2
=
f (q1,q2 ) = sin(q1 + q2 )
x
q1 x
dxdt
= ∂fx∂q1
dq1dt
+ ∂fx∂q2
dq2dt
+ ∂fx∂q3
dq3dt
+ ∂fx∂q4
dq4dt
+ ∂fx∂q5
dq5dt
+ ∂fx∂q6
dq6dt
dPdtdΘdt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
=
dxdtdydtdzdtdαdtdβdtdγdt
⎡
⎣
⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥
=
∂fx∂q1
∂fx∂q2
∂fx∂q3
∂fx∂q4
∂fx∂q5
∂fx∂q6
∂fy∂q1
∂fy∂q2
∂fy∂q3
∂fy∂q4
∂fy∂q5
∂fy∂q6
∂fz∂q1
∂fz∂q2
∂fz∂q3
∂fz∂q4
∂fz∂q5
∂fz∂q6
∂fα∂q1
∂fα∂q2
∂fα∂q3
∂fα∂q4
∂fα∂q5
∂fα∂q6
∂fβ∂q1
∂fβ∂q2
∂fβ∂q3
∂fβ∂q4
∂fβ∂q5
∂fβ∂q6
∂fγ∂q1
∂fγ∂q2
∂fγ∂q3
∂fγ∂q4
∂fγ∂q5
∂fγ∂q6
⎡
⎣
⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥
dq1dtdq2dtdq3dtdq4dtdq5dtdq6dt
⎡
⎣
⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥
= J dQdt
→dPdtdΘdt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
= J dQdt
→
ΔPΔΘ⎡
⎣⎢
⎤
⎦⎥ = J ⋅ ΔQ
→
• (x,y)
dxdt
=
dydt
=
J=
q1=q2=0
q1,q2 10 ±20 ______
q1=0 q2=π/2
q1,q2 10 ±20 ______
x → x ( )
x y
( )
( q1, q2,qn ) ( x, y, z, α, β, γ )
dPdtdΘdt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
=
dxdtdydtdzdtdαdtdβdtdγdt
⎡
⎣
⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥
=
∂ fx∂q1
∂ fx∂q2
∂ fx∂q3
∂ fx∂qn
∂ fy∂q1
∂ fy∂q2
∂ fy∂q3
∂ fy∂qn
∂ fz∂q1
∂ fz∂q2
∂ fz∂q3
∂ fz∂qn
∂ fα∂q1
∂ fα∂q2
∂ fα∂q3
∂ fα∂qn
∂ fβ∂q1
∂ fβ∂q2
∂ fβ∂q3
∂ fβ∂qn
∂ fγ∂q1
∂ fγ∂q2
∂ fγ∂q3
∂ fγ∂qn
⎡
⎣
⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥
dq1dtdq2dtdq3dtdqndt
⎡
⎣
⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥
= J dQdt
n dQdt
?
) : n=7( 3 2 2)
1: ( ) > ( )
2: ( ) = ( )
3: ( ) < ( )
( ) ( )1
dPdtdΘdt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
= J dQdt
dQdt
= J−1dPdtdΘdt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
dQdt
= J−1dPdtdΘdt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
ΔQΔPΔΘ⎡
⎣⎢
⎤
⎦⎥
( ) ( )
: 2
dPdtdΘdt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
= J dQdt
dQdt
= (JTJ)JTdPdtdΘdt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
( ) ( )
: 2
dPdtdΘdt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
= J dQdt
dQdt
= JT (JJT )
dPdtdΘdt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
( 2 )
dxdtdydt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
=−l1s1 − l2s12 −l2s12l1c1 + l2c12 l2c12
⎡
⎣⎢⎢
⎤
⎦⎥⎥
dq1dtdq2dt
⎡
⎣
⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥
1? 2? 3?
J =J11 J12J21 J22
⎡
⎣⎢⎢
⎤
⎦⎥⎥ J −1 = 1
det(J )J22 −J12−J21 J11
⎡
⎣⎢⎢
⎤
⎦⎥⎥
det(J ) =J11 J12J21 J22
���
J
L1=200mm
L2=150mm
x=200,y=150, q1=0,q2=90
x=230,y=120
q1 = tan−1 y
x⎛⎝⎜
⎞⎠⎟ − cos
−1 l12 + x2 + y2 − l2
2
2l1 x2 + y2⎛
⎝⎜
⎞
⎠⎟
q2 = π − cos−1 l12 + l2
2 − x2 − y2
2l1l2
⎛⎝⎜
⎞⎠⎟
q1= -7.6417[deg] , q2= 85.4114[deg]
[rad]
[rad]
x=230, y=120
L1=200mm
L2=150mm
x=200,y=150, q1=0,q2=90
q1= -8.5944 [deg] , q2= 87.1352[deg]
J_inv
J −1 = 0.0000 0.0050−0.0067 −0.0050
⎡
⎣⎢
⎤
⎦⎥
x=230, y=120
q1q2
⎛
⎝⎜
⎞
⎠⎟ =
0π2
⎛
⎝
⎜⎜
⎞
⎠
⎟⎟+ J −1 30
−30⎛⎝⎜
⎞⎠⎟
[rad]
[rad]
dx=30, dy=-30
q1= -7.6417[deg] , q2= 85.4114[deg] q1= -8.5944[deg] , q2= 871352[deg]
( )
( )
det(J ) = l1l2s2 = l1l2 sinq2 = 0
q2 = nπ (n = 0,1, 2,)
q2=0q2=π
→ =0
→ → →
? ?
Recommended