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

知能システム論１（６）
逆運動学：手首自由度
運動学：速度、ャコビアン
２００７．５．１５
講義内容
１．はじめに
２．ベクトルの基礎
３．逆運動学(Kinematics)：手首自由度
４．動力学（Dynamics)
５．行列の演算と応用(Matrix)
６．軌道計算(Trajectory)
７．ロボットの制御(Control)
８．応用(Application)
ロボットマニピュレータの逆運動学
(Inverse Kinematics)
リンクの位置姿勢ー＞自由度変数
逆運動学 (Inverse Kinematics)
手首３自由度の求め方
基幹３自由度と手首３自由度を分離
それぞれ別個に低次元の代数方程式を解く
分離できない場合
高次元の代数方程式になり一般に解けない
数値解法に拠らざるを得ない
手首３自由度
３つの回転軸が１点で交わる場合、位置と姿勢の自由度を分
離できる
その交点（手首Pw）は次のように求められる。
θ5
Pw  Ph  lhxxh  lhy yh  lhzzh
θ1 ,θ2 ,θ3はPwを腕先端の位置
として求められる
θ6
Pw
Yh
θ4
Ph
Lhx.Lhy,Lhz＝一定
Zh
θ4,θ5 ,θ6はX5,Y5,Z5と
Xh,Yh,Zhとの関係から求められる
Xh
手先リンクの姿勢の関係式
x4  cos4  x3  sin 4  y3
y4   sin 4  x3  cos4  y3
z4  z3
x5  c5 x4  s5 z4
y5  y4
z5  s5 x4  c5 z4
xh  c6 x5  s6 y5
yh  s6 x5  c6 y5
zh  z5
Y4,Y5
Y4
θ5
θ6
Yh
Ph
X4
Pw
X5
Z5
Lh
Zh
Y3
Xh
θ4
Z3,Z4
X3
x5  c5 x4  s5 z4
y5  y4
z5  s5 x4  c5 z4
z5  z4  cos5
5   cos ( zh  z3 )
1
Θ4はZ5のX3,Y3方向成分から求まることが予想できる。
z5  s5 x4  c5z4  s5 (c4 x3  s4 y3 )  c5z3
z5  x3  s5c4
z5  y3  s5s4
zh  y3
4  tan
zh  x3
1
Θ6はXhのX5,Y5方向成分から求まる。
xh  c6 x5  s6 y5
xh  x5  cos6
xh  y5  sin 6
xh  y5
6  tan
xh  x5
1
演習問題
A4の用紙に書いて５月２８日までに Z
提出のこと
先端位置Pからθ１～θ３を求める式を導け。
 1 
2

P 1 
 1


2 
Θ３
Θ１
１
Y
のとき、θ1~θ3を求めよ。
１
Θ２
Ｐ
X
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(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
 j
( xi
yi
yi
zi )  ( z j  xi
z j  yi
z j  zi )
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
zj )
Aj1   Ai  ( x j
yj
z j )1( xi
 c j s j 0 s j  c j 0 0  1 0
A
Aj 1 j   s j c j 0 c j  s j 0  1 0 0
 j
0 0 0 0 0
 0 0 1 0

(  )  ( x j
 j
yj
0  1 0
z j )1 0 0( x j
0 0 0
yj
z j )1( xi
yi
zi )
yi
zi )

(  )  ( y j  x j
 j
 ( yj  xj
 x j  xi

0) y j  xi
 z j  xi
 xTj 
 T
0) y j ( xi
 zT 
 j
x j  yi
y j  yi
z j  yi
yi
zi )
x j  zi 

y j  zi 
z j  zi 
 (( x j  xi ) y j  ( y j  xi ) x j ( x j  yi ) y j  ( y j  yi ) x j ( x j  zi ) y j  ( y j  zi ) x j )
 ( xi  ( y j  x j ) yi  ( y j  x j ) yi  ( y j  x j ))
 ( z j  xi z j  yi z j  zi )
xi
の幾何学的導出
 j
d j
dxi
sin 
xi

pj
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
i
i   p jj
zi )  (i  xi i  yi i  zi )
j 1
θ3
1  z11
2  z11  y22
3  z11  y22  y33
P3
リンク３
リンク2
l3
l2
P
マニピュレータ先端部の速度
P  P1  l1z1  l2 z2  l3z3
P  P1  l1z1  l2 z2  l3z3
 l22  z2  l33  z3
θ2
Z2
P2
X2
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
６自由度マニピュレータの先端部の速度
P  P6  l6 z6
P6  P5  l5 z5

P2  P1  l1z1
時間微分
P  P6  6  (l6 z6 )
P6  P5  5  (l5 z5 )
P  P6  6  ( P  P6 )
P6  P5  5  ( P6  P5 )

P2  P1  1  (l1z1 )

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6  P5  ( p11  p22  p33  p44  p55 )  ( P6  P5 )

P2  P1  ( p11 )  ( P2  P1 )
6
P   pi  ( P  Pi )i
i 1
ヤコビアン（係数行列）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:ヤコビアン
例題のヤコビアン
1 
 P   z1  ( P  P1 ) y2  ( P  P2 ) y3  ( P  P3 )  
2 
 


z1
y2
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