Transcript ロボティクスーその来し方行く末

知能システム論１（６）
逆運動学：手首自由度
運動学：速度、ャコビアン
２００８．５．２７
講義内容
１．はじめに
２．ベクトルの基礎
３．逆運動学(Kinematics)：手首自由度
４．動力学（Dynamics)
５．行列の演算と応用(Matrix)
６．軌道計算(Trajectory)
７．ロボットの制御(Control)
８．応用(Application)
ロボットマニピュレータの逆運動学
(Inverse Kinematics)
リンクの位置姿勢ー＞自由度変数
逆運動学 (Inverse Kinematics)
手首３自由度の求め方
基幹３自由度と手首３自由度を分離
それぞれ別個に低次元の代数方程式を解く
分離できない場合
高次元の代数方程式になり一般に解けない
数値解法に拠らざるを得ない
手首３自由度
３つの回転軸が１点で交わる場合、位置と姿勢の自由度を分
離できる（手先リンクの位置姿勢を固定した状態でθ４ ,θ５,
θ６を変化させてもその交点位置Ｐｗは動かない）
その交点（手首Pw）は次のように求められる。
Pw  Ph  l hx x h  l hy y h  l hz z h
θ1 ,θ2 ,θ3はPwを腕先端の位置
として求められる
θ6
θ5
Pw
Yh
θ4
Ph
Lhx,Lhy,Lhz＝一定
Zh
θ4,θ5 ,θ6はX3,Y3,Z3と
Xh,Yh,Zhとの関係から求められる
Xh
手先リンクの姿勢の関係式
x 4  cos  4  x 3  sin  4  y 3
y 4   sin  4  x 3  cos  4  y 3
Y4,Y5
z4  z3
Y4
θ5
Y3
x 5  c5 x 4  s5 z 4
θ6
y5  y4
z 5  s5 x 4  c5 z 4
Yh
Ph
X4
Pw
X5
Z5
Lh
x h  c6 x5  s6 y 5
y h   s6 x5  c6 y 5
zh  z5
Zh
Xh
θ4
Z3,Z4
X3
x 5  c5 x 4  s5 z 4
y5  y4
z 5  s5 x 4  c5 z 4
z 5  z 4  cos  5
1
 5   cos
( zh  z3 )
Θ4はZ5のX3,Y3方向成分から求まることが予想できる。
z 5  s5 x 4  c5 z 4  s5 ( c 4 x 3  s 4 y 3 )  c5 z 3
z 5  x 3  s5 c 4
 4  tan
z 5  y 3  s5 s 4
1
zh  y3
zh  x 3
Θ6はXhのX5,Y5方向成分から求まる。
x h  c6 x5  s6 y 5
x h  x 5  cos  6
x h  y 5  sin  6
 6  tan
1
xh  y5
x h  x5
演習問題
A4の用紙に書いて６月１０日までに Z
提出のこと
先端位置Pからθ１～θ３を求める式を導け。
順運動学は次ページに示す。
次に、
Ｚ１
Θ３
１
Ｚ２
0 
 
P  1
 
 1 
Ｚ３
Ｘ３
Ｙ１
Ｘ１
Ｙ２
Ｘ２
１
Θ１
Ｙ３
Y
Θ２
のとき、θ1~θ3を求めよ。
Ｐ
X
本図はθ１～θ３＝０の場合を示している
x1  c1 x 0  s1 y 0
y 1   s1 x 0  c1 y 0
z1  z 0
x2  x1
y 2  c 2 y 1  s 2 z1
ここで、
1 
0 
0 
 
 
 
x0  0 , y0  1 , z0  0
 
 
 
 0 
 0 
 1 
である。
z 2   s 2 y 1  c 2 z1
x3  c3 x 2  s3 y 2
y3   s3 x 2  c3 y 2
z3  z2
P  x1  x 3
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(Kinematics)：速度
４．動力学（Dynamics)
５．行列の演算と応用(Matrix)
６．軌道計算(Trajectory)
７．ロボットの制御(Control)
８．応用(Application)
ロボットマニピュレータの運動学(Kinematics)
速度と加速度の導出
位置姿勢表現の基本となるのはリンクiに固定した
３軸方向単位ベクトル(xi,yi,zi)
まず、(xi,yi,zi)の時間微分を求める。
dx i
dt
i


j 1
xi 
j 
 j

(
p

x
)

 j i j
ここでpjは第j関節の回転軸方向単位ベクトル
dx i
dt
i
 (  p j j )  x i   i  x i
j 1
ωi：第iリンクの回転速度ベクトル
３軸方向単位ベクトルの自由度変数による偏微分
Θj：第j自由度（ｚ軸まわりの回転）の回転角


( xi

 j
( xi
yi
z j  zi )
j
(   )  A1   A j 1

 1  A j 

A j  1   Ai  A1   A j  A j
 A j  1   Ai
 j
 j 



A j
A1   A j  ( x j
1
Aj
 j
z j  yi
z i )  A1 A2    Ai
yi

zi )  ( z j  xi
yj
 cj
A j

  sj

 j
 0
(   )  ( x j
yj
A j  1   Ai  ( x j
zj)
sj
cj
0
0

zj) 1

 0
0  s j

0
c
 j
1   0
1
0
0
 cj
 sj
0
0

0 (xj

0 
1
0 0
 
0  1
 
0   0
yj
0
0
1
z j ) ( xi
yj
1
z j ) ( xi
0

0

0 
yi
zi )
yi
zi )

 j
 (yj
(   )  ( y j
 xj
 xj
 x j  xi

0 ) y j  xi

 z j  xi

 ( ( x j  xi ) y j  ( y j  xi ) x j
 ( xi  ( y j  x j )
 ( z j  xi
 x Tj 


T
0 ) y j ( xi
 T 
 zj 
x j  yi
y j  yi
z j  yi
zi )
x j  zi 

y j  zi

z j  z i 
( x j  yi ) y j  ( y j  yi ) x j
yi  ( y j  x j )
z j  yi
yi
z j  zi )
y i  ( y j  x j ))
( x j  zi ) y j  ( y j  zi ) x j )
xi
 j
の幾何学的導出
d
sin 
dx i
xi
j
pj

n
dx i  (sin  )( d  j ) n
 ( p j  xi )d 
xi
 j

dx i
d j

j
( p j  xi ) d  j
d j
 p j  xi
速度の算出
姿勢を表す単位ベクトルの変化速度
i
( x i
y i
zi )  ( i  x i
 i  yi
i 
 i  zi )

j 1
θ3
 1  z11
P3
 2  z11  y 22
リンク３
 3  z11  y 22  y 33
リンク2
l3
l2
P
マニピュレータ先端部の速度
θ2
Z2
P2
P  P1  l1 z1  l 2 z 2  l 3 z 3
X2
P  P1  l1 z1  l 2 z 2  l 3 z 3
 l 2 2  z 2  l 3 3  z 3
p j j
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
６自由度マニピュレータの先端部の速度
P  P6  l 6 z 6
P  P6   6  ( l 6 z 6 )
P  P6   6  ( P  P6 )
P6  P5  l 5 z 5 時間微分 P  P    ( l z )
6
5
5
5 5
P6  P5   5  ( P6  P5 )



P2  P1  l1 z 1
P2  P1   1  ( l1 z1 )
P2  P1   1  ( P2  P1 )
θiに関する式の形は次のようになる
P  P6  ( p 11  p 22  p 33  p 44  p 55  p 66 )  ( P  P6 )
P6  P5  ( p 11  p 22  p 33  p 44  p 55 )  ( P6  P5 )

P2  P1  ( p 11 )  ( P2  P1 )
P 
6

i 1
p i  ( P  Pi )i
ヤコビアン（係数行列）Jacobian
P 
6

p i  ( P  Pi )i
マニピュレータ先端の並進速度
i 1
6
  6 

p j j
マニピュレータ先端リンクの回転速度
j 1
上式を１つにまとめると次のようになる
 P   p 1  ( P  P1 )
  
p1
  
 1 
p 6  ( P  P6 )   
  
p6
  

 6


先端の速度
例題のヤコビアン
 P   z 1  ( P  P1 )
  
z1
  
J:ヤコビアン
y 2  ( P  P2 )
y2
 1 
y 3  ( P  P3 )   


 2
y3
  

 3
自由度変数の
変化速度
練習問題
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
1
 1
2

 x1 y 1 z 1    1

2
 0

 0

x3 y 3 z 3    0

 1

0

0,

1

2
1
2
0
1
2
1
2
0

2
1
,
2
0 

1
 x 2 y 2 z 2    x1 y1 z1 ,
0 
 
P1  P2  0 ,
 
 0 
 0
 x 3 y 3 z 3    x 2 y 2 z 2  0
  1
 1

2


P   1

2
 1 


0 
 
P3  0 ,
 
 1 
 1
2

J   z 1  ( P  P1 ) y 2  ( P  P2 ) y 3  ( P  P3 )    1

2
 0

1
2
1
2
1
0 

0 

 1

0
1
0
1

0 ,

0 