JP2706555B2 - 多自由度作業機械の位置と力の制御装置 - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は多自由度作業機械の位置と力の制御装置に関
し、特に、少なくとも２以上の自由度を有するロボット
や工作機械等において力制御モード又はコンプライアン
ス制御モードで作業対象物に接触して作業を行いながら
移動する手先効果器が滑らかに作業対象物から離脱する
ようにした位置と力の制御装置に関するものである。

〔従来の技術〕

位置情報に基づき位置制御のみで動作している従来の
産業用ロボットや自動工作機械では、嵌め合い作業や研
磨作業の如く作業対象物に作業工具を接触させ、その接
触力を制御して作業することができなかった。そこで、
最近ではかかる作業も行うことができるように、位置の
制御のみならず力の制御を行うことのできる制御方式が
提案されている。この制御方式には、位置制御モードと
力制御モードを状況に応じて適宜に切り替えて制御する
ハイブリッド制御方式（トランザクション・オブ・ザ・
エー・エス・エム・イー，ジャーナル・オブ・ダイナミ
ック・システムズ，メージャラメント・アンド・コント
ロール、（Transaction of the ASME,Journal of Dynam
ic Systems,Measurement and Control,vol102,p.126〜1
33,1981）や、機械系をバネ要素と質量要素とダンパ要
素からなる力学的モデルとして仮想的に実現する仮想コ
ンプライヤンス制御方式（特開昭62-35915号公報）があ
る。

前記の制御方式では、いずれの方式でも、作業対象物
に作業工具を接触させて作業を行う場合には、先ず接触
状態になるまで作業工具を空中において位置制御モード
に基づいて移動させ、作業工具が作業対象物に接触した
後に接触方向に力制御を実行するか又は手先部にバネが
入ったように制御するコンプライアンス制御を実行し、
適当な押付け力で接触状態を保持するように制御を行な
い、作業が終わった後には、接触状態から離脱し空中を
位置制御モードで移動して、次の作業に移行する。

〔発明が解決しようとする課題〕

従来の改良された、位置制御モードと力制御モードと
が組み合わされて実行される産業用ロボット等では、作
業工具が作業対象物から離脱する場合、前述の如く作業
対象物に接触して所要の力作業を行うときには接触面に
対して力制御又はコンプライアンス制御が行われ、空中
の移動状態では位置制御が行われるが、ここで制御上問
題となるのは接触作業状態から空中移動状態に変化する
過渡的な時点における制御をどのように行うかというこ
とである。すなわち、作業対象物に接触した状態にある
作業工具が空中に離脱する瞬間の制御が問題となる。

かかる制御状態変化の時点の制御の問題を具体的に説
明する。作業対象物に力制御等で接触状態にある作業工
具を空中に離脱させるため、力制御あるいはコンプライ
アンス制御から位置制御に切換えると、位置制御では接
触状態での力を逃がすことができないため、多大な力が
作業工具に加わるおそれがある。すなわち、例えば力制
御から位置制御に切換えるには位置目標値を与え、設定
する必要があり、この位置目標値には現在の接触状態に
おける位置検出値を用いるが、一般的に多関節ロボット
では位置制御での位置目標値と位置検出値との間にはロ
ボットの自重等の影響によって偏差が生じており、また
位置検出後にロボットが移動したり、その他の演算上の
誤差等に起因して、位置制御への切換え前の位置を正確
に保つことは困難である。従って、位置制御へ切換え
後、作業工具が例えば作業対象物の内部の位置に移動す
るように位置制御を受けると、作業工具は作業対象物に
衝突し、力を受けることになる。このことは場合によっ
て、多大な力が作業工具に加わるおそれがある。

またコンプライアンス制御から位置制御に切換える場
合に、位置目標値をそのままにして切換えると、今まで
仮想バネでたわんでいたものが急にバネが固くなったの
と同様になり、多大な力を発生するおそれがある。

本発明の目的は、作業工具等の手先効果器が、力制御
又はコンプライアンス制御のモードで作業対象物に接触
している状態から、空中の非接触状態で且つ位置制御の
モードで移動する状態に変化する場合、作業対象物から
の離脱の瞬間に、過大な力を発生することなく滑らかに
離脱を行い、装置等を破損することなく高い安全性をも
って移動することのできる多自由度作業機械の位置と力
の制御装置を提供することにある。

〔課題を解決するための手段〕

本発明に係る多自由度作業機械の位置と力の制御装置
は、手先効果器に加わる力を検出する力検出手段と、手
先効果器の位置を検出する位置検出手段と、力目標値設
定手段と、位置目標値設定手段と、位置・力制御演算手
段とを備え、力目標値設定手段で設定された力目標値と
力検出手段で検出される力検出値の偏差により手先効果
器の移動速度を決める力制御モードと、位置目標値設定
手段で設定された位置目標値と位置検出手段で検出され
る位置検出値の偏差により位置の制御を決める位置制御
モードとが実現され、位置・力制御演算手段の設定値を
任意に与えることにより位置制御モードと力制御モード
のうちいずれか一方又は複合された両方を実行して手先
効果器が作業対象物に対して作業を行い得るように制御
し、手先効果器の位置と力を制御し作業対象物に接触し
ながら作業を行う多自由度作業機械の位置と力の制御装
置において、力目標値設定手段と位置目標値設定手段と
位置・力制御演算手段の各設定値を変更し、手先効果器
の制御モードを位置制御モードと力制御モードのうちい
ずれか一方に切換える制御モード切換え手段と、手先効
果器が作業対象物に接触している状態から作業対象物に
接触しない状態へ離脱する時、離脱方向について、手先
効果器が作業対象物から離れる時点から、位置制御モー
ドに切換えてこの位置制御モードによる手先効果器の移
動を行うとき当該移動が安全になるまでの区間で、制御
モード切換え手段に対して、力制御モードへの切換えを
指示する離脱動作指示手段を備えることを特徴とする。

また本発明に係る多自由度作業機械の位置と力の制御
装置は、前記の構成において、手先効果器が作業対象物
に接触して移動する時には、送り方向には位置制御モー
ド及び作業対象物への押付け方向には力制御モードが実
行され、手先効果器が区間の移動後、作業対象物に接触
しない状態で移動する時には位置制御モードが実行され
ることを特徴とする。

更に本発明に係る多自由度作業機械の位置と力の制御
装置は、前記の構成において、手先効果器の制御状態に
ついて各軸の制御モードを判定し、力制御モードから位
置制御モードへの切換えを検出する制御モード切換え検
出手段を備え、制御モード切換え検出手段は、いずれか
の軸で力制御モードから位置制御モードへの切換えを検
出した時に、離脱動作指示手段に対して指令を出し、当
該指令を受けた離脱動作指示手段は制御モード切換え手
段に対し力制御モードへの切換えを指示することを特徴
とする。

〔作用〕

本発明による多自由度作業機械の位置と力の制御装置
では、力目標値と力検出値との偏差によって速度を制御
する方式を備える制御装置であって、この方式によって
外部より力の加わらない空中の移動を力制御によって行
うことを可能とし、力目標値を与えることにより当該力
目標値に比例した速度で移動するという「力制御モード
による移動」を手先効果器に行わせることができる。本
発明では、力制御モードによる移動を手先効果器に行わ
せることができるため、力制御等のモードで作業対象物
に接触している状態において作業対象物から離脱すると
き、力制御モードを保持することができ、接触状態の間
では常に力制御を行うことができるため、過大な力が発
生することがない。かかる力制御モードによる移動は、
手先効果器が位置制御モードに切換えても安全となる位
置へ移動するまでの所定の区間で実行され、これにより
手先効果器は作業対象物から滑らかに離脱し、更にその
位置制御モードに切換えられて次の作業ステップに滑ら
かに移行することになる。

また力制御モード又はコンプライアンス制御モードか
ら位置制御モードへの切換えが行われる時には常に自動
的に前記の力制御モードによる移動が行われるように構
成することにより、ロボット教示の時には接触状態での
作業の最後の点で接触方向を位置制御モードに変更し、
後は空中での次の動作を教示するだけで良く、再生時に
は離脱直前に前記力制御モードによる移動が自動的に入
り、取扱いが非常に便利となる。

〔実施例〕

以下に、本発明の実施例を添付図面に基づいて説明す
る。

第１図は本発明に係る位置と力の制御装置を有する多
自由度作業機械の一実施例を示す。第１図において、１
はロボット本体で、複数の関節部を有するアーム２が支
持基台３に取付けられ、アーム２の各関節部の可動作用
により、アーム先部は作業上必要とされる位置に移動
し、アーム全体の姿勢は作業上必要な姿勢に変化する。
アーム２の先部に位置するリスト部４に６軸の力センサ
５が取付けられ、更に力センサ５の先には、作業対象物
であるワーク６に対して所要の作業を行う手先効果器７
が取付けられている。ロボット本体１の内部にはアーム
２の各関節等を動かすための駆動モータ（図示せず）が
複数内蔵されている。これらの駆動モータには、その駆
動量を計測する角度計８が取り付けられており、これら
の角度計８によって各関節の軸角度データを得ることが
できる。

次いで、上記の構成を有するロボット本体１の動作を
制御する制御装置の構成を説明する。

第１図ではブロック回路で制御装置の構成を示してい
る。制御装置は例えばマイクロコンピュータを利用した
ソフト的構成によって実現される。なお、ハード的な回
路構成で実現できるのは勿論である。９は力目標値設定
部、10は位置目標値設定部である。力目標値設定部９に
はx,y,zの各座標軸について押付け力の目標値 （各軸の目標値を要素とするベクトルとして表示され
る）が予め設定され、力目標値設定部９は設定された目
標値 を減算器11に対し出力する。力目標の設定には例えば教
示データが使用される。位置目標値設定部10には各軸に
ついて手先効果器７の位置の目標値 （各軸の目標値を要素とするベクトルとして表示され
る）が予め設定され、位置目標値設定部10は設定された
目標値 を減算器12に対し出力する。位置目標値の設定には例え
ば教示データを補間した値が使用される。

前記力センサ５で検出された力は力演算部13に送給さ
れ、力演算部13で、センサ座標系から手先座標系である
基準座標系に変換すると共に手先効果器７の重力分を差
し引く重力補償を行うことによりワーク６と手先効果器
７の接触点での手先効果器７に加わる力 を算出する。力演算部13で得られた力 のデータは減算器11に与えられる。一方、角度計８で検
出された各軸の角度データは位置データとして位置演算
部14に送給され、位置演算部14でこれらの角度データに
よって絶対座標系における手先効果器７の位置と姿勢 が算出される。位置演算部14で得られた位置と姿勢 のデータは減算器12に与えられる。

減算器11では、 とが比較され、その偏差 が出力される。一方、減算器12では、 が比較され、その偏差 が出力される。このようにして得られた上記の各偏差 は位置・力制御演算部15に入力される。

位置・力制御演算部15は仮想コンプライアンス制御系
として構成され、任意に調整できる不感帯幅Ｗが設定さ
れた不感帯演算部16と、調整可能なバネ定数行列（コン
プライアンス行列）Ｋが設定されたバネ定数乗算部17
と、減算器18と、コントローラゲインKcが設定された特
性補償演算部19とから構成される。かかる構成によっ
て、不感帯演算部16を通過した とバネ定数乗算部17を通過した は減算器18で減算が行われ、その後、特性補償演算部19
でコントローラゲインKcが乗じられることにより目標速
度 を表す速度指令信号が位置・力制御演算部15から出力さ
れる。このようにして位置・力制御演算部15で求められ
た速度指令値 は駆動指令部20に供給され、この駆動指令部20で速度指
令値 はロボット本体１の各駆動モータの駆動指令値 に変換される。変換された駆動指令値 はロボット本体１の各駆動モータに供給され、この指令
値によってモータは所要速度で動作する。

上記構成において、仮想コンプライアンス制御は、ソ
フトウェアで形成される制御方式によって手先効果器７
に見掛け上あたかもバネが存在するような動作をさせる
もので、このバネ定数を制御軸ごとに自由に変更するこ
とができる。仮想コンプライアンス制御によれば、その
設定値の与え方によって、位置制御モード又は力制御モ
ード、両制御が複合された制御モードで動作させること
ができる位置と力の制御方式である。バネ定数行列Ｋや
コントローラゲインKcはｘ軸、ｙ軸、ｚ軸の各軸に対応
する値を対角成分に有する対角行列であり、また不感帯
演算部16は各軸に対応して設けられる。例えば、不感帯
演算部16については、不感帯幅を各軸ごとに任意に設定
することができ、不感帯をなくすこともできるし、不感
帯を大きくとることによってその出力を常に０にし、力
フィードバックをなくして位置制御のみを行うこともで
きる。バネ定数乗算部17については、各軸の対角成分Ki
を任意に設定することができ、特にKiを０にすれば位置
のフィールドバックをなくして力制御のみを行うことが
できる。このように、x,y,zの各座標軸はそれぞれ独立
した制御特性（力制御特性又は位置制御特性、あるいは
両特性が複合された制御特性）を有するように設定する
ことが可能である。

次に動作指令部21は、一般的に仮想コンプライアンス
制御を実行する前記制御装置の制御モードを指令し設定
する回路要素で、予め教示された動作データを格納し、
この動作データに基づいて制御装置を動作せしめロボッ
ト本体１に対し教示動作を再生させる機能を有する。本
実施例では特に制御モード切換え命令22を制御モード切
換え部23に与えたり、教示された位置データを補間し、
そのデータ24を位置目標値設定部10に与える機能を備え
ている。更に動作指令部21は離脱動作指示部25に対し離
脱動作指示を行わせるための指令26を与える。

制御モード切換え部23は、動作指令部21から制御モー
ド切換え指令22を受けると、この指令に従って力目標値
設定部９、位置目標値設定部10、不感帯演算部16、バネ
定数乗算部17の設定値を適宜に設定することにより、制
御装置の制御モードを、位置制御モード、コンプライア
ンス制御モード又は力制御モードに適宜に切換えること
ができる。また離脱動作指示部25は、ロボット本体１の
手先効果器７の離脱動作に備えて、動作指令部21からの
指令26に基づき制御モード切換え部23に対し手先効果器
の７の移動方向、すなわちワーク６からの離脱が開始さ
れる方向を離脱のための力制御モードに設定する指令27
を出力する。つまり離脱動作指示部25は、動作指示部21
の指令に従って、制御モード切換え部23及び位置・力制
御演算部15等を介して手先効果器７の離脱しようとする
方向の制御モードを離脱移動を行うための力制御モード
に設定し、手先効果器７の力制御モードで移動させてワ
ーク６から離脱させることを指示する機能を有してい
る。加えて、離脱動作指示部25は位置演算部14の出力信
号を入力し位置演算部14で出力される位置データを監視
し、手先効果器７がワーク６から離脱して所定の距離移
動した時には終了信号28を動作指令部21に返送する。

次に上記構成を有した制御装置による手先効果器７の
離脱動作について説明する。

先ず、ワーク６の表面に対して力制御等に基づいて所
要の作業を行い、前記表面に並行に位置制御の下で移動
する手先効果器７が、ワーク６から離脱するときの制御
装置における制御動作を第２図のフローチャートを用い
て説明する。動作指令部21が所要のタイミングで離脱動
作指示指令26を離脱動作指示部25に与えると、離脱動作
指示部25は制御モード切換え部23に指令27を出力して、
手先効果器７のワーク６からの離脱方向の制御モードを
移動のための力制御とし、それ以外の他方向の制御モー
ドを必要に応じて位置制御、力制御又はコンプライアン
ス制御のモードとして設定する（ステップ30）。この設
定は制御モード切換え部23が、力目標値設定部９と位置
目標値設定部10と不感帯演算部16とバネ定数乗算部17の
各軸の値を予め定められた値に変更することにより実行
される。ステップ30において切換えられる不感帯幅、バ
ネ定数、力目標値、位置目標値のそれぞれに関し離脱方
向及びその他の方向の各設定値の一例を第２図中の注釈
31に示している。なお、この注釈31においてその他の方
向に関する〔 〕の中の値はコンプライアンス制御モー
ドに切換えたときの値である。離脱方向については、力
目標値設定部９に設定される力目標値が−frである状態
において力制御が行われる。ここで、−frとしたのは押
付け方向を正にとった場合に負の方向を表している。

次のステップ32では前述された設定状態により、手先
効果器７の移動では離脱方向について力制御モードでの
移動動作が行われる。かかる離脱方向に関しては力制御
が実行されているので、制御装置では、力目標値設定部
９に設定された前記の力目標値−frと力演算部13で算出
された力ｆとの偏差が減算器11で求められる。しかし、
非接触状態の空中においては、手先効果器の７の受ける
力は０であるから、Δｆ＝frの値が不感帯演算部16を通
して減算器18に加えられる。また、第２図中に示された
注釈31に記載されるように離脱方向のバネ定数Ｋは０で
あるので、位置制御ループからの入力は存在しない。そ
のためfrの値が特性補償演算部19に直接に入力される。
特性補償演算部19のコントローラゲインをKcであるとす
ると、ｖ＝Kc・frが駆動指令部20に入力され、手先効果
器７を含むロボット本体１が前記ｖの速度指令値により
動作する。換言すれば、ロボット本体１は力目標値設定
部９に設定された力目標値−frに比例した速度で動作す
る。以下の説明において、このような移動動作を力制御
モードでの移動を呼ぶことにする。離脱動作の初期にお
いて手先効果器７はワーク６と接触状態にあるが、手先
効果器７の受ける反力ｆによりΔｆ＝ｆ−（fr）＝ｆ＋
frとなり、手先効果器７はｖ＝Kc・（ｆ＋fr）の速度で
動作する。すなわち、反力分で離脱方向に押されて素早
く離脱方向に動き、非接触状態になってから一定速度で
動く。また、離脱方向以外の方向は位置制御モード又は
必要に応じてコンプライアンス制御モードに設定されて
おり、且つその位置目標値は一定値に保持されるので、
離脱方向以外の方向には移動しない。その結果、手先効
果器７は離脱方向に直線的に移動することになる。

上記の移動状態において、離脱動作指示部25は適当な
タイミングで位置演算部14にて算出される位置データを
監視し、離脱方向への移動距離が離脱動作指示部25で設
定された値ｌ₀よりも大きいか否かを判定する（ステッ
プ33）。このｌ₀の値は、位置制御モードに切換えた時
に手先効果器７がワーク６に接触したり、衝突したりし
ないような適当な距離に設定される。

ステップ33で位置演算部14の出力を監視している離脱
動作指示部25は、手先効果器７がワーク６から離脱して
所定の距離ｌ₀を移動した時に、離脱動作のための力制
御を終了する信号28を動作指令部21に出力する。動作指
令部21はこの終了信号28を受け取ると、制御モード切換
え部23に制御モード切換え指令22を与え、制御モード切
換え部23を介して制御装置の各定数及び変数の設定値を
空中移動のための制御モードに切換えさせる。

次に、ロボット本体１による前記離脱動作を含む一連
の作業のための手先効果器７の動作制御の一例を第３図
及び第４図を参照して説明する。第３図は手先効果器７
の移動軌跡とワークとの関係を示し、第４図は倣い作業
の動作と前記離脱動作との関係を明らかにしたフローチ
ャートである。

第３図では前記ワーク６の表面6aを手先効果器７によ
って点Ｐ₁まで力制御に基づき倣い動作を行った後に離
脱動作を行う状態図を示している。手先効果器７は倣い
動作によってワーク表面6aの上を点Ｐ₁まで移動する
（ステップ40）。この倣い動作のための制御モードで
は、送り方向（第３図中ｘ方向）を位置制御モード、押
付け方向（第３図中ｚ方向）を作業のための力制御モー
ドに設定している。送り方向及び押付け方向のそれぞれ
の不感帯幅、バネ定数、力目標値、位置目標値の各設定
値については第４図の注釈41に記載されている。上記の
如き制御モードにてステップ40と42を繰り返し実行する
ことにより、手先効果器７はワーク表面6a上を点Ｐ₁ま
で倣いながら移動する（移動状態Ａ）。

手先効果器７が点Ｐ₁の位置に到達すると（ステップ4
2）、手先効果器７は空中に向う所定方向、すなわち例
えばｚ軸方向に向い、ワーク表面から離脱動作を行う
（ステップ43）。ステップ43による離脱を行うための移
動状態Ｂでは、離脱方向であるｚ軸方向に関して、前述
した第２図で説明した力制御モードによる移動が行われ
る。従って、動作指令部21は手先効果器７が点Ｐ₁に到
達すると、離脱動作指示部25に指令26を送り、更に制御
モード切換え部23を介して制御装置の各制御パラメータ
を力制御モードによる移動に切換える。この時力目標値
設定部９に設定される力目標値の値−frは、倣い動作の
時に設定した力目標値fr′と符号が逆になっており、逆
方向に力制御を行うことになる。この時、−frの絶対値
はfr′と同じにすることもできる。この場合には、倣い
動作の押付け方向と離脱動作時の離脱方向が反対で且つ
同じ方向線上にある場合には、制御モードは同一の力制
御モードのままで、目標力の値の符号を反転するだけで
良い。また倣い動作における送り方向については、点Ｐ
₁に向って時々刻々と変化する位置目標値から、固定値
に変更する。

手先効果器７がワーク表面6aから離脱して所定の距離
を動き、点Ｐ₂に到達し、離脱動作が終了すると、動作
指令部21は離脱動作指示部25から終了信号28を受けて、
制御モード切換え部23に次の制御ステップのための制御
切換え指令22を与える。制御モード切換え部23は、この
指令を受けると、制御装置の各制御パラメータを空中に
移動すべく位置制御を行うための制御モードに切換える
（ステップ44）。注釈45には不感帯幅、バネ定数、力目
標値、位置目標値、の設定状態が示されている。この位
置制御モードによって、ステップ46,47に示すように手
先効果器７は空中を、例えば他の目的地である点Ｐ₃ま
で移動する（移動状態Ｃ）。移動状態Ｃにおける移動速
度は任意に設定される。また点Ｐ₃の位置も任意に設定
される。

以上の動作において、制御装置の各設定部において設
定される諸データは、最初のロボット本体１に対する教
示動作によって適宜に設定され、これらの教示データは
動作指令部21に蓄えられている。動作指令部21は教示デ
ータを制御モード切換え部23を経由して力目標値設定部
９や位置・力制御演算部15などに与える。

以上のように倣い動作の移動状態Ａと非接触の移動状
態Ｃとの間において、手先効果器７が適切にワーク表面
から離脱するための離脱動作用の移動状態Ｂを介在させ
るように構成したため、Ｐ₁点で過大な力を発生するこ
となく、滑らかに倣い動作から離脱動作及び空中移動へ
移行することができる。

次に上記の離脱動作、すなわち力制御モードによる移
動を移動状態ＡとＣとの間において自動的に行う制御の
実施例を説明する。この制御方式は、第５図（Ａ）に示
されるように、力制御モードから位置モードに切換わっ
た時（ステップ50）に、自動的に離脱動作のステップ51
を実行させるように制御を行うものである。この制御の
方式では、動作指令部21は、ステップ50を実行して力制
御モードから位置制御モードの切換えを各軸ごとに判定
し、切換えがいずれかの軸に発生した時には、離脱動作
指示部25に対し指令26を与え、離脱動作を行わせる。

上記の如く力制御モードから位置制御モードに切換え
が行われる時に自動的に離脱動作を行うように構成した
場合の動作記述例を第５図（Ｂ）に示す。第５図（Ｂ）
の記述で明らかなように、第３図における状態ＡとＣの
動作を記述するだけで、第５図（Ｂ）のステップ３で離
脱動作が自動的に行われる。この場合の離脱動作におい
て、離脱動作の方向や力目標値などは別途に予め設定さ
れる。また教示作業をいっそう簡単にするために、離脱
動作の方向や力目標値の絶対値をその前のステップでの
力制御と同じにしても良い。すなわち、第３図及び第４
図の実施例を参照して説明すれば、離脱動作の離脱方向
を、状態Ａで行う倣い動作の押付け方向の逆にとり、力
目標値を倣い動作の目標値の符号を変えた−frとするこ
とにより、離脱動作についての特別の記述を一切行うこ
となく第５図（Ｂ）の記述によって自動的に離脱動作を
実行することができる。

前述の各実施例では、第１図に示した制御装置構成に
おける位置・力制御演算部15として、仮想コンプライア
ンス制御の場合を例にして説明したが、位置と力のハイ
ブリッド制御など他の制御方式を用いることもできる。
これらの場合においても、制御演算部の出力としては速
度ｖを出力することにより、ロボット本体１の各部の速
度を指示するように構成される。力目標値と力検出値の
力の偏差によって速度指令値を与える方式のものでは、
前述の力制御モードでの移動を行うことが可能となる。

また第２図のフローチャートでは離脱動作の判定を移
動距離を用いて行なっているが、判定の基準としては、
当該移動距離に相当するような時間を用いるようにする
こともできる。

〔発明の効果〕

以上の説明で明らかなように本発明によれば、手先効
果器の制御がワークにおける接触状態の力制御モードか
ら非接触移の位置動制御モードに変更される時、離脱を
行う所定の区間において力制御モードで移動させるよう
に構成したため、離脱時に過大な力が発生するのを防ぐ
ことができ、倣い動作から離脱及び空中移動への移行を
滑らかに行うことができる。また力制御モードから位置
制御モード等への切換えの際に常に本発明による力制御
モードでの移動が自動的に実行されるように構成したた
め、制御装置を作製するに当たって、離脱動作を含む作
業のための動作制御の記述を簡単に行うことができる。

【図面の簡単な説明】

第１図は本発明に係る位置と力の制御装置が適用された
ロボット本体と制御装置を示す構成図、第２図は離脱動
作の制御フローを示すフローチャート、第３図は本発明
による離脱動作の制御を倣い動作に適用した例を示す説
明図、第４図は第３図に示された手先効果器の動作の制
御を示すフローチャート、第５図は本発明により離脱動
作を自動的に実行するフローチャートとそのソフト的な
記述例を示す図である。 〔符号の説明〕 １……ロボット本体 ２……アーム ５……力センサ ６……ワーク ７……手先効果器 ９……力目標値設定部 10……位置目標値設定部 13……力演算部 14……位置演算部 15……位置・力制御演算部 21……動作指令部 23……制御モード切換え部 25……離脱動作指示部

Claims (3)

(57)【特許請求の範囲】
1. 【請求項１】手先効果器に加わる力を検出する力検出手
   段と、前記手先効果器の位置を検出する位置検出手段
   と、力目標値設定手段と、位置目標値設定手段と、位置
   ・力制御演算手段とを備え、前記力目標値設定手段で設
   定された力目標値と前記力検出手段で検出される力検出
   値の偏差により前記手先効果器の移動速度を決める力制
   御モードと、前記位置目標値設定手段で設定された位置
   目標値と前記位置検出手段で検出される位置検出値の偏
   差により位置の制御を決める位置制御モードとが実現さ
   れ、前記位置・力制御演算手段の設定値を任意に与える
   ことにより前記位置制御モードと前記力制御モードのう
   ちいずれか一方又は複合された両方を実行して前記手先
   効果器が作業対象物に対して作業を行い得るように制御
   し、前記手先効果器の位置と力を制御し前記作業対象物
   に接触しながら作業を行う多自由度作業機械の位置と力
   の制御装置において、 前記力目標値設定手段と前記位置目標値設定手段と前記
   位置・力制御演算手段の各々の設定値を変更し、前記手
   先効果器の制御モードを前記位置制御モードと前記力制
   御モードのうちいずれか一方に切換える制御モード切換
   え手段と、 前記手先効果器が前記作業対象物に接触している状態か
   ら前記作業対象物に接触しない状態へ離脱する時、離脱
   方向について、前記手先効果器が前記作業対象物から離
   れる時点から、位置制御モードに切換えてこの位置制御
   モードによる前記手先効果器の移動を行うとき当該移動
   が安全になるまでの区間で、前記制御モード切換え手段
   に対して、力制御モードへの切換えを指示する離脱動作
   指示手段を備える、 ことを特徴とする多自由度作業機械の位置と力の制御装
   置。
2. 【請求項２】請求項１記載の多自由度作業機械の位置と
   力の制御装置において、前記手先効果器が前記作業対象
   物に接触して移動する時には、送り方向には前記位置制
   御モード及び前記作業対象物への押付け方向には力制御
   モードが実行され、前記手先効果器が前記区間の移動
   後、前記作業対象物に接触しない状態で移動する時には
   前記位置制御モードが実行されることを特徴とする多自
   由度作業機械の位置と力の制御装置。
3. 【請求項３】請求項１記載の多自由度作業機械の位置と
   力の制御装置において、前記手先効果器の制御状態につ
   いて各軸の制御モードを判定し、前記力制御モードから
   前記位置制御モードへの切換えを検出する制御モード切
   換え検出手段を備え、前記制御モード切換え検出手段
   は、いずれかの前記軸で前記力制御モードから前記位置
   制御モードへの前記切換えを検出した時に、前記離脱動
   作指示手段に対して指令を出し、当該指令を受けた前記
   離脱動作指示手段は前記制御モード切換え手段に対し力
   制御モードへの切換えを指示することを特徴とする多自
   由度作業機械の位置と力の制御装置。