JPH0683976B2 - コンプライアンス制御方法 - Google Patents

Description

【発明の詳細な説明】 A.産業上の利用分野 本発明はコンプライアンス制御方法に関し、より具体的
には力センサからのフィードバックより産業用マニピュ
レータの手先等の制御対象物に任意の硬さ・減衰等を実
現する制御手法に関する。

B.従来技術およびその問題点 マニピュレータを使って組立てなどの作業をする場合、
何らかの手段によって手先の力あるいは硬さを調整する
ことが必要である。つまり、従来の産業用マニピュレー
タのように位置制御しか行なわれていないためにただ単
に位置剛性が高く作業対象物からの反力を無視して動作
をするものでは、挿入等の組立作業を行わせる場合に作
業対象物にならわせて動作することができない。それば
かりか位置の偏差が大きいときは作業対象物を破損する
ことになる。通常このような物同志の接触の際の緩衝の
ために手先にばね等を取り付け、手先の硬さを調整して
いる。この手先の硬さの調整をばね等の機構によらず制
御によってあたかも手先にばね等があるかのようにマニ
ピュレータを動かし外部環境とのマッチングを計るもの
をコンプライアンス制御と呼んでいる。

さて力学系における力の釣合においてはただ単にばねの
硬さによる力だけでなくダンパによる減衰力や慣性力お
よび力源による力等があり、外部環境とマッチングをと
るためにはこれらのパラメータを考慮することが必要で
ある。ここでは外部環境とのマッチングのためのコンプ
ライアンスを第２図および式（１）に示すような二次の
運動方程式で表す。この運動は一般的にはあるコンプラ
イアンスを考える直交座標系（Ｃ）において各座標軸に
ついて並進及び回転が独立に扱われる。すなわち Ｍ（−_ｒ）＋Ｋ_ｄ（−_ｒ）＋ Ｋ_ｓ（ｐ−ｐ_ｒ）＝Ｋ_ｆ（ｆ_ｒ−ｆ） （１） C ^ｃM C^Ｔ（−_ｒ）＋ C ^ｃＫ_ｄ C^Ｔ（−_ｒ） ＋C ^ｃＫ_ｓ C^Ｔ（ｐ−ｐ_ｒ） ＝C ^ｃＫ_ｆ C^Ｔ（ｆ_ｒ−ｆ） ｐ_ｒ：ハンドの位置の目標値（６ ベクトル） p:ハンドの位置（６ ベクトル） ｆ_ｒ：ハンドにかかる力の目標値（６ ベクトル） f:ハンドにかかる力（６ ベクトル） M:世界座標系における仮想質量・慣性（６×６ 行列） ｋ_ｄ：世界座標系における減衰係数（６×６ 行列） ｋ_ｓ：世界座標系におけるばね係数（６×６ 行列） Ｋ_ｆ：世界座標系におけるコンプライアンス選択行列
（６×６ 行列） C:コンプライアンス座標系^ｃ M:コンプライアンス座標系における仮想質量・慣性
（６×６ 対角行列）^ｃ Ｋ_ｄ：コンプライアンス座標系における減衰係数（６
×６ 対角行列）^ｃ Ｋ_ｓ：コンプライアンス座標系におけるばね係数（６
×６ 対角行列）^ｃ Ｋ_ｆ：コンプライアンス座標系におけるコンプライア
ンス選択行列 （６×６ 対角要素が１または０の対角行列） ここでコンプライアンス選択行列（^ｃＫ_ｆ）とはコンプ
ライアンス座標系において力を考慮するか（１）しない
か（０）を選択するためのスイッチである。これは軸ご
とにみれば車等にも見られる通常のサスペンションであ
る。つまりコンプライアンス制御の目的は、第２図に示
すように、あたかも手先にばねやダンパ、仮想質量、力
源からなるサスペンション機構があるかのようにマニピ
ュレータを制御すること、さらにいえば運動方程式
（１）を満足するようにマニピュレータを制御すること
といえる。

さて、従来のコンプライアンス制御はこれを簡便にし単
にばねのみがダンパのみしか扱わないものが主流であっ
た。

またこのコンプライアンス制御を、力センサにより外力
を検出しマニピュレータを速度制御することにより行な
う方法が、下記文献に記載されている。

杉本浩一著「ロボットアームの力フィードバック制御」
計測と制御第25巻第１号、1986年１月 上記文献によれば、検出した力に関する量を含む、世界
座標系における運動方程式（１）を積分して速度を求
め、これをヤコビアンによりジョイント座標系における
速度に変換し、あとは通常の速度制御を行うものであ
る。式（１）においてＫ_ｆを単位行列とし、速度、加速
度および力の目標値（_ｒ，_ｒ,f_ｒ）を０とし、整理
すれば、 Ｍ＝−Ｋ_ｄ＋Ｋ_ｓ（ｐ_ｒ−ｐ）−ｆ （２） この式の両辺にＭの逆行列を掛け、積分すれば次式を得
る。

＝M^-1∫｛＝Ｋ_ｄ＋Ｋ_ｓ（ｐ_ｒ−ｐ）−ｆ｝dt
（３） これを差分の形で表せば、_ｎ ＝TM^-1｛Ｋ_ｓ（ｐ_ｒ−ｐ）−ｆ｝＋ （Ｉ−TM^-1K_ｄ）_ｎ−１ （４） T:サンプリング時間 この速度（_ｎ）をヤコビアン行列によりジョイント座
標系に変換すれば、ジョイントの目標角速度（_ｎ）を
得る。_ｎ ＝J^-1 _ｎ ＝J^-1［TM^-1｛Ｋ_ｓ（ｐ_ｒ−ｐ）−ｆ｝ ＋（Ｉ−TM^-1K_ｄ）_ｎ−１］ （５） つまり、フィードバックされた位置（ｐ）と力（ｆ）を
式（５）に代入して求めたジョイイント角速度（_ｎ）
によりマニピュレータを速度制御すれば、任意の二次の
コンプライアンスが実現される。

しかしながらこの方法にも問題がある。この制御方式で
は摩擦に対して不安定になることがある。ここでは位置
の安定性について検討する。第３図にこの制御方式のブ
ロック図を示す。ここで位置のフィードバックが軌道の
生成を行うところまで戻っていることに注意されたい。
さて、式（３）から位置のフィードバックに係わる項を
抜き出すと次式のようになる。_rc ＝M^-1K_ｓ∫（ｐ_ｒ−ｐ）dt （６） 世界座標系の速度はフィードバック値と間違えないよ
うに_rcと書くことにする。さてこの_rcをジョイント
座標系に変換すればジョイントの目標角速度_rcを得
る。_rc ＝Ｊ（ｑ）^-1M^-1K_ｓ∫（ｐ_ｒ−ｐ）dt （７） 第３図のように速度フィードバックゲインをＫとすれば
ジョイントに印加するトルクτは以下のようになる。

τ＝Ｋ_rc ＝KJ（ｑ）^-1M^-1K_ｓ∫（ｐ_ｒ−ｐ）dt （８） あるいは、位置フィードバックゲインK₂を K₂＝KJ（ｑ）^-1M^-1K_ｓ （９） とすれば τ＝K₂∫（ｐ_ｒ−ｐ）dt （10） となり、位置のフィードバックについては積分制御にな
る。

一般に産業用マニピュレータは減速ギア等の機構上摩擦
が大きい。このジョイントにおける静摩擦トルクを
τ_fs、動摩擦トルクをτ_fdとする。ここで位置ｐがその
目標値ｐ_ｒから少しずれているとしよう。この場合、は
じめは静摩擦により止まっているが式（10）の右辺が時
間積分され、時間の経過とともに大きくなり、ある時間
ｔ_ｉにおいてついには静摩擦トルクτ_fsと等しくなる。
つまり τ_fs＝τ＝K₂（ｐ_ｒ−ｐ）ｔ_ｉ （11） この後、ジョイントトルクは静摩擦より大きくなりジョ
イントが動きはじめる。動作中の摩擦つまり動摩擦トル
クτ_fdは静摩擦トルクτ_fsより小さいので目標値ｐ_ｒを
通り過ぎ、反対側にずれてしまう。するとまた時間の経
過とともに逆方向のトルクが増大し静摩擦を越えると反
対方向に動きだす。この制御の下ではこのような動作を
永久に続けることになる。この動作を第４図に示す。こ
のような動作は位置ゲインK₂が小さいとき顕著に現れ
る。逆にいえばこの動作の繰返周期および位置偏差の振
幅を小さくするには位置ゲインK₂を大きくすればよく、
一般の積分制御では発振しない程度までこの位置ゲイン
K₂を大きくする、言い替えればフィードバックゲインは
制御性を考えるとあまり変えることは好ましくない。

さてここでこのコンプライアンス制御の目的を振り返っ
てみよう。コンプライアンス制御の目的は手先の動作に
おいて、そのばね（硬さ）やダンパ（減衰）を自由に設
定することである。さて、この制御方法では式（３）に
おいてばね定数Ｋ_ｓを変えることによってばねの硬さを
変える。これは式（９）から位置ゲインK₂を変えること
を意味する。例えばいま弱いばねを設定したいとしよ
う。そのためにばね定数Ｋ_ｓを小さくすれば必然的に位
置ゲインK₂が小さくなってしまい、前述した振動を避け
られない。また式（９）を見ると位置ゲインK₂を変える
他の要因として世界座標系からジョイント座標系への変
換行列すなわち逆ヤコビアン行列Ｊ（ｑ）^-1があげられ
る。この行列はジョイント角度の関数でありマニピュレ
ータの位置姿勢により０から無限大まで大きく値を変え
る。したがって位置ゲインK₂もその値を大きく変えるこ
とになり、ある位置姿勢で設定できた硬さが別の位置姿
勢では設定できないといったことが少なからず起こりえ
る。これらのことは位置ゲインK₂を変えることによりコ
ンプライアンス制御のばねの硬さを変えることに起因す
る。

従って、これらの欠点を克服するには、指定されたコン
プライアンスを満足する軌道の生成とフィードバックゲ
インをきちんと分けて考えなければならない。

C.問題点を解決するための手段 本発明はサーボによる位置剛性の高い産業用マニピュレ
ータに対し、運動方程式（１）を積分して検出された外
力により変位された位置を求め、これを逆キネマティク
スによりジョイント座標系の位置の軌跡を計算し、さら
にそれを微分して速度および加速度の軌跡を求め、これ
らの軌跡により位置制御をかけることによりコンプライ
アンス制御を行うものである。これによりコンプライア
ンスは任意の直交座標系においてただ単なる硬さだけで
なく減衰などを含む二次の振動の形で指定され、さらに
コンプライアンスの指定とフィードバックゲインを明確
に分けることにより指定されたコンプライアンスが忠実
に実現される。

D.実施例 本制御手法の概要を第１図に示す。第1A図は、後記式
（12）−（16）をそのまま反映したものである。また、
第1B図は、第３図との比較のため、第1A図のパラメータ
のうち、Ｋ_ｆを単位行列とし、かつ速度、および力の目
標値（_ｒ,f_ｒ）を０とした場合を示す。本制御手法は
次の三つの段階に分けて考えることができる。

（１）コンプライアンス軌道の生成 指定された世界座標系の位置（ｐ_ｒ）、速度（_ｒ）、
力（ｆ_ｒ）の目標値とセンサからのフィードバック値
（ｆ）および仮想質量・慣性（Ｍ）、減衰係数
（Ｋ_ｄ）、ばね係数（Ｋ_ｓ）、コンプライアンス選択行
列（Ｋ_ｆ）からなる二次のコンプライアンスの式（１）
において、位置（ｐ）をコンプライアンス軌道（ｐ_rc）
と書きなおし、同式を２回積分することにより、同式を
満足する位置の軌道（ｐ_rc）を求める。つまり、 ｐ_rc＝∫∫M^-1｛Ｋ_ｄ（_ｒ−_rc）＋ Ｋ_ｓ（ｐ_ｒ−ｐ_rc）＋Ｋ_ｆ（ｆ_ｒ−ｆ）｝ dtdt＋ｐ_ｒ （12） （２）座標変換およびジョイント座標系におけるコンプ
ライアンス軌道の生成 この世界座標系の位置の軌道（ｐ_rc）から逆キネマティ
クスを解きジョイント座標系の角度の軌道（ｑ_rc）を求
める。また、この角度の軌道（ｑ_rc）を微分し、角速度
および角加速度の軌道（_rc，_rc）を求める。

ｑ_rc＝IK（ｐ_rc） （13） IK（ ）：逆キネマティクス_rc ＝dq_rc/dt （14）_rc ＝ｄ_rc/dt （15） （３）位置制御 この生成されたジョイント座標系におけるコンプライア
ンス軌道に追従させるべく、位置制御をかける。

τ＝K_{0 rc}＋K₁（_rc−） ＋K₂（ｑ_rc−ｑ） （16） q:フィードバックされたジョイントの角度 ：フィードバックされたジョイントの角速度 τ：ジョイントに印加するトルク K₀,K₁,K₂:フィードバックゲイン この方法によれば、コンプライアンスのパラメータ（M,
K_ｄ,K_ｓ,K_ｆ）とフィードバックゲイン（K₀,K₁,K₂）が
明確に別れており、したがってコンプライアンスのパラ
メータ、とくにばねの硬さ（Ｋ_ｓ）を自由に設定でき
る。つまり、本発明の制御方法では設定されたばねの硬
さを実現するに際し式（12）から（15）によりその軌道
を求め、しかるのち式（16）により位置制御を行ってい
るので原理的にコンプライアンス制御のばねの硬さＫ_ｓ
と位置フィードバックゲインK₂はまったく別物であり、
したがって上記文献の方法で見られたような振動とは無
縁である。同様に位置フィードバックゲインが逆ヤコビ
アン行列Ｊ（ｑ）^-1を含んでいないので位置姿勢により
制御性が損なわれることはない。

さて、第5A図ないし第5E図を参照しつつ、本制御手法を
用いてコネクタ51にプラグ52を差し込む作業を考えてみ
よう。ここでは周辺環境の低コスト化のためにコネクタ
51およびプラグ52の置かれる位置の精度は悪いものとす
る。なお、マニピュレータ53は第5A図に示すような手先
に６方向の力センサ54が取り付けられた６軸のものであ
る。作業はプラグ52をコネクタ51のところまで持ってい
き挿入できるようにきちっと位置合わせすることから始
まる。まずおおよそのところまで位置制御によってプラ
グ52を持っていく（第5B図）。つぎに全方向にやや硬い
ばねの状態で小刻に動かしそのときの力の様子からコネ
クタ51の位置を探る（第5C図）。ほぼ位置の確認が終わ
ったら、差し込む方向には硬くそれ以外の方向にはばね
を柔らかくしプラグ52を差し込んでいく（第5D図）。こ
こでこの挿入作業はどこまで差し込めばいいかは位置と
してはわからず、ただ差し込めるまで差し込むものとす
る。これは人間が行う場合はごく普通のことである。さ
て本体挿入がすんだあたりまできたらこんどは挿入方向
の力の目標値を大きくして力で押し込んでいく。そして
ある値より大きな力が検出されたら挿入が済んだと判断
し、作業を終える（第5E図）。なお減衰のパラメータの
設定は系が発振しないように定める。

これらの作業は位置制御しかできない市販のマニピュレ
ータでは行えない。また、手法上設定できるコンプライ
アンスに制限の大きい従来のコンプライアンス制御では
このような微妙な力の制御を産業用マニピュレータに行
わせることは困難である。

E.効果 本発明はコンプライアンス制御を位置制御に帰着させ、
あとは通常のジョイントの角度制御を行なっているた
め、サーボによる位置剛性の高い産業用マニピュレータ
において比較的速度の遅い場合のコンプライアンス制御
においては有効である。従来の方法と比べて二次のコン
プライアンスのパラメータ、とくにばねの硬さ（Ｋ_ｓ）
を自由に設定することができ微妙な力加減を必要とする
組立作業などを行うことができる。

【図面の簡単な説明】

第1A図および第1B図は本発明のコンプライアンス制御方
法を示すブロック図、 第２図はマニピュレータの手先のコンプライアンスを表
現するための図、 第３図は従来のコンプライアンス制御方法を示すブロッ
ク図、 第４図は従来のコンプライアンス制御に見られる振動の
様子を説明するための図、 第5A図ないし第5E図は本発明の制御方法を用いたプラグ
のエッジ・コネクタへの挿入作業を説明するための図で
ある。

Claims (1)

【特許請求の範囲】
1. 【請求項１】制御対象物に作用する力を検出し、検出さ
   れた力に基づいて制御対象物の位置を制御するシステム
   において、 コンプライアンスに関するパラメータ、およびコンプラ
   イアンスの釣合に関する量を指定し、 上記指定されたコンプライアンスに関するパラメータ、
   コンプライアンスの釣合に関する量、および検出された
   力に関する量を含む、世界座標系における制御対象物の
   ２次の運動方程式を解いて、同運動方程式を満足する制
   御対象物の位置の軌道を生成し、 上記世界座標系における制御対象物の位置の軌道を、同
   制御対象物に固有の座標系における同制御対象物の位置
   の軌道に変換し、 上記固有の座標系における制御対象物の位置の軌道か
   ら、同座標系における制御対象物の速度の軌道と加速度
   の軌道を生成し、 上記固有の座標系における制御対象物の位置、速度、お
   よび加速度の軌道に追従させるべく、同制御対象物の位
   置制御を行う ことを特徴とするコンプライアンス制御方法。