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