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

生産情報システム学（１３）
ロボットの制御（その２）
応用
２００５．７．１９
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(Kinematics)
４．動力学（Dynamics)
５．行列の演算と応用(Matrix)
６．軌道計算(Trajectory)
７．ロボットの制御(Control)
８．応用(Application)
サーボシステム
角度目標値 θｒ
＋
Ｋ
ー
慣性モーメント Ｉ
モータトルク
Ｔ
角度検出器 θ
T  K ( r   )
T  I
θｒ
Ｋ
1
s2
I  K  K r
   r  C1 sin
K
K
t  C2 cos
t
I
I
K
2
K
 ( s)  s  r ( s)  2
 r ( s)
K
s

K
1 2
s
θ
I  K (1  1)  0の解
K
  1 cos
t
I

１
r
時間t
サーボ系の応答
関節位置制御による先端部軌道制御
T1  k p1 ( r1  1 )  kv11  A111
A111  kv11  k p1 (1   r1 )  0
Pr (t )
関節サーボ系
r1
Pr (t )
座
標
変
換
k p1
-
T1 関節１
－
k v1
 rn
座標変換：逆運動学
d
dt
1
z0
T1  A111
T1  A11
x0
1 
 
... A1n  :   N1
n 
 
y0
A111  kv11  k p1 (1   r1 )  0の解
1  C1e
1
t
kv21  4 A11k p1  0
 C2e
 t
  r1
,  
 kv1  kv21  4 A11k p1
2 A11
臨界減衰
kv21  4 A11k p1  0
kv21  4 A11k p1  0
r1
時間t
サーボ系
制御誤差
A111  A122  ..  A1nn  N1  kv11  k p1 (1   r1 )  0の定常解
1
1   r1 
( A122  ..  A1nn  N1 )
k p1
1
kv21  4 A11k p1  0
臨界減衰
kv21  4 A11k p1  0
r1
kv21  4 A11k p1  0
時間t
ロボットのトルク計算制御：
T
関節制御

  N (, 
)
T  A

T (1)  AT ( 2 )  A

T (2)  
フィードバック補償
ゲイン補償
)
N (, 
T
(1)
T
  N
T  T (1)  N  A

T (1)  A

  N (, 
)
T  A
T ( 2)
T (1)
A
T
( 1)

 A

フィードバック制御+オープンループ制御
d2
r
e
ー
dt 2
k p e  kv e


r
  

T ( 2 )  k p e  kv e  
r
T ( 2)

T (2)  

e  r  
e  kv e  k p e  0
直交座標系での力・位置組合せ制御
Z
位置
X
力
Y
z0
x0
y0
ロボットのトルク計算制御： 直交座標制御

J
T ( 3)
ー
J 1
T ( 2)

T (2)  

X 
 Y   F ()
 
 Z 
)T 

 J
 
  0
 J 1T ( 3)  J 1 J
1
J (T
( 3)
( 2)
  J
  0
 T ( 3)  J
T ( 3)
 X 
  J
   Y 
 J
 
 Z 
 
X 
Y 
 
 Z 
 X 
 

 Y   J
 Z 
 
 X 
  
  
 Y   J  J
 Z 
 
Xr
Y 
 r
 Z r 
d
e
dt 2
K p e  K v e
－
T ( 3)
2
T
 X r   X 
   
 K p e  K v e   Yr    Y 
 Zr   Z 
   
k px

Kp   0
 0
0
k py
0
0

0
k pz 
e  K v e  K p e  0
kvx
Kv   0

 0
( 3)
T ( 3)
 X 
 
  Y 
 Z 
 
X 
Y 
 
 Z 
 ex   X r  X 
e  e y    Yr  Y 
  

 ez   Z r  Z 
0
kvy
0
0
0

kvz 
ex  kvx ex  k pxex  0
ey  kvy e y  k py e y  0
ez  kvz ez  k pzez  0
ex  kv ex  k pex  0の解
ex  C1e t  C2e  t
1
０
kv2  4k p  0
,  
 kv  kv2  4k p
2
臨界減衰
kv2  4k p  0
k  4k p  0
2
v
時間t
力ベクトルの生成
F
 T1 
T   :   JTF
 
Tn 
z0
１）各関節をロックした状態で先端に力Fを与えると、
関節トルク－Tが生じる
x0
２）先端を固定した状態で関節トルクTを与えると、
先端で力Fを外界に与える
３）先端を自由にして関節トルクTを与えると、
関節トルクをゼロにして先端にFを与えたときと同じ運動が生じる
y0
部分拘束運動（Partially constrained motion)
人工拘束面
コンプライアンス
物体
Z
X
Y
拘束面
人工拘束とコンプライアンス
コンプライアンス(Compliance)：相手に合わせて変化する性質、ばね、一定力
コンプライアンス制御
F  K s ( Z r  Z )  K d (  Z )  Fc
ばね特性
ダンパー特性
*
*
 
 Z r 
目標値
Xr
Y 
 r
 * 
X 
 Y   F ()
 
 Z 
X 
Y 
 
 Z 
d
dt
一定力
0 : 0 
... : ... 


 0 : kd 
－
0 : 0 
... : ... 


 0 : k s 
－
F
J
0
Fc   0 
 
 f c 
T
T
マニピュレータ
位置制御
コントローラ

作業システムの構築
100cm
P1
100cm
P2
10cm
設問：図のロボットで机に力Fを与えながら、
x0
P1 から P2 まで滑らかに移動する作業
システム（制御系）を構築せよ。ただし、
 0 
50cm 
100cm 
F   0 , P1   0 , P2   0  とする。






 0.1kg
50cm 
 50cm 
z0
y0
滑らかな軌道の設計
Z
S
X,Y,Zごとの運動を時間の多項式で表す。
・指定時刻に指定の位置を通る。
・位置、速度、加速度が連続となる。
・始点終点で速度が指定値となる。
G
X
Y
X
G
S
時間
例：始点（X=50）が時刻0、終点（X=100）が時刻1となるような軌道の
設計。ただし位置、速度、加速度が連続で、
始点終点で速度と加速度が0となること。
条件式が6個
100
50
1
時間
5次の多項式
5次の多項式
2
3
4
5
[位置] x (t )  a0  a1t  a2t  a3t  a4t  a5t
[速度]
[加速度]
x (t )  a1  2a2t  3a3t 2  4a4t 3  5a5t 4
x(t )  2a2  6a3t  12a4t 2  20a5t 3
x (0)  50, x (0)  x(0)  0
x (1)  100, x (1)  x(1)  0
a0  50, a1  a2  0
a3  500, a4  750, a5  300
X 100
x(t )  50  500t 3  750t 4  300t 5 75
一般にはスプライン関数を用いて
軌道を表現する。
50
0.5
1 時間
y
yj
y j 1
xj
x
x j 1
1
1
y  Ay j  By j 1  ( A3  A)( x j 1  x j )2 y j  ( B 3  B )( x j 1  x j ) 2 y j 1
6
6
x  x jの時、 A  1, B  0
y  yj
x  x j 1の時、 A  0, B  1
y  y j 1
y j 1  y j
3 A2  1
3B 2  1
y 

( x j 1  x j ) y j 
( x j 1  x j ) y j 1
x j 1  x j
6
6
y  Ay j  By j 1
y j 1
y j
xj
x j 1
y
100
50
0
1
x
1
1
y  Ay j  By j 1  ( A3  A)( x j 1  x j ) 2 y j  ( B 3  B )( x j 1  x j ) 2 y j 1
6
6
1
1
y  A50  B100  ( A3  A) y j  ( B 3  B ) y j 1
6
6
100  50 3 A2  1
3B 2  1
y1 
y2
y 

1
6
6
x  0、１の時、y  0の条件から、
0  50  y1 / 3  y2 / 6  50  y1 / 6  y2 / 3
上の式を解いて、y1  300, y2  300
1 3
1 3
y  A50  B100  ( A  A)300  ( B  B )300
6
6
A  1  t , B  tであるから、
y  50(1  t )  100(t )  50((1  t )3  (1  t ))  50(t 3  t )
y  50  150t 2  100t 3
100
50
1
力と位置の混成(ハイブリッド）制御
（１）直交座標制御＋力ベクトル制御
直交座標での位置制御と力制御に必要なトルクを力学的に
算出し重ね合わせる。
（２）関節位置制御に基づく方法
直交座標での位置を関節座標に変換し、それを関節位置
サーボの目標値として与える。力を制御するために手先に
バネを取り付け、力が望ましい値になるように位置を調整
する。
（１）直交座標制御＋力ベクトル制御

T (2)  
T ( 3)

J
 X 
 
  Y 
 Z 
 
)
B(, 
T ( 3)
＊
F
ー
J
0
0
 
 F0 
Xr
Y 
目標値  r 
 * 
1
T ( 2)
A
T
(1)
T
  B(, 
)
T  A
k px
Kp   0

 0
JT
d2
e
ー

0
k py
0
0
kvx
0 Kv   0


0
 0
dt 2
K p e  K v e
X 
 Y   F ()
 
 Z 
T ( 3)
＊
0
kvy
0
0
0

0
X 
Y 
 
 Z 
（２）関節位置制御に基づく方法
 x (t ) 
 y (t )


 z (t ) 
 xr ( t ) 
 y (t )
 r 
 zr (t ) 
r1
座
標
変
換
-
d
k p1  k v 1
dt
関節１
1
z0
x0
r3
座標変換：逆運動学
y0
関節位置制御に基づく力の設定
xr  x ( t )
yr  0
F0
zr  z E 
ks
F0 : 力の設定値
手先
ばね常数 k
s
z E : 環境側の位置
生産情報システム論レポート課題
設問：図のこれまで例題とし 100cm
てもちいてきたロボットで机に
力Foを与えながら、P1 から P2
Fo
P1
まで滑らかに移動
する作業シス
P2
テム（制御系）
を構築せよ。
ただし、
10cm
 0 
50cm
100cm
F0   0 , P1   0 , P2   0  とする。 x






0
 0.1kg
50cm
 50cm 
100cm
z0
y0
力と位置の混成(ハイブリッド）制御の２つの方法の中から１つ選び、ブロック線図を
作成せよ。入力や各ブロックの処理内容を数式を用いて説明せよ。関節位置制御に
基づく方法を選択した場合はそれらのサーボ系を詳しく述べる必要はない。
(提出期限：8月27日(金）、提出先：P棟4階U専攻事務室前ポスト）