Transcript （直交座標）制御系 - 知能システム学講座

知能システム論１（１２）
ロボットの腕の制御(力制御）
２００８．７．１
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(Kinematics)
４．動力学（Dynamics)
５．ロボットの腕の制御(Control)力制御
６．軌道計算(Trajectory)
７．行列の演算と応用(Matrix)
８．応用(Application)
作業システムの構築
P1
P2
z0
x0
ロボットで机に力Fを与えながら、
滑らかに移動する作業システム（制御系）
y0
直交座標系での力・位置組合せ制御
Z
位置
X
力
Y
z0
x0
y0
力と位置の混成(ハイブリッド）制御
（１）直交座標制御＋力ベクトル制御
（コンプライアンス制御）
直交座標での位置制御と力制御に必要なトルクを力学的に
算出し重ね合わせる。
（２）関節位置制御に基づく方法
直交座標での位置を関節座標に変換し、それを関節位置
サーボの目標値として与える。力を制御するために手先に
バネを取り付け、力が望ましい値になるように位置を調整
する。
位置制御に基づく力の設定
xr  x(t )
yr  0
F0  ks ( zE  z)
F0
zr  z E 
ks
F0 : 力の設定値
ばね常数 k
手先
s
zE : 環境側の位置
y
x
力の誤差積分項の付加
F0 : 力の設定値
Fs : 力センサによる観測値
xr  x(t )
yr  0
F0
zr  zE   k  ( F0  Fs )dt
ks
Fs  ks ( zE  zr )
手先
zr  k (F0  Fs )
Fs  ks zr
Fs
  k (Fo  Fs )  Fs  Cekkst  F0
ks
アーム
ばね常数 k
s
zE : 環境側の位置
関節位置制御による位置と力の制御
xr  x(t )
yr  0
F0
zr  zE   k  ( F0  Fs )dt
ks
角度検出器
０
xr (t )
yr (t )
zr (t )
k  dt
F0
r1
座
標
変
換
-
k p1
関節サーボ系
T1 関節１
－
kv1
 rn
d
dt
T1  A111
モータ
1
z0
Fs
x0
力センサ
フィードバック
y0
力と位置の混成(ハイブリッド）制御
（１）直交座標制御＋力ベクトル制御
（コンプライアンス制御）
直交座標での位置制御と力制御に必要なトルクを力学的に
算出し重ね合わせる。
（２）関節位置制御に基づく方法
直交座標での位置を関節座標に変換し、それを関節位置
サーボの目標値として与える。力を制御するために手先に
バネを取り付け、力が望ましい値になるように位置を調整
する。
力と力のモーメント
第 i 関節にかかる力のモーメント:
M  ( P  Pi )  F
第 i 関節の回転力 Ti ：
Ti  pi  ( M  ( P  Pi )  F )
 pi  M  ( pi  ( P  Pi ))  F

P  J
F
M
P
T1   ( p1  ( P  P1 ))
:
:

 
T6  ( p6  ( P  P6 ))T
T
TF
T J  
M 
p 
 F 
:  
T M 
p6 
P-Pi
Zi
T
1
J
T
Pi
Yi=pi
Z0
X０
P1
Y0
力ベクトルの生成
F
T1 
T   :   J T F
Tn 
z0
１）各関節をロックした状態で先端に力Fを与えると、
関節トルク－Tが生じる
x0
y0
２）先端を固定した状態で関節トルクTを与えると、
先端で力Fを外界に与える
３）先端を自由にして関節トルクTを与えると、
関節トルクをゼロにして先端にFを与えたときと同じ運動が生じる
部分拘束運動（Partially constrained motion)
人工拘束面
コンプライアンス
物体
Z
X
Y
自然拘束面
人工拘束とコンプライアンス
コンプライアンス(Compliance)：相手に合わせて変化する性質、ばね、一定力
コンプライアンス制御(直交座標サーボ制御）
ばね特性
ダンパー特性
Fx  ks ( xr  x)  kd (x)
Fy  ks ( yr  y)  kd ( y )
Fz  Fc
一定力
バネ特性やダンパー特性のゲインを大きくすると
人工拘束がかけられる
直交座標サーボ制御の特性
ばね特性
ダンパー特性
Fx  ks ( xr  x)  kd ( x)
腕先端における等価慣性をM xとすると、
Fx  M x x  ks ( xr  x)  kd ( x)
M x x  kd x  ks x  ks xr
これを解くと次のよう になる。
x  C1e
kd  kd2 4 M x k s
t
2M x
 C2e
kd  kd2 4 M x k s
t
2M x
 xr
パラメータ (ks , kd ) を適切に設定することで
x  xr
となる。
x  C1e
x
kd  kd2 4 M x ks
t
2M x
kd2  4M xks  0
k s , kd  大
 C2e
kd  kd2 4 M x ks
t
2M x
 xr
臨界減衰
kd2  4M xks  0
kd2  4M xks  0
xr
時間t
Ｘの応答
コンプライアンス（直交座標）制御系
目標値
 xr 
y 
 r
 *  e
0
0
 
 F0 
＋
＋
Kse  Kd e
＋
F

T
JT
ロボットの腕
－
ksx 0 0
Ks   0 ksy 0
 0 0 0
kdx 0 0
Kd   0 kdy 0
 0 0 0
X 
 Y   F ()
 
 Z 
座標変換：
順運動学
X 
Y 
 
 Z 
作業システムの構築
100cm
P1
100cm
P2
10cm
演習：図のロボットで机に力Fを与えながら、
x0
P1 から P2 まで滑らかに移動する作業
システム（制御系）を構築せよ。ただし、
 0 
50cm
100cm
F   0 , P1   0 , P2   0  とする。
 0.1kg
50cm
 50cm 
z0
y0