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

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

Download Report