知能システム論１（７）
運動学(Kinematics)
速度・加速度
２００８．６．３
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(Kinematics)：速度・加速度
４．動力学（Dynamics)
５．行列の演算と応用(Matrix)
６．軌道計算(Trajectory)
７．ロボットの制御(Control)
８．応用(Application)
練習問題
z
図のマニピュレータのPに関する（姿勢に関しては不要）
ヤコビアンを求めよ。
θ3
(1) (xi,yi,zi)を求める(i=1~3)。
y3
z3
x3
1
1
(2) Pi、Pを求める。
P
z1,z2
(3) ヤコビアンの式に上を代入する。
y1,y2
θ2
θ1
x
x1,x2
θ1=45度
θ2=0度
θ3=90度
y
ヤコビアン（係数行列）Jacobian
6
P   pi  ( P  Pi )i
マニピュレータ先端の並進速度
i 1
6
  6   p jj
マニピュレータ先端リンクの回転速度
j 1
上式を１つにまとめると次のようになる
1 
 P   p1  ( P  P1 )    p6  ( P  P6 )  

 


p1

p6
  
  
6

先端の速度
J:ヤコビアン
例題のヤコビアン
 P   z1  ( P  P1 )
 
z1
  
y2  ( P  P2 )
y2
1 
y3  ( P  P3 )   
2 


y3
  
 3
自由度変数の
変化速度
練習問題の解
図のマニピュレータのPに関するヤコビアンを求めよ。ただしθ1=45度、
θ 2=0、 θ3=90度、 l1  0, l2 l 3 1 とせよ。
(1)
( x1
( x3
(xi,yi,zi)を求める(i=1~3)。
y1
z1 )  ( x2
y3
y2
1
0 
2

0 1
 
2
1  0

c1  s1
z2 )   s1 c1

0
 0
1
0
2 
1
0
2

0
1

z3 ) 
 c3
( x2 y 2 z 2 )  0

 s3
0
 ( x2 y 2 z 2 )  0

 1
θ3
P3
リンク2
リンク３
0 s3 
1 0

P
0 c3 
 0 1
0 1
2


1 0 0 1

2

0 0
0
 1

l3
l2
Z2

2
1 
2
0 

1
P2
X2
(2) Pi、Pを求める。
1 
0
2



P1  P2  0, P3  l2 z2  0 , P  P3  l3 z3   1 
 
2

1
 1 
θ2
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
問題のマニピュレータのヤコビアンは次のとおり。
P  z1  ( P  P1 )
ヤコビアン J
y2  ( P  P2 )
 1   1 
0 
2 
2
z1  ( P  P1 )  0   1    1 
  
2 
2
1  1   0 

 

 1   1   1 
2 
2 
2

y2  ( P  P2 )   1    1    1 
2 
2 
2

 0   1   1 

 
 

 1   1 
2 
2  0 

y3  ( P  P3 )   1    1    0 
2 
2  

 0   0   1

 

1 

y3  ( P  P3 ) 2 
3 
 
 1
2

J  1
2

 0

1
2
1
2
1
0

0

 1

p
速度の関係式(幾何学的導出）
6
P   pi  ( P  Pi )i
i 1
 P   p1  ( P  P1 )   
 
p1

  

6
  6   pii
i 1
1 
p6  ( P  P6 )  
:


p6
  
 6
r


| r | sin 
V  p  r
P
P
P-Pi
Pi
同じ関係式は微小変位に関しても成り立つ
 1 
 P 
 : 

J
 


 
  6 
pi
p
n
r


V  ( r sin  ) n
| r | sin 
V  p  r
 p r sin   n
 r sin   n
先端の速度から関節角速度を求める
1 
 P 
 
   J : 
 
6 
 
 p1  ( P  P1 )   
J 
p1


1 

 
1  P 
 :   J  
 
6 
 
p6  ( P  P6 )

p6

加速度の算出
θ3
マニピュレータ先端部の加速度
P  P0  l1 z1  l2 z2  l3 z3
P  P  l z  l z  l z
0
1 1
2 2
P3
リンク３
3 3
 l2 2  z2  l33  z3
P

d
P
 
P
 l2 2  z2  l2 2  z2
dt
 l3 3  z3  l33  z3
 l2 2  z2  l2 2  ( 2  z2 )
 l3 3  z3  l33  (3  z3 )
リンク2
l3
l2
θ2
Z2
P2
X2
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
回転加速度ベクトル
i
i  
j 1
p 
j
i
 i   ( p jj  p jj )
j 1
j
i
  ( p jj   j  p j j )
j 1
前図マニピュレータの回転加速度ベクトル
1  z11  1  z11  z11
 2  z11  y22  2  y22
 3  z11  y22  y33  2  y22  3  y33
1  z11
2  z11  y22
3  z11  y22  y33
６自由度マニピュレータの加速度
P  P    ( P  P )
6
6
6
P6  P5  5  ( P6  P5 )

P2  P1  1  ( P2  P1 )
時間微分
  P
    ( P  P )    ( P  P )
P
6
6
6
6
6
  P
    ( P  P )    ( P  P )
P
6
5
5
6
5
5
6
5

  P
    ( P  P )    ( P  P )
P
2
1
1
2
1
1
2
1
加速度のヤコビアンを用いた表現
1 
 P 
 
   J : 
 
6 
 
 p1  ( P  P1 )   
J 
p1


1 
1 

P
   
   J :  J : 
 
6 
6 
 
 
p6  ( P  P6 )

p6

  P )   


p

(
P

P
)

p

(
P
1
1
1
J   1
p1


p 6  ( P  P6 )  p6  ( P  P6 )

p 6

応用
直動自由度を含む場合
長さをあらわすパラメータを時間の関数として扱う。
P  sx1
1  1 z1
P  sx1  sx1
1  1 z1

  sx  sx  sx  sx
P
1
1
1
1
 sx1  2 s(1  x1 )  s1  (1  x1 )  s1  x1
x1 
d
d
( x1 )  (1  x1 )  1  x1  1  (1  x1 )
dt
dt
z1
s
x1
1
y1
Ｐ
P  sx  2 s ( z  x )  s 2 z  ( z  x )  s z  x
1
1 1
1
1 1
1
1
1 1
1
2
 sx  2 s y  s z  ( y )  s y
1
1 1
1
1
1
1 1
2
2










 sx1  2 s1 y1  s1 (  x1 )  s1 y1  ( s  s1 ) x1  (2 s1  s1 ) y1
練習問題
1  0,  2  0,  3   / 2,
1  1rad / sec, 2  0, 3  0
θ3
P3
のとき、マニピュレータ先端部の
加速度ベクトルをもとめよ。 リンク３
リンク2
l3
l2
P
θ2
Z2
P2
X2
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
姿勢の算出
x1  cos1  x0  sin 1  y0
y1   sin 1  x0  cos1  yo
A1
x1
y1
z1   x0
z1  z0
y0
単位行列
x2  c2 x1  s2 z1
y2  y1
x2
z2  s2 x1  c2 z1
y2
z2   x1
y1
x3  c3 x2  s3 z2
y3  y2
x3
z3  s3 x2  c3 z2
x3
c1
z0  s1

 0
y3
z3   x0
y0
y3
z3   x2
z0 A1 A2 A3  A1 A2 A3
y2
xi
 s1 0
c1 0

0 1
A2
 c2
z1  0

 s2
0 s2 
1 0

0 c2 
A3  c3
0 s3 
1 0

0 c3 
z2  0

 s3
yi
zi   A1    Ai
姿勢の計算 (
1
x1
x3
y1
y3
 0,2  0,3   / 2radのとき )
1 0 0
z1   0 1 0
0 0 1
z3   x2
y2
x2
y2
z 2   x1
y1
1 0 0   1 0 0 
z1 0 1 0  0 1 0
0 0 1 0 0 1
 0 0 1  0 0 1
z 2  0 1 0   0 1 0
 1 0 0  1 0 0
回転速度・加速度の計算
0 
0 
1  z11  0 1  0
1
1
0 
2  z11  y22  0
1
0 
3  z11  y22  y33  0
1
(1  1rad / s,2  0,3  0,1  0,2  0,3  0のとき )
1  z11  1  z11  z11  0
 2  z11  y22  2  y22  0
 3  z11  y22  y33  2  y22  3  y33  0

第 回 ランチョンセミナー登壇者用雛形

Document 7375895