(57)【特許請求の範囲】
【請求項１】
マニピュレーターと、
前記マニピュレーターに作用する力を検出する力検出器と、
を備えるロボットを制御するロボット制御装置において、
前記マニピュレーターの動作を制御する制御部は、前記マニピュレーターを動作させるコマンドを実行するとき、前記力検出器の出力に基づいて前記マニピュレーターの動作を制御するか、前記力検出器の出力に基づかずに前記マニピュレーターの動作を制御するかを前記コマンドのパラメータに応じて切り換え、
前記制御部は、前記コマンドを実行し、前記力検出器の出力に基づいて前記マニピュレーターの動作を制御するとき、前記力検出器をリセットするとともに、前記力検出器のリセットから所定時間経過後に、前記力検出器の出力に基づく前記マニピュレーターの動作制御を開始する、
ロボット制御装置。
【請求項２】
マニピュレーターと、前記マニピュレーターに作用する力を検出する力検出器と、を備えるロボットと、
前記マニピュレーターの動作を制御する制御部を備えるロボット制御装置と、を備え、
前記制御部は、
前記マニピュレーターを動作させるコマンドを実行するとき、前記力検出器の出力に基づいて前記マニピュレーターの動作を制御するか、前記力検出器の出力に基づかずに前記マニピュレーターの動作を制御するかを前記コマンドのパラメータに応じて切り換え、
前記コマンドを実行し、前記力検出器の出力に基づいて前記マニピュレーターの動作を制御するとき、前記力検出器をリセットするとともに、前記力検出器のリセットから所定時間経過後に、前記力検出器の出力に基づく前記マニピュレーターの動作制御を開始する、
を備えるロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置およびロボットシステムに関する。
【背景技術】
【０００２】
ロボットの分野においては、マニュピレーターに加わる力に応じてマニピュレーターを制御する力制御が用いられている。たとえば、特許文献１の図２には、「Activate ForceControl;」コマンドと、「Deactivate ForceControl;」コマンドとによって、マニュピレーターに加わる力に応じてマニピュレーターを制御する力制御モードと、マニュピレーターに加わる力と無関係にマニピュレーターを制御する位置制御モードとの切換が行われるプログラムが記載されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】米国特許７３４０３２３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、特許文献１に記載されているようにコマンドによって位置制御モードから力制御モードに切り換えた後に、加わる力に応じてマニピュレーターを動作させるコマンドを実行する場合、マニピュレーターの動作結果の再現性が低くなるという問題がある。すなわち、水晶の圧電効果を用いている場合など、力検出器によっては出力が時間に依存するため、力検出器のどの時間の出力に基づいてマニピュレーターを制御するかによって、動作結果が異なることになる。具体的には、位置制御モードから力制御モードに切り換わった時点における力検出器の出力を基準としてマニピュレーターに加わる力を検出すると、力制御モードに切り換わってからマニピュレーターを実際に動かすコマンドが実行されるまでの時間の違いにより、マニピュレーターの動作結果が異なることになる。
【０００５】
本発明は、このような問題を解決するために創作されたものであって、ロボットに対する力制御の再現性を高める技術を提供することを目的とする。
【課題を解決するための手段】
【０００６】
前記目的を達成するためのロボット制御装置は、マニピュレーターと、前記マニピュレーターに作用する力を検出する力検出器と、を備えるロボットを制御するロボット制御装置であって、前記マニピュレーターの動作を制御する制御部は、前記マニピュレーターを動作させるコマンドを実行するとき、前記力検出器の出力に基づいて前記マニピュレーターの動作を制御するか、前記力検出器の出力に基づかずに前記マニピュレーターの動作を制御するかを当該コマンドのパラメータに応じて切り換える。
【０００７】
ここで「マニピュレーターを動作させるコマンド」は、それ自体でマニピュレーターを動作させるコマンドを意味し、設定、モードの切換といった、それ自体ではマニピュレーターを動作させることがないコマンドを含まない。すなわち例えば、本発明では、マニピュレーターを動作させるコマンドの引数やコマンドの本体自体によって、力検出器の出力に基づいてマニピュレーターの動作を制御するか否かが決定される。したがって本発明によると、力検出器の出力に基づいてマニピュレーターの動作を制御する場合には、マニピュレーターを実際に動かすコマンドが実行される時点を出力の基準とする力検出器の出力に基づいてマニピュレーターの動作を制御することができる。このため本発明によるとロボットに対する力制御の再現性を高めることができる。
【０００８】
なお請求項に記載された各手段の機能は、構成自体で機能が特定されるハードウェア資源、プログラムにより機能が特定されるハードウェア資源、又はそれらの組み合わせにより実現される。また、これら各手段の機能は、各々が物理的に互いに独立したハードウェア資源で実現されるものに限定されない。
【図面の簡単な説明】
【０００９】
【図１】ロボットシステムの斜視図である。
【図２】ロボットシステムのブロック図である。
【図３】制御装置で実行される動作制御コマンドを示す表である。
【図４】力制御対応コマンドが実行される場合の処理順序を示すフローチャートである。
【図５】力覚センサーの出力を示す折れ線グラフである。
【図６】引数オブジェクトの階層構造を説明するための図である。
【図７】引数オブジェクトの設定画面を示す画面構成図である。
【図８】プログラムコードの実施例と比較例である。
【発明を実施するための形態】
【００１０】
以下、本発明の実施形態について添付図面を参照しながら説明する。なお、各図において対応する構成要素には同一の符号が付され、重複する説明は省略される。
【００１１】
（１）ロボットシステムの構成
本発明の一実施例としてのロボットシステムは、図１に示すように、ロボット１と、エンドエフェクター２と、制御装置３と、教示装置４（ティーチングペンダント）と、を備えている。制御装置３は、本発明のロボット制御装置の構成例である。制御装置３は図示しないケーブルによりロボット１と通信可能に接続される。なお、制御装置３の構成要素がロボット１に備えられていても良い。制御装置３は、ロボット１に駆動電力を供給する電源部３１とロボット１を制御するための制御部３２とを備える。制御装置３と教示装置４とはケーブルで、または無線通信可能に接続される。教示装置４は、専用のコンピューターであってもよいし、ロボット１を教示するためのプログラムがインストールされた汎用のコンピューターであってもよい。例えばロボット１を教示するための専用装置であるティーチングペンダント５を教示装置４の代わりに用いても良い。さらに、制御装置３と教示装置４とは、図１に示すように別々の筐体を備えていてもよいし、一体に構成されていてもよい。
【００１２】
ロボット１は、アームＡに各種のエンドエフェクター２を装着して使用される単腕ロボットである。アームＡは６つの関節Ｊ１〜Ｊ６を備える。関節Ｊ１〜Ｊ６によって６個のアーム部材Ａ１〜Ａ６が連結される。関節Ｊ２、Ｊ３、Ｊ５は曲げ関節であり、関節Ｊ１、Ｊ４、Ｊ６はねじり関節である。関節Ｊ６には、ワークに対して把持や加工等を行うための各種のエンドエフェクター２が装着される。先端の関節Ｊ６の回転軸上の所定位置をツールセンターポイント（ＴＣＰ）と表す。ＴＣＰの位置は各種のエンドエフェクター２の位置の基準となる。アームＡとエンドエフェクター２とは、本発明のマニピュレーターの構成例である。
【００１３】
関節Ｊ６には力覚センサーＦＳが備えられている。力覚センサーＦＳは、６軸の力検出器である。力覚センサーＦＳは、固有の座標系であるセンサー座標系において互いに直交する３個の検出軸と平行な力の大きさと、当該３個の検出軸まわりのトルクの大きさとを検出する。なお、力覚センサーＦＳは本発明の力検出器の構成例であるが、関節Ｊ６以外の関節Ｊ１〜Ｊ５のいずれか１つ以上に力検出器としての力覚センサーを備えても良い。
【００１４】
ロボット１が設置された空間を規定する座標系をロボット座標系というとき、ロボット座標系は、水平面上において互いに直交するＸ軸とＹ軸と、鉛直上向きを正方向とするＺ軸とによって規定される３次元の直交座標系である。Ｚ軸における負の方向は概ね重力方向と一致する。またＸ軸周りの回転角をＲＸで表し、Ｙ軸周りの回転角をＲＹで表し、Ｚ軸周りの回転角をＲＺで表す。Ｘ，Ｙ，Ｚ方向の位置により３次元空間における任意の位置を表現でき、ＲＸ，ＲＹ，ＲＺ方向の回転角により３次元空間における任意の姿勢を表現できる。以下、位置と表記した場合、姿勢も意味し得ることとする。また、力と表記した場合、トルクも意味し得ることとする。制御装置３は、アームＡを駆動することによって、ロボット座標系においてＴＣＰの位置を制御する。
【００１５】
図２は、ロボットシステムのブロック図である。制御部３２はロボット１の制御を行うための制御プログラムがインストールされたコンピューターである。制御部３２は、プロセッサーやＲＡＭやＲＯＭを備え、これらのハードウェア資源がプログラムと協働することによりロボット１を制御する。
【００１６】
ロボット１は、図１に示した構成のほかに、アクチュエーターとしてのモーターＭ１〜Ｍ６と、センサーとしてのエンコーダーＥ１〜Ｅ６とを備える。アームＡを制御することはモーターＭ１〜Ｍ６を制御することを意味する。モーターＭ１〜Ｍ６とエンコーダーＥ１〜Ｅ６とは、関節Ｊ１〜Ｊ６のそれぞれに対応して備えられており、エンコーダーＥ１〜Ｅ６はモーターＭ１〜Ｍ６の回転角度を検出する。制御装置３は、モーターＭ１〜Ｍ６の回転角度の組み合わせと、ロボット座標系におけるＴＣＰの位置との対応関係Ｕ１を記憶している。また、制御装置３は、ロボット１が行う作業の工程ごとに目標位置Ｓtと目標力ｆStの少なくともいずれかをコマンドに基づいて記憶する。目標位置Ｓtと目標力ｆStを引数（パラメーター）とするコマンドはロボット１が行う作業の工程ごとに設定される。
【００１７】
制御装置３は、設定された目標位置と目標力とがＴＣＰにて実現されるように、コマンドに基づいてアームＡを制御する。目標力とは、アームＡの動作に応じて力覚センサーＦＳが検出すべき力である。ここでＳの文字は、ロボット座標系を規定する軸の方向（Ｘ，Ｙ，Ｚ，ＲＸ，ＲＹ，ＲＺ）のなかのいずれか１個の方向を表すこととする。また、Ｓは、Ｓ方向の位置も表すこととする。例えば、Ｓ＝Ｘの場合、ロボット座標系にて設定された目標位置のＸ方向成分がＳt＝Ｘtと表記され、目標力のＸ方向成分がｆSt＝ｆXtと表記される。
【００１８】
制御装置３は、モーターＭ１〜Ｍ６の回転角度Ｄaを取得すると、対応関係Ｕ１に基づいて、当該回転角度Ｄaをロボット座標系におけるＴＣＰの位置Ｓ（Ｘ，Ｙ，Ｚ，ＲＸ，ＲＹ，ＲＺ）に変換する。また制御装置３は、ＴＣＰの位置Ｓと、力覚センサーＦＳの検出値とに基づいて、力覚センサーＦＳに現実に作用している作用力ｆSをロボット座標系において特定する。作用力ｆｓの作用点は、ＴＣＰとは別に原点Ｏとして定義される。原点Ｏは、力覚センサーＦＳが力を検出している点に対応する。なお制御装置３は、ロボット座標系におけるＴＣＰの位置Ｓごとに、力覚センサーＦＳのセンサー座標系における検出軸の方向を規定した対応関係Ｕ２を記憶している。従って、制御装置３は、ロボット座標系におけるＴＣＰの位置Ｓと対応関係Ｕ２とに基づいて、ロボット座標系における作用力ｆSを特定できる。また、ロボットに作用するトルクは、作用力ｆSと、ツール接触点（エンドエフェクター２とワークの接触点）から力覚センサーＦＳまでの距離とから算出することができ、図示されないfsトルク成分として特定される。
【００１９】
制御装置３は、作用力ｆSに対して重力補償を行う。重力補償とは、作用力ｆSから重力に起因する力やトルクの成分を除去することである。重力補償を行った作用力ｆSは、エンドエフェクター２に作用している重力以外の力と見なすことができる。
【００２０】
本実施例のインピーダンス制御は、仮想の機械的インピーダンスをモーターＭ１〜Ｍ６によって実現する能動インピーダンス制御である。制御装置３は、このようなインピーダンス制御を、ワークの嵌合作業、研磨作業など、エンドエフェクター２が対象物（ワーク）から力を受ける接触状態の工程で適用する。インピーダンス制御では、目標力を後述する運動方程式に代入してモーターＭ１〜Ｍ６の回転角度を導出する。制御装置３がモーターＭ１〜Ｍ６を制御する信号は、ＰＷＭ（Pulse Width Modulation）変調された信号である。運動方程式に基づいて目標力から回転角度を導出してモーターＭ１〜Ｍ６を制御するモードを力制御モードというものとする。また制御装置３は、エンドエフェクター２がワークから力を受けない非接触状態の工程では、目標位置から線形演算で導出する回転角度でモーターＭ１〜Ｍ６を制御する。目標位置から線形演算で導出する回転角度でモーターＭ１〜Ｍ６を制御するモードを位置制御モードというものとする。また制御装置３は、目標位置から線形演算で導出する回転角度と目標力を運動方程式に代入して導出する回転角度とを例えば線型結合によって統合し、統合した回転角度でモーターＭ１〜Ｍ６を制御するハイブリッドモードでもロボット１を制御する。制御装置３は、位置制御モードと力制御モードとハイブリッドモードを力覚センサーＦＳまたはエンコーダーＥ１〜Ｒ６の検出値に基づいて自律的に切り替えることもできるし、コマンドに応じて位置制御モードと力制御モードとハイブリッドモードを切り替えることもできる。以上の構成より制御装置３は、エンドエフェクター２が目標の位置において目標の姿勢となり、かつ、エンドエフェクター２に目標の力とモーメントとが作用するようにアームＡを駆動することができる。
【００２１】
制御装置３は、目標力ｆStと作用力ｆSとをインピーダンス制御の運動方程式に代入することにより、力由来補正量ΔＳを特定する。力由来補正量ΔＳとは、ＴＣＰが機械的インピーダンスを受けた場合に、目標力ｆStとの力偏差ΔｆS（ｔ）を解消するために、ＴＣＰが移動すべき位置Ｓの大きさを意味する。下記の（１）式は、インピーダンス制御の運動方程式である。
【数１】
0006668629.tif 000002
【００２２】
（１）式の左辺は、ＴＣＰの位置Ｓの２階微分値に仮想慣性係数ｍを乗算した第１項と、ＴＣＰの位置Ｓの微分値に仮想粘性係数ｄを乗算した第２項と、ＴＣＰの位置Ｓに仮想弾性係数ｋを乗算した第３項とによって構成される。（１）式の右辺は、目標力ｆStから現実の力ｆを減算した力偏差ΔｆS（ｔ）によって構成される。（１）式における微分とは、時間による微分を意味する。ロボット１が行う工程において、目標力ｆStとして一定値が設定される場合もあるし、目標力ｆStとして時間の関数が設定される場合もある。
【００２３】
仮想慣性係数ｍはＴＣＰが仮想的に有する質量を意味し、仮想粘性係数ｄはＴＣＰが仮想的に受ける粘性抵抗を意味し、仮想弾性係数ｋはＴＣＰが仮想的に受ける弾性力のバネ定数を意味する。各係数ｍ，ｄ，ｋは方向ごとに異なる値に設定されてもよいし、方向に拘わらず共通の値に設定されてもよい。
【００２４】
そして、制御装置３は、対応関係Ｕ１に基づいて、ロボット座標系を規定する各軸の方向の動作位置を、各モーターＭ１〜Ｍ６の目標の回転角度である目標角度Ｄtに変換する。そして、制御装置３は、目標角度ＤtからモーターＭ１〜Ｍ６の現実の回転角度であるエンコーダーＥ１〜Ｅ６の出力Ｄaを減算することにより、駆動位置偏差Ｄe（＝Ｄt−Ｄa）を算出する。そして、制御装置３は、駆動位置偏差Ｄeに位置制御ゲインＫpを乗算した値と、現実の回転角度Ｄaの時間微分値である駆動速度との差である駆動速度偏差に、速度制御ゲインＫvを乗算した値とを加算することにより、制御量Ｄcを導出する。なお、位置制御ゲインＫpおよび速度制御ゲインＫvは、比例成分だけでなく微分成分や積分成分にかかる制御ゲインを含んでもよい。制御量Ｄcは、モーターＭ１〜Ｍ６のそれぞれについて特定される。以上説明した構成により、制御装置３は、目標力ｆStとに基づいてアームＡを力制御モードで制御することができる。ハイブリッドモードでは、制御装置３は、目標位置Ｓtに、力由来補正量ΔＳを加算することにより動作位置（Ｓt＋ΔＳ）を特定する。
【００２５】
教示装置４には、制御装置３に目標位置Ｓtと目標力ｆStとを引数とする実行プログラムを生成して制御装置３にロードするための教示プログラムがインストールされている。教示装置４は、ディスプレイ４３やプロセッサーやＲＡＭやＲＯＭを備え、これらのハードウェア資源が教示プログラムと協働して実行プログラムを生成する。
【００２６】
（２）実行プログラム
実行プログラムは予め定められたプログラム言語によって記述され、翻訳プログラムにより中間言語を経て機械語プログラムに変換される。制御部３２のＣＰＵはクロックサイクルで機械語プログラムを実行する。翻訳プログラムは教示装置４で実行しても良いし、制御装置３で実行しても良い。実行プログラムのコマンドは本体と引数とから構成される。コマンドには、アームＡとエンドエフェクター２を動作させる動作制御コマンド、エンコーダーやセンサーの検出値を読み出すモニターコマンド、各種の変数を設定する設定コマンド等が含まれる。なお、本明細書において、コマンドの実行は、当該コマンドが翻訳された機械語プログラムの実行と同義である。
【００２７】
図３に動作制御コマンド（本体）の一例を示す。図３に示すように動作制御コマンドは、力制御モードでアームＡを動作させることができる力制御対応コマンドと、力制御モードでアームＡを動作させることができない位置制御コマンドとを含む。力制御対応コマンドでは、引数により力制御モードのオンを指定できる。当該引数により力制御モードのオンが指定されない場合には、位置制御モードで当該力制御対応コマンドが実行され、当該引数により力制御モードのオンが指定される場合には、当該力制御対応コマンドは力制御モードで実行される。また力制御対応コマンドは力制御モードにおいて実行可能であり、位置制御コマンドは力制御モードでは実行不能である。力制御モードにおいて位置制御コマンドが実行されることがないように、翻訳プログラムによる構文エラーチェックが実行される。さらに、力制御対応コマンドでは、引数により力制御モードの継続を指定できる。力制御モードで実行される力制御対応コマンドにおいて当該引数により力制御モードの継続が指定された場合、力制御モードは継続され、当該引数により力制御モードの継続が指定されない場合、当該力制御対応コマンドの実行完了までに力制御モードは終了する。すなわち力制御対応コマンドが力制御モードで実行されるとしても、引数により明示的に指定されない限り、力制御モードは当該力制御対応コマンドに応じて自律的に終了し、当該力制御対応コマンドの実行終了後においてまで力制御モードが継続することはない。なお図３において「ＣＰ」は移動方向を指定できるコマンドの分類、「ＰＴＰ」は目標位置を指定できるコマンドの分類、「ＣＰ＋ＰＴＰ」は移動方向と目標位置を指定できるコマンドの分類である。
【００２８】
ここで、引数により力制御モードのオンが指定され、かつ、力制御モードの継続が指定されていない「Move」コマンドが力制御モードでない状態において実行開始される場合を例にして、力制御対応コマンドの実行手順、すなわち制御部３２の動作順序について図４を参照しながら説明する。
【００２９】
まずはじめに制御部３２は、力制御モードのオンを指定する引数があるか無いかを判定する（Ｓ１０２）。
力制御モードのオンを指定する引数がある場合、制御部３２は、力覚センサーＦＳをリセットする（Ｓ１１２）。力覚センサーＦＳのリセットは、力覚センサーＦＳの出力が、このリセットタイミングでゼロとして検出されるように力覚センサーＦＳ自体または制御部３２が記憶するオフセット値を設定することである。
【００３０】
続いて制御部３２は、力制御モードに遷移してインピーダンス制御を実行し、アームＡを動作させる（Ｓ１１４）。すなわち制御部３２は、力覚センサーＦＳの出力に基づいてモーターＭ１〜Ｍ６の回転角度を制御する。ここで目標力は、力制御対応コマンドの引数として指定されていてもよいし、力制御対応コマンドより先に実行される設定コマンドで指定されていてもよい。位置制御モードから力制御モードへの切換は、例えば図２に示すΔＳとＳｔとを足し合わせる算術要素においてΔＳの入力をオンにしてＳｔの入力をオフ（ゼロ）にする操作によって実施される。
【００３１】
続いて制御部３２は、力制御モードを終了する（Ｓ１１６）。力制御モードから位置制御モードへの切換は、例えば図２に示すΔＳとＳｔとを足し合わせる算術要素においてΔＳの入力をオフ（ゼロ）にしてＳｔの入力をオンにする操作によって実施される。
【００３２】
力制御モードのオンを指定する引数が無い場合、制御部３２は位置制御モードにおいて位置制御を実行し、アームＡを動作させる（Ｓ１２０）。すなわち制御部３２は、目標位置から線形演算で導出する回転角度でモーターＭ１〜Ｍ６の回転角度を制御する。
【００３３】
ところで水晶を圧電素子として用いる力覚センサーでは、力覚センサーに加わる力に変化が無くても、図５Ａに示すように、時間の経過に伴って出力が大きくなる傾向にある。したがって、例えば図５Ｂに示す時間ｔ０で力制御モードがオンになるとすると、同じ力が加わっているにもかかわらず、ｔ０からｔ１が経過した時点で検出される力ｆ１と、ｔ０からｔ２が経過した時点で検出される力ｆ２とが異なる結果となる。本実施例では、上述したように力制御対応コマンドの実行時点で力制御モードに遷移して力覚センサーＦＳがリセットされるため、力制御対応コマンドが実行される時点を出力の基準とする力覚センサーＦＳの出力に基づいてアームＡの動作を制御することができる。このためアームＡに対する力制御の再現性を高めることができる。例えば、動作制御コマンドに対して力覚センサーＦＳのリセットが同期しない場合、すなわち動作制御コマンドの実行に先行して力覚センサーＦＳがリセットされる場合、動作制御コマンドの実行が３ｍｓｅｃ程度（ｔ２−ｔ１）遅れることになる。力制御が１０ｍｓｅｃ間隔で実行されるとすると、この３ｍｓｅｃは力制御の間隔に対し大きな実行の遅れであり再現性を悪化させる。
【００３４】
実行プログラムのコマンドの引数は階層化されたオブジェクト構造となっている。力制御コマンドに関連する引数のクラスを図６に示す。「Force Coordinate System Object」は、座標系の設定に用いられるクラスであって、ＦＣＳｘ（ｘは任意の整数）で定義することができる。「Force Control Object」は、力制御の対象となる座標軸等の設定に用いられるクラスであって、ＦＣｘ（ｘは任意の整数）で定義することができる。「Force Trigger Object」は、分岐条件、停止条件、割り込みの設定に用いられるクラスであって、ＦＴｘ（ｘは任意の整数）で定義することができる。「Force Monitor Object」は、力データの取得、ログ記録等の設定に用いられるクラスであって、ＦＭｘ（ｘは任意の整数）で定義することができる。「Force Control Object」、「Force Trigger Object」および「Force Monitor Object」で指定される座標系は「Force Coordinate System Object」で定義された座標系となる。すなわち、「Force Control Object」、「Force Trigger Object」および「Force Monitor Object」は、上位層クラスの「Force Coordinate System Object」に従属する下位層クラスである。「Force Sensor Object」は、力覚センサーＦＳの制御と力覚センサーＦＳからの情報取得の設定に用いるクラスであって、ＦＳｘ（ｘは任意の整数）で定義することができる。「Mass Properties Object」は、重心位置の設定に用いるクラスであって、ＭＰｘ（ｘは任意の整数）で定義することができる。「Robot Object」は、力制御と関連したロボット１の情報を取得するための設定に用いるクラスであって、Ｒｏｂｏｔｘ（ｘは任意の整数）で定義することができる。
【００３５】
次に、このように階層化された引数の設定方法について説明する。図７Ａは「Force Coordinate System Object」クラスの引数オブジェクトを設定する画面の構成例である。図７Ａは、「ハンド面３先端」というラベルが付された「力覚座標４」（プログラムコードではＦＣＳ４と記述される引数オブジェクト）の原点のＴＣＰを基準とするロボット座標系における相対的な位置と、ＴＣＰの姿勢（ＴＣＰに固定された座標系）を基準とする相対的な姿勢とを設定した状態を示している。ここで制御部３２は、モーターＭ１〜Ｍ６の回転角度の組み合わせと、ロボット座標系におけるＴＣＰの位置との対応関係Ｕ１を記憶しているため、「Force Coordinate System Object」クラスの引数オブジェクトにおいてＴＣＰに対する相対的な位置と姿勢を設定することにより、基準座標系に対して任意の座標系を定義することができる。
【００３６】
図７Ｂは、「Force Control Object」クラスの引数オブジェクトを設定する画面の構成例である。図７Ｂは、「姿勢倣い」というラベルが付された「力制御２」（プログラムコードではＦＣ２と記述される引数オブジェクト）の座標系と、力制御の有効軸と、インピーダンス制御の運動方程式の係数（仮想バネ係数、仮想粘性係数、仮想質量係数）とを設定した状態を示している。ここで座標系として「４」が選択されているため、図７Ａの画面で設定された「力覚座標４」（ＦＣＳ４）の座標系の各軸について、「有効」の項目において力制御のオン（True）とオフ（False）が設定される。すなわち、力制御が有効になる方向は、上位の「Force Coordinate System Object」クラスのオブジェクトの設定に依存する。図７Ｃは「Force Trigger Object」クラスの引数オブジェクトを設定する画面の構成例である。図７Ｃにおいても、「接触検知」というラベルが付された「力トリガー１」（プログラムコードではＦＴ１と記述される引数オブジェクト）の座標系として「４」が選択されているため、図７Ａの画面で設定された「力覚座標４」（ＦＣＳ４）の座標系の各軸について処理条件が定められる。すなわち、停止条件の各変数が示す値やベクトルの方向は、上位の「Force Coordinate System Object」クラスのオブジェクトの設定に依存する。このように本実施例のコマンドの引数は階層化されているため、「Force Coordinate System Object」クラスのオブジェクトとして１つの座標系を設定すれば、その座標系を様々なコマンドに適用することができ、設定作業が容易になる。
【００３７】
力制御対応コマンドを用いたプログラムコードの実施例を比較例とともに図８に示す。図８に示す実施例は、「ＦＣ１」という引数オブジェクトが６つの設定コマンド「Ｆｓｅｔ」で定義された後に、力制御対応コマンド「Ｍｏｖｅ」が実行され、その後に「ＦＣ１」オブジェクトの変数の１つである「Fz_TargetForce」が設定コマンド「Ｆｓｅｔ」で変更されるプログラムコードである。
【００３８】
図８に示す比較例では、コマンドの引数が階層化されておらず、力制御モードで用いられる全ての設定項目が「ForceControlSetting」コマンドにより一度に設定される。そして、カンマで区切られたそれぞれの引数の定義は、コマンド本体と引数の間に記述するカンマの数で決まる。したがって比較例のプログラムコードの意味を即座に理解することは困難である。これに対し、本発明の実施例では、力制御対応コマンドに関連する引数は、階層化されたオブジェクト毎に「Fset」コマンドにより設定されるため、プログラムコードの意味を解釈することも、プログラムコードを記述することもはるかに容易である。
【００３９】
本実施例のコマンド「Move P1 FC1 CF」では、「Fset」コマンドにより設定された「ＦＣ１」オブジェクトの他に、図示しない設定コマンドにより設定された「Ｐ１」オブジェクトと、「ＣＦ」が引数として設定されている。ここで「Ｐ１」は、接触位置を指定する引数である。「ＣＦ」は、コマンドの実行終了後も力制御モードの継続を指定する引数である。コマンド「Move P1 FC1 CF」は、「Force Control Object」クラスの引数オブジェクト「ＦＣ１」が指定されており、目標位置が引数「Ｐ１」で指定されているため、コマンド「Move P1 FC1 CF」が実行されると、力覚センサーＦＳがリセットされ、力覚センサーＦＳの出力に基づくハイブリッドモードでアームＡが制御される。また引数「ＣＦ」が指定されているため、コマンドの実行終了後も力制御モードが継続する。
【００４０】
比較例では、「ForceControl On」コマンドの実行により、力覚センサーＦＳがリセットされ、力覚センサーＦＳの出力に基づいてアームＡを制御可能な状態となる。続いて「Move P1」コマンドの実行により、力覚センサーＦＳの出力に基づいてアームＡが制御される。これに対して、本実施例のコマンド「Move P1 FC1 CF」の実行が開始されてから力覚センサーＦＳがリセットされるまでの実時間は、対応する機械語プログラムが常に同じになるため、制御部３２のクロック数によって予め定まる一定の時間となる。したがって上述したように、本実施例では力制御の再現性が保証される。
【００４１】
（３）他の実施形態
本発明の技術的範囲は上述した実施例に限定されるものではなく、本発明の要旨を逸脱しない限りにおいて変更を加え得ることは勿論である。例えば、上記実施例ではコマンドの引数で力制御モードのオンを指定できたが、コマンドの本体で力制御モードのオンを指定できてもよい。具体的にはコマンドの本体自体が、力制御モードをオンするものと、力制御モードをオンしないものとに分かれているプログラム言語をロボットシステムに実装しても良い。
【００４２】
また力制御対応コマンドにより力覚センサーをリセットしてから力制御によりアームを動作させるまでの時間をタイマーで制御しても良い。すなわち、力制御対応コマンドの実行手順にタイマーのセットを組み込んでも良い。
また力検出器の出力に基づいてマニピュレーターの動作を制御する例としてインピーダンス制御を取り上げたが、運動方程式を用いずに、例えば力検出器の出力に対して線形な制御量でマニピュレーターの動作を制御しても良い。
【符号の説明】
【００４３】
１…ロボット、２…エンドエフェクター、３…制御装置、４…教示装置、５…ティーチングペンダント、３１…電源部、３２…制御部、４３…ディスプレイ、Ａ…アーム、Ａ１-Ａ６…アーム部材、Ｅ１-Ｅ６…エンコーダー、ＦＳ…力覚センサー、Ｊ１-Ｊ６…関節、Ｍ１-Ｍ６…モーター
【図１】
0006668629.tif 000003
【図２】
0006668629.tif 000004
【図３】
0006668629.tif 000005
【図４】
0006668629.tif 000006
【図５】
0006668629.tif 000007
【図６】
0006668629.tif 000008
【図７】
0006668629.tif 000009
【図８】
0006668629.tif 000010