(57)【特許請求の範囲】
【請求項１】
マニピュレータのハンドで把持されたワークを対象物に対して所定関係に置くロボットにおいて、
前記ハンドの位置および力を制御するマニピュレ−タ制御部と、
ワークが対象物と所定関係に置かれたか否かを判定する接触状態検出部と、
予め入力された命令を保持するとともにこの命令を解釈し、前記接触状態検出部による判定結果を参照して前記マニピュレ−タ制御部の制御係数を演算して前記マニピュレ−タ制御部へ転送する命令保持実行部と
を備え、
前記命令は、ワークを直線移動させ対象物と接触させる接触命令を含み、
前記制御係数は、前記接触命令においてはこの命令の中で指定された方向へワークを指定された速度で移動し、対象物と接触後には指定された力またはトルクで押しつけることにより接触を維持するための接触命令制御係数を含み、
前記接触状態検出部は、前記接触命令においてはこの命令の中で指定された接触位置と実際の位置との差およびこの命令の中で指定された許容位置誤差の値、この命令の中で指定された押しつけ力と実際の力との差、およびマニピュレータ先端位置の一定時間内の変動量とにより目標の接触が実現された否かの判定を行う、
ことを特徴とするロボット。
【請求項２】
マニピュレータのハンドで把持されたワークを対象物に対して所定関係に置くロボットにおいて、
前記ハンドの位置および力を制御するマニピュレ−タ制御部と、
ワークが対象物と所定関係に置かれたか否かを判定する接触状態検出部と、
予め入力された命令を保持するとともにこの命令を解釈し、前記接触状態検出部による判定結果を参照して前記マニピュレ−タ制御部の制御係数を演算して前記マニピュレ−タ制御部へ転送する命令保持実行部と
を備え、
前記命令は、ワークを対象物に接触させた状態で回転運動させワークを対象物に稜線接触させるならい命令を含み、
前記制御係数は、前記ならい命令においてはこの命令の中で指定された方向へワークを回転させ、対象物との稜線接触を実現させて維持するためのならい命令制御係数を含み、
前記接触状態検出部は、前記ならい命令においてはこの命令の中で指定された接触位置と実際の位置との差およびこの命令の中で指定された許容位置誤差の値、この命令の中で指定された回転軸まわりのモーメントの値、およびマニピュレータ先端位置の一定時間内の変動量とにより目標の接触が実現されたか否かの判定を行う、
ことを特徴とするロボット。
【請求項３】
マニピュレータのハンドで把持されたワークを対象物に対して所定関係に置くロボットにおいて、
前記ハンドの位置および力を制御するマニピュレ−タ制御部と、
ワークが対象物と所定関係に置かれたか否かを判定する接触状態検出部と、
予め入力された命令を保持するとともにこの命令を解釈し、前記接触状態検出部による判定結果を参照して前記マニピュレ−タ制御部の制御係数を演算して前記マニピュレ−タ制御部へ転送する命令保持実行部と
を備え、
前記命令は、ワークを対象物と接触させた状態で回転運動させワークの面と対象物の面とを拘束状態にするはめ合い命令を含み、
前記制御係数は、前記はめあい命令においてはこの命令の中で指定された方向へ指定された角速度でワークを回転させ、ワークの面と対象物の面との拘束状態を実現させて維持するためのはめあい命令制御係数を含み、
前記接触状態検出部は、前記はめ合い命令においてはこの命令の中で指定された接触位置と実際の位置との差およびこの命令の中で指定された許容位置誤差の値、この命令の中で指定された回転軸まわりのモーメントと実際のモーメントとの差、マニピュレータ先端位置の一定時間内の変動量、およびこの命令の中で指定されたはめあいの結果移動可能となる方向の力の値とにより目標の拘束状態が実現されたか否かの判定を行う、
ことを特徴とするロボット。
【請求項４】
ロボットの作業環境およびロボット自身の３次元の幾何情報を有するとともに、ワークと対象物との所定の位置関係と実際の位置関係との間の誤差として起こり得る最大値の情報を有する環境モデル部と、
前記接触命令の上位の上位接触命令を解釈可能なユーザインタフェース部と、
前記上位接触命令においてはこの命令の中で指定された方向に移動したときにこの命令の中で指定されたワークの面と対象物との接触が起きる位置を前記環境モデル内部の値を利用することにより導出し、前記環境モデル部に含まれる位置誤差の値および前記上位接触命令に含まれる引き数により引き数の値を生成して前記接触命令を前記命令保持実行部へ転送する上位命令翻訳部と、
をさらに備えることを特徴とする請求項１に記載のロボット。
【請求項５】
ロボットの作業環境およびロボット自身の３次元の幾何情報を有するとともに、ワークと対象物との所定の位置関係と実際の位置関係との間の誤差として起こり得る最大値の情報を有する環境モデル部と、
前記ならい命令の上位にある上位ならい命令を解釈可能なユーザインタフェース部と、
前記上位ならい命令においてはこの命令で指定された方向へワークを回転させたときにこの命令の中で指定された面と稜線との接触が起きる姿勢を前記環境モデル内部の値を利用することにより算出し、前記環境モデル部に含まれる位置誤差の値および前記上位ならい命令に含まれる引き数により引き数の値を生成して前記ならい命令を前記命令保持・実行部へ転送する上位命令翻訳部と、
をさらに備えることを特徴とする請求項２に記載のロボット。
【請求項６】
ロボットの作業環境およびロボット自身の３次元の幾何情報を有するとともに、ワークと対象物との所定の位置関係と実際の位置関係との間の誤差として起こり得る最大値の情報を有する環境モデル部と、
前記はめあい命令の上位にある上位はめ合い命令を解釈可能なユーザインタフェース部と、
前記上位はめ合い命令においてはこの命令の中で指定された方向へ回転したときにこの命令の中で指定されたワークの面と対象物の面のふたつの面が平行になる姿勢の値を算出するとともに、前記上位はめ合い命令が実行される前に前記接触命令が実行されている方向の中から命令の中で指定されたふたつの面の方向から移動可能な方向を求め、前記環境モデル部に含まれる位置誤差の値および前記上位はめ合い命令に含まれる引き数により引き数の値を生成して前記はめ合い命令を前記命令保持実行部へ転送する上位命令翻訳部と、
をさらに備えることを特徴とする請求項３に記載のロボット。
【発明の詳細な説明】
【０００１】
〔発明の目的〕
【産業上の利用分野】
本発明は、ハンドに加わる力／モーメントを検出する力覚センサおよびロボットの位置・力を制御する制御装置を有するロボットに関する。
【０００２】
【従来の技術】
産業用ロボットの製造工場への導入意欲は衰えることを知らず、特に近年は電子部品の組み立てのロボット化が盛んになってきている。組み立て作業に限らず、産業用のロボットでは部品供給装置により部品を正確に位置決めすることにより、ロボットはあらかじめ教示しておいた動作を毎回繰り返すだけで作業を実行することを可能にしている。また、ロボットの先端に加わる力／トルクを計測する小型の力覚センサの応用が広まるにつれて、コンプライアンス制御によりロボットの位置決め精度以上の高精度なはめ合いを実現している例など、ロボットの位置だけでなく力を制御することにより高精度な作業を実現している例が見られるようになってきた。
【０００３】
一般に、ロボットの力を制御する方法として、力制御、コンプライアンス制御、インピーダンス制御、ダンピング制御、ハイブリッド制御等が代表的である。
【０００４】
また特開平４−１４１３８６号記載の従来技術では、凸嵌合部と凹嵌合部の嵌合およびボルト・ナットの嵌合・締結を１回の試行で確実に実施するための、力の制御方法、状態の検出方法および作業手順について記載されている。
【０００５】
特開平４−１４１３８７号記載の従来技術では、トラス構造物を組み立てる際に、力覚センサが受ける力に基づき慣性項とバネ項を除いた粘性項からなる運動方程式を立ててハンドの速度を算出してハンドの位置制御を行うという、ダンピング制御により組み立て作業を行うことにより少ない計算量で組み立て作業を行うことを可能にしている。
【０００６】
また、日本ロボット学会誌Ｖｏｌ．８，  Ｎｏ．３  に記載の「スキルに基づくマニピュレーションシステム」では、マニピュレータによる複雑な作業を実現するための基本的な動作としてスキルを定義し、各スキルで力制御やセンサの利用により計算機内部のデータと実環境との違いも吸収して確実な動作を実現している。
【０００７】
【発明が解決しようとする課題】
ところが、コンプライアンス制御やインピーダンス制御では、目標位置と実際の部品の位置が大きな誤差を有している場合には過大な力を部品に与えてしまったり、作業ごとに適したバネ・粘性・質量等の係数の値の設定が難しい等の問題があるとともに、はめ合い作業においては凸嵌合部が凹嵌合部と直接内側外側の接触状態に誘導できないと作業が成功しないという限界が存在した。また、作業実行時に部品との接触状態を検出する方法が確立されていないために、複雑な作業を一連の作業の組み合わせにより実現する場合には作業の切り替え時の判定が困難であるだけでなく、作業が失敗したことの検出もかならずしも容易ではなかった。特開平４−１４１３８６号記載の従来技術のように特定の作業に限定して、その作業を実現するための一連の作業手順を規定し、それぞれの作業の実現の検出方法を考案した例はあるものの、限定した作業にのみ適用できる方法にすぎなかった。
【０００８】
日本ロボット学会誌Ｖｏｌ．８，  Ｎｏ．３  に記載の「スキルに基づくマニピュレーションシステム」では、複雑な作業をその組み合わせで実現するための基本的の作業としてのスキルを定義し、その実現方法および実現の検出方法・接触状態の変化の検出方法についても考案されている。しかし、本文献に記載されている接触状態の検出方法はマニピュレータ先端の移動量の変化を瞬間回転中心の変化として検出するものであり、本文献中の実験で用いられたような減速器を使用しないダイレクトドライブのマニピュレータではマニピュレータ先端の絶対位置が正確に計測・制御できるために有効であるが、一般に使用される減速器を使用したマニピュレータにおいては対象物との接触・衝突により加わる力により比較的剛性の低い減速器部でのたわみが生じるために本文献にあるような瞬間回転中心の変化を正確に検出することは必ずしも容易ではない。
【０００９】
そこで、本発明の目的は、治具や部品供給装置により十分な精度で位置決めされていない場合や、治具や部品供給装置で位置決めされてはいるが現場でのロボットの据え付け誤差やロボットの関節部でのたわみなどによる誤差により現場での教示やキャリブレーションを行わないとオフラインの教示による位置制御のみでは作業の実行が不可能な場合や、ロボットの精度以上に高精度なはめ合い作業などを行う場合において、現場での教示やキャリブレーションを行うことなく、オフラインの教示のみにより確実な動作で作業を実行することができるロボットを提供することである。
【００１０】
【課題を解決するための手段】
上記目的を達成するために、本発明によるロボットは、マニピュレータのハンドで把持されたワークを対象物に対して所定関係に置くロボットにおいて、前記ハンドの位置および力を制御するマニピュレ−タ制御部と、ワークが対象物と所定関係に置かれたか否かを判定する接触状態検出部と、予め入力された命令を保持するとともにこの命令を解釈し、前記接触状態検出部による判定結果を参照して前記マニピュレ−タ制御部の制御係数を演算して前記マニピュレ−タ制御部へ転送する命令保持実行部とを備え、前記命令は、ワークを直線移動させ対象物と接触させる接触命令を含み、前記制御係数は、前記接触命令においてはこの命令の中で指定された方向へワークを指定された速度で移動し、対象物と接触後には指定された力またはトルクで押しつけることにより接触を維持するための接触命令制御係数を含み、前記接触状態検出部は、前記接触命令においてはこの命令の中で指定された接触位置と実際の位置との差およびこの命令の中で指定された許容位置誤差の値、この命令の中で指定された押しつけ力と実際の力との差、およびマニピュレータ先端位置の一定時間内の変動量とにより目標の接触が実現された否かの判定を行う、ことを特徴とする。
【００１１】
また、マニピュレータのハンドで把持されたワークを対象物に対して所定関係に置くロボットにおいて、前記ハンドの位置および力を制御するマニピュレ−タ制御部と、ワークが対象物と所定関係に置かれたか否かを判定する接触状態検出部と、予め入力された命令を保持するとともにこの命令を解釈し、前記接触状態検出部による判定結果を参照して前記マニピュレ−タ制御部の制御係数を演算して前記マニピュレ−タ制御部へ転送する命令保持実行部とを備え、前記命令は、ワークを対象物に接触させた状態で回転運動させワークを対象物に稜線接触させるならい命令を含み、前記制御係数は、前記ならい命令においてはこの命令の中で指定された方向へワークを回転させ、対象物との稜線接触を実現させて維持するためのならい命令制御係数を含み、前記接触状態検出部は、前記ならい命令においてはこの命令の中で指定された接触位置と実際の位置との差およびこの命令の中で指定された許容位置誤差の値、この命令の中で指定された回転軸まわりのモーメントの値、およびマニピュレータ先端位置の一定時間内の変動量とにより目標の接触が実現されたか否かの判定を行う、ことを特徴とする。
また、マニピュレータのハンドで把持されたワークを対象物に対して所定関係に置くロボットにおいて、前記ハンドの位置および力を制御するマニピュレ−タ制御部と、ワークが対象物と所定関係に置かれたか否かを判定する接触状態検出部と、予め入力された命令を保持するとともにこの命令を解釈し、前記接触状態検出部による判定結果を参照して前記マニピュレ−タ制御部の制御係数を演算して前記マニピュレ−タ制御部へ転送する命令保持実行部とを備え、前記命令は、ワークを対象物と接触させた状態で回転運動させワークの面と対象物の面とを拘束状態にするはめ合い命令を含み、前記制御係数は、前記はめあい命令においてはこの命令の中で指定された方向へ指定された角速度でワークを回転させ、ワークの面と対象物の面との拘束状態を実現させて維持するためのはめあい命令制御係数を含み、前記接触状態検出部は、前記はめ合い命令においてはこの命令の中で指定された接触位置と実際の位置との差およびこの命令の中で指定された許容位置誤差の値、この命令の中で指定された回転軸まわりのモーメントと実際のモーメントとの差、マニピュレータ先端位置の一定時間内の変動量、およびこの命令の中で指定されたはめあいの結果移動可能となる方向の力の値とにより目標の拘束状態が実現されたか否かの判定を行う、ことを特徴とする。
また、ロボットの作業環境およびロボット自身の３次元の幾何情報を有するとともに、ワークと対象物との所定の位置関係と実際の位置関係との間の誤差として起こり得る最大値の情報を有する環境モデル部と、前記接触命令の上位の上位接触命令を解釈可能なユーザインタフェース部と、前記上位接触命令においてはこの命令の中で指定された方向に移動したときにこの命令の中で指定されたワークの面と対象物との接触が起きる位置を前記環境モデル内部の値を利用することにより導出し、前記環境モデル部に含まれる位置誤差の値および前記上位接触命令に含まれる引き数により引き数の値を生成して前記接触命令を前記命令保持実行部へ転送する上位命令翻訳部と、をさらに備えることを特徴とする。
また、ロボットの作業環境およびロボット自身の３次元の幾何情報を有するとともに、ワークと対象物との所定の位置関係と実際の位置関係との間の誤差として起こり得る最大値の情報を有する環境モデル部と、前記ならい命令の上位にある上位ならい命令を解釈可能なユーザインタフェース部と、前記上位ならい命令においてはこの命令で指定された方向へワークを回転させたときにこの命令の中で指定された面と稜線との接触が起きる姿勢を前記環境モデル内部の値を利用することにより算出し、前記環境モデル部に含まれる位置誤差の値および前記上位ならい命令に含まれる引き数により引き数の値を生成して前記ならい命令を前記命令保持・実行部へ転送する上位命令翻訳部と、をさらに備えることを特徴とする。
また、ロボットの作業環境およびロボット自身の３次元の幾何情報を有するとともに、ワークと対象物との所定の位置関係と実際の位置関係との間の誤差として起こり得る最大値の情報を有する環境モデル部と、前記はめあい命令の上位にある上位はめ合い命令を解釈可能なユーザインタフェース部と、前記上位はめ合い命令においてはこの命令の中で指定された方向へ回転したときにこの命令の中で指定されたワークの面と対象物の面のふたつの面が平行になる姿勢の値を算出するとともに、前記上位はめ合い命令が実行される前に前記接触命令が実行されている方向の中から命令の中で指定されたふたつの面の方向から移動可能な方向を求め、前記環境モデル部に含まれる位置誤差の値および前記上位はめ合い命令に含まれる引き数により引き数の値を生成して前記はめ合い命令を前記命令保持実行部へ転送する上位命令翻訳部と、をさらに備えることを特徴とする。
【００１２】
【作用】
本発明によれば、オペレータがあらかじめオフラインで接触命令を入力していた場合には命令保持実行部が入力された命令に従った値をマニピュレ−タ制御部へ転送することにより、マニピュレ−タは指定した方向に指定した速度で移動し、マニピュレ−タのハンドに把持されたワークと対象物との接触の後には指定した力で接触を保つ。一方、接触状態検出部が命令の中で指定された接触位置と実際の位置の差が命令の中で指定された位置誤差の値より小さくなるとともに命令の中で指定された押しつけ力と実際の力の差がデフォルトの力誤差の値より小さくなり、さらにマニピュレータ先端位置の一定時間内での変動量が基準値より小さくなりほとんど静止していることを確認してはじめて目標の接触の実現と判定し、その結果を命令保持実行部へ転送することにより、直線運動により目標とする接触が実現されたか否かを確実に判定することが可能になる。あらかじめならい命令が入力されていた場合には、命令保持実行部が入力された命令に従った値をロボットの制御部へ転送することにより、ロボットは指定された方向に回転することにより対象物との稜線接触を実現し維持する。
【００１３】
一方、接触状態検出部が命令の中で指定された接触位置と実際の位置の差が命令の中で指定された位置誤差の値より小さくなるとともに命令の中で指定された回転軸まわりのモーメントがデフォルトの力誤差の値より小さくなり、さらにマニピュレータ先端位置の一定時間内の変動量が基準値より小さくなるほとんど静止していることを確認してはじめて目標の接触の実現と判定し、その結果を命令保持実行部へ転送することにより、回転運動により目標とする接触が実現されたか否かを確実に判定することが可能になる。予めはめ合い命令が入力されていた場合には、命令保持実行部が入力された命令に従った値をロボットの制御部に転送することにより、ロボットは指定された方向に回転することにより対象物との内側外側拘束の状態を実現し維持する。一方、接触状態検出部が命令の中で指定された接触位置と実際の位置との差が命令の中で指定された位置誤差の値より小さくなるとともに命令の中で指定された回転軸まわりのモーメントが命令の中で指定されたモーメントの値とデフォルトのモーメント誤差の値より小さくなり、さらにマニピュレータ先端位置の一定時間内の変動量が基準値より小さくなりほとんど静止していることを確認するか、あるいは命令中で指定されたはめ合いの結果移動可能となる方向の力の値が一定時間以上、基準値より小さくなっていることを確認してはじめて目標の接触の実現と判定し、その結果を命令保持実行部へ転送することにより、回転運動により目標とする接触が実現されたか否かを確実に判定することが可能になる。
【００１４】
以上のような、接触命令、ならい命令、はめ合い命令により組み立て作業等で基本とする点接触の実現動作、稜線接触の実現動作、はめ合い動作の実現動作が確実に実現できるとともに、状況により動作が実現できない場合があったとしても、その状態を検出できるために、治具や部品供給装置により十分な精度で位置決めされていない場合や、治具や部品供給装置で位置決めされてはいるが現場でのロボットの据え付け誤差やロボットの関節部でのたわみなどによる誤差により現場での教示やキャリブレーションを行わないとオフラインの教示による位置制御のみでは作業の実行が不可能な場合や、ロボットの精度以上に高精度なはめ合い作業などを行う場合においても、現場での教示やキャリブレーションを行うことなく、オフラインでの教示のみにより確実な動作でさまざまな作業を実行することができるとともに、状況により対応した動作を行うことも可能となる。
【００１５】
また本発明によれば、オペレータが上位接触命令を入力した場合には、ユーザインタフェース部が上位命令翻訳部へ命令を転送し、上位命令翻訳部では、命令の内で指定された方向に移動したときに、命令の中で指定されたふたつの面の接触が起きる位置を、環境モデル内部の値を利用することにより算出し、環境モデル部に含まれる許容位置誤差の値および上位接触命令に含まれる引き数により引き数の値を生成して接触命令を命令保持実行部に転送する。オペレータが上位ならい命令を入力した場合には、やはりユーザインタフェース部が上位命令翻訳部へ命令を転送し、上位命令翻訳部では、命令の中で指定された方向に回転したときに、命令の中で指定された面と稜線との接触が起きる姿勢を環境モデル内部の値を利用することにより算出し、環境モデル部に含まれる許容位置誤差の値および上位ならい命令に含まれる引き数により引き数の値を生成してならい命令を命令保持実行部へ転送する。オペレータが上位はめ合い命令を入力した場合には、やはりユーザインタフェース部が上位命令翻訳部へ命令を転送し、上位命令翻訳部では、上位はめ合い命令のときには命令の中で指定された方向へ回転したときに命令の中で指定されたふたつの面が平行になる姿勢の値を算出するとともに、上位はめ合い命令が実行される前に接触命令が実行されている方向の中から命令の中で指定されたふたつの面の方向から移動可能な方向を求め、環境モデル部に含まれる許容位置誤差の値および上位はめ合い命令に含まれる引き数により引き数の値を生成してはめ合い命令を命令保持実行部に転送する。
【００１６】
以上の結果、上位接触命令が入力された場合には命令の中で指定された面同士の接触が実現され、上位ならい命令が入力された場合には命令の中で指定された稜線と面との稜線接触が実現され、上位はめ合い命令が入力された場合には命令の中で指定された面同士が平行になることにより実現される内側外側の拘束状態が確実に実現される。以上のような、上位接触命令、上位ならい命令、上位はめ合い命令によりオペレータはシステムの環境モデル内部で定義された物体の面や稜線の名前あるいは番号を指定することにより容易にオフラインでの作業教示を行うことが可能となる。
【００１７】
【実施例】
以下、図面を参照しながら実施例を説明する。図１は本発明の第１実施例の概略構成を示すものである。
【００１８】
この図において、ロボットは命令保持実行部１と、マニピュレ−タ制御部２と、接触状態検出部３と、マニピュレータ４とを備えている。マニピュレータ４には各関節軸に関節角度を計測するエンコーダが、また手首部にはハンドに加わる力／トルクを検出する６軸力覚センサが取り付けてある。また、マニピュレ−タ制御部２は、命令保持実行部１から転送された制御点の位置および制御点での力／トルクの値を入力とし、やはり命令保持実行部１から転送されたインピーダンス係数の値に従ってインピーダンス制御を行う。
【００１９】
図２にマニピュレ−タ制御部２の概略構成を示す。マニピュレ−タ制御部２は、命令保持実行部１からの転送された対象物座標系での目標位置および対象物座標系で表した制御点での目標力／トルクとなるように、命令保持実行部から転送されたインピーダンス係数に基づき制御する。仮想モデル算出部５では、外部から入力された目標位置・力およびインピーダンス係数をもとにバネ・質量・ダンパから構成される仮想モデルの位置・速度の計算を行い、制御点の仮想モデル目標位置・速度を算出する。なお仮想モデルはｘ，ｙ，ｚ，ロール、ピッチ、ヨーの各軸について、それぞれのインピーダンス係数をもとに独立に計算を行う。ロール・ピッチ・ヨーはそれぞれｘ軸、ｙ軸、ｚ軸まわりの回転とする。以下に仮想モデルの位置・速度の値の計算方法を示す。
【００２０】
インピーダンス係数はバネ、質量、ダンパのそれぞれについてｋ，ｍ，ｃとする。また、制御点の目標位置ｘｐ、目標力ｆｐおよび制御点換算・重力補償された力／トルクの値をｆ、仮想モデル目標位置をｘｍとする。
【００２１】
ａ）ｍが０でない場合
0003580562.tif 000002
により（ｄ２／ｄｔ２）△ｘを算出する。（１）式右辺の△ｘ、（ｄ／ｄｔ）△ｘの値はひとつ前の制御周期での値を用いる。（ｄ２／ｄｔ２）△ｘが算出されたら、数値積分により順次（ｄ／ｄｔ）△ｘ、△ｘの値を求めることにより△ｘ、（ｄ／ｄｔ）△ｘの値を更新する。
【００２２】
ｂ）ｍが０の場合
（ｄ／ｄｔ）△ｘ＝（△ｆ−ｋ・△ｘ）／ｃ                  （２）
により（ｄ／ｄｔ）△ｘを算出する。（２）式右辺の△ｘの値はひとつ前の制御周期での値を用いる。（ｄ／ｄｔ）△ｘが算出されたら、やはり数値積分により△ｘの値を求め、更新する。なお、ｍ＝ｋ＝０の場合に、目標の力ｆｐが考えられると、マニピュレータがどことも接触していない場合には、ｆｐ／ｃの一定速度で移動し、対象物と接触すると一定の力ｆｐで力制御されるという、ダンピング制御が実現される。
【００２３】
先端位置への変換部６では、命令保持実行部から転送されたマニピュレータ先端座標での制御点への変位量ｘｅおよびベース座標系から対象物座標系への変位量ｘｏの値をもとに、対象物座標系での制御点の仮想モデル目標位置・速度の値を実現するマニピュレータ先端のベース座標系での位置・速度の目標値を算出する。先端位置制御装置７では、マニピュレータ先端の位置・速度目標値を実現するようにフィードバック制御を行う。重力補償部８では、マニピュレータの手首部に取り付けてある６軸力覚センサ信号の値から力覚センサより先に取り付けてあるハンド・把持物体の重力成分を除去する。重力成分は、力覚センサの姿勢をマニピュレータ各軸に取り付けてあるエンコーダの値により算出することにより求める。制御点力への変換部９では、命令保持実行部から転送されたマニピュレータ先端から制御点への変位量およびベース座標系から対象物座標への変位量をもとに、力覚センサに加わっている重力成分を除いた力／トルクの値を対象物座標系で表した制御点での力／トルクの値に静的なつりあいの関係式をもとに変換する。従って、本マニピュレ−タ制御部により対象物座標系での目標位置へ対象物座標系で表したインピーダンス係数で制御点の位置がインピーダンス制御される。
【００２４】
また、図３は、座標系の定義を示した図である。ベース座標系１１は作業場に固定された場所に定義され、物体座標系１２は各物体のそれぞれに固定されて定義される。またマニピュレータ先端座標系１３はマニピュレータ１０先端に定義され、マニピュレータ先端の動きに伴い移動する。
【００２５】
以下に、上記構成のロボットにおける直線移動により対象物との接触を実現する接触命令、すでに対象物と接触している状態で回転運動をすることにより稜線接触を実現するならい命令、および対象物と接触している状態で回転運動をすることにより対象物との内側外側拘束の状態を実現するはめ合い命令をオペレータがあらかじめ入力したときのロボットの動作を図１を参照しつつ説明する。
【００２６】
接触命令、ならい命令、およびはめ合い命令が表１に示すように定義されている。
【００２７】
【表１】
0003580562.tif 000003
接触命令ｔｏｕｃｈが入力されると、その命令が命令保持実行部１に送られる。
【００２８】
命令保持実行部１では、引き数に含まれる値から
ｃ＝ｆｏｒｃｅ／ｖｅｌ                                    （３）
のようにダンパ係数を計算し、マニピュレ−タ制御部２に
・“ａｘｉｓ”軸のインピーダンス係数として、
ｍ＝ｋ＝０、ｃ＝ｆｏｒｃｅ／ｖｅｌ
・マニピュレータ先端座標系での制御点までの変位量として“ｘｅ”
・対象物座標系のベース座標系からの変位量として“ｘｏ”
・目標の“ａｘｉｓ”軸方向の目標力として“ｆｏｒｃｅ”
・目標位置の“ａｘｉｓ”軸座標値として現在の、対象物座標系での制御点位置を転送する。すると、接触前には対象物座標系の“ａｘｉｓ”軸方向に“ｖｅｌ”の速度で移動し、接触後には“ａｘｉｓ”方向に力“ｆｏｒｃｅ”で押しつけるダンピング制御を行うように設定される。
【００２９】
一方、命令保持実行部１から接触状態検出部３には、オペレータから入力された命令および、ロボットの動作開始時点となる命令転送時の制御点の対象物座標系での座標値ｘ０が送られる。
【００３０】
接触状態検出部３の状態判定処理を図４に従って説明する。状態判定処理は毎制御周期実行される。まずステップｓ１１において、対象物座標系での制御点の“ａｘｉｓ”軸座標値の値の０．５秒間の最大値と最小値を計算し、その差をＭａ×Ｍｉｎとする。次に、ステップｓ１２において、命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を
0003580562.tif 000004
のように比較し、現在の位置が許容範囲を逸脱して行き過ぎである場合には、動作が失敗したと判定する。もし、行き過ぎでない場合には、ステップｓ１３において命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を
0003580562.tif 000005
のように比較し、現在の位置が許容範囲を逸脱して手前である場合には、ステップｓ１４に進む。ステップｓ１４では、０．５秒前から変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、許容範囲より手前で停止してしまっていると判断し、失敗したと判定する。一方、△ｖより大きい場合には、まだ運動中であると判断し、実行中であると判定する。ステップｓ１３で手前ではないと判定された場合には、ステップｓ１５に進む。ステップｓ１５では、命令で指定された目標の力ｆｐと現在の対象物座標系で表した制御点での力ｆとの差を計算し、許容範囲外ならば、衝突直後、あるいはまだ接触前であると考えられるので、まだ実行中であると判定する。一方、許容範囲内ならばステップｓ１６へ進む。ステップｓ１６では、０．５秒前からの変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、命令の中で指定された位置付近で目標の力で接触して安定に静止しているとみなせるので、目標の接触が実現されたと判定する。一方、変動量Ｍａ×Ｍｉｎの値が△ｖより大きい場合にはまだ衝突直後だとみなして実行中であると判定する。
【００３１】
以上の手順により、接触状態検出部３は実行中・失敗・完了のいずれかの結果を命令保持実行部１に応答する。命令保持実行部１は、完了の応答が返ってきたときには、あらかじめストアされていた次の命令に従い、再びマニピュレ−タ制御部２および接触状態検出部３へ値を転送する。また、失敗の応答が返ってきたときには、あらかじめストアされていた、失敗時の命令がある場合にはその命令に従い、マニピュレ−タ制御部２および接触状態検出部３へ値を転送し、特にストアされていた命令がない場合にはデフォルトで設定されている動作、例えば基準姿勢への位置制御での移動動作などを行う。
【００３２】
ならい命令ｆｉｔが入力されると、その命令が命令保持実行部１に送られる。命令保持実行部１では、マニピュレ−タ制御部２に
・“ａｘｉｓ”軸のインピーダンス係数として、
ｍ＝ｋ＝０、ｃ＝Ｃｆ
・マニピュレータ先端座標系での制御点までの変位量として“ｘｅ”
・対象物座標系のベース座標系からの変位量として“ｘｏ”
・目標の“ａｘｉｓ”軸方向の目標力として  ０
・目標位置の“ａｘｉｓ”軸座標値として現在の、対象物座標系での制御点位置を転送する。Ｃｆはデフォルトで設定されているならい命令時のインピーダンス制御のダンパ係数である。なお制御点位置は、稜線接触を実現する稜線の中心位置が設定されるのが望ましい。また本ならい命令が実行される前に稜線接触を実現する稜線の両端点のうち一方がすでに接触している必要がある。すると、対象物座標系での“ａｘｉｓ”方向にトルク０の制御を行うように設定されるので、稜線接触が制御的に実現される。
【００３３】
一方、命令保持実行部１から接触状態検出部３には、オペレータから入力された命令および、ロボットの動作開始時点となる命令転送時の制御点の対象物座標系での座標値ｘ０が送られる。
【００３４】
接触状態検出部３の状態判定処理を図５に従って説明する。状態判定処理は毎制御周期実行される。まずステップｓ２１において、対象物座標系での制御点の“ａｘｉｓ”軸座標値の値の０．５秒間の最大値と最小値を計算し、その差をＭａ×Ｍｉｎとする。次に、ステップｓ２２において、命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を
0003580562.tif 000006
のように比較し、現在の位置が許容範囲を逸脱して行き過ぎである場合には、動作が失敗したと判定する。もし、行き過ぎでない場合には、ステップｓ２３において命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を
0003580562.tif 000007
のように比較し、現在の位置が許容範囲を逸脱して手前である場合には、ステップｓ２４に進む。ステップｓ２４では、０．５秒前から変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、許容範囲より手前で停止してしまっていると判断し、失敗したと判定する。一方、△ｖより大きい場合には、まだ運動中であると判断し、実行中であると判定する。ステップｓ２３で手前ではないと判定された場合には、ステップｓ２５に進む。ステップｓ２５では、現在の対象物座標系で表した制御点での力ｆの大きさを計算し、許容範囲外ならば、衝突直後、あるいはまだ接触前であると考えられるので、まだ実行中であると判定する。一方、許容範囲内ならばステップｓ２６へ進む。ステップｓ２６では、０．５秒前からの変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、命令の中で指定された位置付近で目標の力で接触して安定に静止しているとみなせるので、目標の接触が実現されたと判定する。一方、変動量Ｍａ×Ｍｉｎの値が△ｖより大きい場合にはまだ衝突直後だとみなして実行中であると判定する。
【００３５】
以上の手順により、接触状態検出部３は実行中・失敗・完了のいずれかの結果を命令保持実行部１に応答する。命令保持実行部１は、接触命令のときと同様に応答結果に従った動作を行う。
【００３６】
はめ合い命令ｐａｒａｌｌｅｌが入力されると、その命令が命令保持実行部１に送られる。
【００３７】
命令保持実行部１では、マニピュレ−タ制御部２に
・“ａｘｉｓ”軸のインピーダンス係数として、
ｍ＝ｋ＝０
ｃ＝ｆｏｒｃｅ／ｖｅｌ
・マニピュレータ先端座標系での制御点までの変位量として“ｘｅ”
・対象物座標系のベース座標系からの変位量として“ｘｏ”
・目標の“ａｘｉｓ”軸方向の目標力として“ｆｏｒｃｅ”
・目標位置の“ａｘｉｓ”軸座標値として現在の、対象物座標系での制御点位置を転送する。なお本はめ合い命令が実行される前にすでに内側外側の拘束を実現する対象物と接触している必要がある。また“ｆａｘｉｓ”方向に押し付ける制御をしている必要がある。通常は“ｆａｘｉｓ”方向にすでに接触命令を実現している必要がある。
【００３８】
すると、対象物座標系の“ａｘｉｓ”軸方向に“ｖｅｌ”の角速度で回転することにより内側外側の拘束状態を実現しようとしているふたつの面が平行になった時点で、“ｆａｘｉｓ”方向に制御的に押し付けているので、図６（ｂ）のような乗り越え動作をした後に図６（ｃ）のような内側外側の拘束状態に移行する。その後も回転運動を続け、図６（ｄ）のような２ケ所での接触状態になると回転運動が停止し、“ｆｏｒｃｅ”のトルクでの接触状態となる。
一方、命令保持実行部１から接触状態検出部３には、オペレータから入力された命令および、ロボットの動作開始時点となる命令転送時の制御点の対象物座標系での座標値ｘ０が送られる。
【００３９】
接触状態検出部３の状態判定処理を図７に従って説明する。状態判定処理は毎制御周期実行される。まずステップｓ３１において、対象物座標系での制御点の“ａｘｉｓ”軸座標値の値の０．５秒間の最大値と最小値を計算し、その差をＭａ×Ｍｉｎとする。次に、ステップｓ３２において、０．２秒間の“ｆａｘｉｓ”方向の目標の力の絶対値から実際の力の値の絶対値を引いた最小値を計算し、ｆｆｒｅｅとする。次に、ステップｓ３３において、命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を
0003580562.tif 000008
のように比較し、現在の位置が許容範囲を逸脱して行き過ぎである場合には、動作が失敗したと判定する。もし、行き過ぎでない場合には、ステップｓ３４において命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を
0003580562.tif 000009
のように比較し、現在の位置が許容範囲を逸脱して手前である場合には、ステップｓ３５に進む。ステップｓ３５では、０．５秒前から変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、許容範囲より手前で停止してしまっていると判断し、失敗したと判定する。一方、△ｖより大きい場合には、まだ運動中であると判断し、実行中であると判定する。ステップｓ３４で手前ではないと判定された場合には、ステップｓ３６に進む。ステップｓ３６では、“ｆａｘｉｓ”方向の力が目標の力の値よりどの程度小さいかどうかを計算したｆｆｒｅｅの値と力の制御誤差の最大量としてデフォルトで設定してある△ｆｒｅｅの値との比較を行い、ｆｆｒｅｅが△ｆｒｅｅより大きい場合には、内側外側の拘束状態に移行した時点で１点のみの接触状態になり（図６（ｃ））、“ｆａｘｉｓ”方向に移動できるようになったと判定し、動作の完了と判定する。０．２秒間というある程度の長さの時間の間、１点接触とみなせる力の値を継続したことによりはじめて内側外側の拘束が実現されたとみなしていることになる。このような判定手続きとすることにより、力覚センサのノイズや接触による衝撃、制御による振動等による誤認識を防ぐことができる。一方、ステップｓ３６でｆｆｒｅｅの値が△ｆｒｅｅより小さい場合には、図６（ｄ）に相当する２ケ所での接触が安定に実現されているかどうかの判定であるステップｓ３７からステップｓ３８に進む。ステップｓ３７では命令で指定された目標のトルクｆｐと現在の対象物座標系で表した制御点での力トルクとの差を計算し、許容範囲外ならば、衝突直後、あるいはまだ２点接触前であると考えられるので、まだ実行中であると判定する。一方、許容範囲内ならばステップｓ３８へ進む。ステップｓ３８では、０．５秒前からの変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、命令の中で指定された位置付近で目標の力で接触して安定に静止しているとみなせるので、目標の接触が実現されたと判定する。一方、変動量Ｍａ×Ｍｉｎの値が△ｖより大きい場合にはまだ衝突直後だとみなして実行中であると判定する。
【００４０】
以上の手順により、接触状態検出部３は実行中・失敗・完了のいずれかの結果を命令保持実行部１に応答する。命令保持実行部１は、接触・ならい命令のときと同様に応答結果に従った動作を行う。
【００４１】
以上のような働きにより、あらかじめオペレータから入力され、命令保持実行部にストアされていた一連の命令をそれぞれ確実に実行するとともに、その動作の完了・失敗を検出することが可能になる。従って、オペレータはあらかじめ、目的とする組み立てを実現するための手順を、直線移動により対象物との接触を実現する接触命令、すでに対象物と接触している状態で回転運動をすることにより稜線接触を実現するならい命令、および対象物と接触している状態で回転運動をすることにより対象物との内側外側拘束の状態を実現するはめ合い命令および、通常の位置制御による移動、インピーダンス制御による移動の組み合わせで表現しておくことにより、治具や部品供給装置により十分な程度で位置決めされていない場合や、治具や部品供給装置で位置決めされてはいるが、現場でのロボットの据え付け誤差やロボットの関節部でのたわみなどによる誤差により現場での教示やキャリブレーションを行わないとオフラインの教示による位置制御のみでは作業の実行が不可能な場合や、ロボットの精度以上に高精度なはめ合い作業などを行う場合に、現場での教示やキャリブレーションを行うことなく、オフラインの教示のみにより確実な動作で作業を実行することが可能となる。
【００４２】
図８は、本発明の第２実施例を示すブロック図である。ロボットは、ユーザインタフェース１６、上位命令翻訳部１７、環境モデル部１８、命令保持実行部１９、マニピュレ−タ制御部２０、接触状態検出部２１、およびマニピュレータ２２を備えている。環境モデル部１８は、ロボットの作業環境およびロボット自身の幾何情報を３次元で有し、各物体の位置の値と実際との間の誤差として起こり得る最大値の値も内部に有する。データ表現は境界表現で、多面体近似で表現されている。
【００４３】
以下に、上記構成のロボットにおける直線移動により指定されたふたつの面の点接触を実現する上位接触命令、すでに対象物と接触している状態で回転運動をすることにより指定された面と稜線との稜線接触を実現する上位ならい命令、および対象物と接触している状態で回転運動をすることにより指定されたふたつの面を平行にし対象物との内側外側拘束の状態を実現する上位はめ合い命令をオペレータがあらかじめ入力したときのロボットの動作を図８を参照しつつ説明する。
【００４４】
上位接触命令、上位ならい命令、および上位はめ合い命令が表２に示すように定義されている。
【００４５】
【表２】
0003580562.tif 000010
上位接触命令がオペレータにより入力されると、ユーザインタフェース部１６から上位命令翻訳部１７へその命令が転送される。上位命令翻訳部１７では、環境モデル部１８の値を利用しながら命令保持実行部へ転送する接触命令の引き数の値を計算により求める。まず、命令の中で指定された対象物の座標系“Σｏ”のベース座標系からの変位量ｘｏの値を環境モデルから読み込む。“ａｘｉｓ”、“ｖｅｌ”、“ｆｏｒｃｅ”、の値はそのまま接触命令の引き数とする。また、対象物座標系“Σｏ”の位置・姿勢の値に含まれる誤差量の値△ｘを環境モデルから読み込む。制御点のマニピュレータ先端座標系での変位量ｘｅの値は、本命令が実行される時点で設定されている値を接触命令の引き数とする。ｘｅの値は、後に説明する上位ならい命令その他の命令による自動設定も行われるが、ｘｅの値を設定・変更するための専用の命令もシステムに含まれるとする。最後に、命令の中で指定された面同士の接触が起きる位置を以下の手順により求める。
【００４６】
まず、命令が実行されるときの対象物座標系“Σｏ”でのマニピュレータ制御点位置をｘ０とする。制御点の位置をｘ０から“ａｘｉｓ”軸方向に“ｖｅｌ”の値が正ならば正方向に、“ｖｅｌ”の値が負ならば負の方向に一定きざみ幅で移動させてゆき、各点で面“ｆａ”と面“ｆｂ”の接触の有無を環境モデル部１８の値をもとに計算し、はじめて接触が起きるときの対象物座標系“Σｏ”での“ａｘｉｓ”座標値の値をｘｐとする。
【００４７】
以上の処理により求めた値を引き数として接触命令を命令保持実行部１９に転送する。以後は第１の実施例と同様の処理が行われることにより、面“ｆａ”と面“ｆｂ”の接触の実現およびその完了・失敗の検出が行われる。
【００４８】
上位ならい命令がオペレータにより入力されると、ユーザインタフェース部１６から上位命令翻訳部１７へその命令が転送される。上位命令翻訳部１７では、環境モデル部１８の値を利用しながら命令保持実行部の転送する接触命令の引き数の値を研鑽により求める。まず、命令の中で指定された対象物の座標系“Σｏ”のベース座標系からの変位量ｘｏの値を環境モデルから読み込む、“ａｘｉｓ”の値はそのまま接触命令の引き数とする。また、対象物座標系“Σｏ”の位置・姿勢の値に含まれる誤差量の値△ｘを環境モデルから読み込む。制御点のマニピュレータ先端座標系での変位量ｘｅの値は、命令の中で指定された稜線“ｅｂ”の中心点のマニピュレータ先端座標での値を求め、ならい命令の引き数とする。最後に、命令の中で指定された面と稜線の接触が起きる位置を上位接触命令の時と同様に環境モデル部１８の値を利用することにより求め、ならい命令の引き数“ｘｐ”とする。以上の処理により求めた値を引き数としてならい命令を命令保持実行部１９に転送し、やはり第１実施例と同様の処理が行われることにより、面“ｆａ”と稜線“ｅｂ”の稜線接触の実現およびその完了・失敗の検出が行われる。なお本命令はマニピュレータで把持している物体の稜線“ｅｂ”と対象物の面“ｆａ”との稜線接触を実現する。
【００４９】
上位はめ合い命令がオペレータにより入力されると、ユーザインタフェース部１６から上位命令翻訳部１７へその命令が転送される。上位命令翻訳部１７では、環境モデル部１８の値を利用しながら命令保持実行部へ転送する接触命令の引き数の値を計算により求める。まず、命令の中で指定された対象物の座標系“Σｏ”のベース座標系からの変位量ｘｏの値を環境モデルから読み込む。“ａｘｉｓ”，“ｖｅｌ”，“ｆｏｒｃｅ”の値はそのままはめ合い命令の引き数とする。また、対象物座標系“Σｏ”の位置・姿勢の値に含まれる誤差量の値△ｘを環境モデルから読み込む。制御点のマニピュレータ先端座標系での変位量ｘｅの値は、本命令が実行される時点で設定されている値を接触命令の引き数とする。はめ合い命令の“ｘｐ”の値は、上位はめ合い命令の指定された“ｆａ”、“ｆｂ”のふたつの面が平行、又は“ａｘｉｓ”軸まわりの回転運動により最も平行に近付くときの制御点の位置・姿勢の値を環境モデル部１８の値を利用することにより求め、その“ａｘｉｓ”軸まわりの姿勢の値を設定する。はめ合い命令の“ｆａｘｉｓ”の値は、ｘ，ｙ，ｚのいずれかであるが、本上位はめ合い命令が実行される時点で接触命令が完了している軸の中から、面“ｆｂ”の法線方向と最も大きく方向が異なる軸として設定する。つまり、押し付けの制御が行われている方向の中で、面ｆｂと最も平行に近い、従って１点接触になったときに並進移動を起こしやすい方向を“ｆａｘｉｓ”に設定する。以上の処理により求めた値を引き数としてはめ合い命令を命令保持実行部１９に転送し、やはり第１の実施例と同様の処理が行われることにより、面“ｆａ”と面“ｆｂ”がほぼ平行になることにより実現される内側外側の拘束状態の実現およびその完了・失敗の検出が行われる。なお、本命令では面“ｆａ”はマニピュレータで把持している物体の面であり、面“ｆｂ”は対象物の面である。
【００５０】
以上のような働きにより、あらかじめオペレータから入力された、第２に示すような命令をそれぞれ確実に実行するとともに、その動作の完了・失敗を検出することが可能になる。上位接触命令、上位ならい命令、上位はめ合い命令は、それぞれ環境モデル部１８で定義された物体、それらの物体の面・稜線の間の接触の実現を指定する命令であり、３次元のソリッドモデルを有する環境モデル、環境モデルデータをもとに作業環境を両面表示するシミュレータ等と組み合わせることにより、オペレータは容易にロボットの動作をオフラインで教示することが可能になる。
【００５１】
また治具や部品供給装置により十分な精度で位置決めされていない場合や、治具や部品供給装置で位置決めされてはいるが、現場でのロボットの据え付け誤差やロボットの関節部でのたわみなどによる誤差により現場での教示やキャリブレーションを行わないとオフラインの教示による位置制御のみでは作業の実行が不可能な場合や、ロボットの精度以上に高精度なはめ合い作業などを行う場合でも、確実な動作を行うように教示することが可能となる。
【００５２】
【発明の効果】
以上説明したように、本発明の構成によれば、各基本命令により確実に目標とする動作を実行するとともに、目標とする動作の完了または失敗を検出しつつ動作をすることが可能となり、治具や部品供給装置により十分な精度で位置決めされていない場合や、治具部品供給装置で位置決めされてはいるが現場でのロボットの据え付け誤差やロボットの関節部でのたわみなどによる誤差により現場での教示やキャリブレーションを行わないとオフラインの教示による位置制御のみでは作業の実行が不可能な場合や、ロボットの精度以上に高精度ははめ合い作業などを行う場合において、現場での教示やキャリブレーションを行うことなく、オフラインの教示のみにより確実な動作で作業を実行することができる。
【図面の簡単な説明】
【図１】本発明の第１実施例の概略構成を示すブロック図。
【図２】図１に示すマニピュレ−タ制御部の一構成例を示すブロック図。
【図３】マニピュレータおよび作業環境における座標系の定義を示す図。
【図４】接触命令を実行している場合の接触状態判定部での判定アルゴリズムの詳細を示すフローチャートを示す図。
【図５】ならい命令を実行している場合の接触状態判定部での判定アルゴリズムの詳細を示すフローチャートを示す図。
【図６】はめ合い作業における接触状態の変化を示す説明図。
【図７】はめ合い命令を実行している場合の接触状態判定部での判定アルゴリズムの詳細を示すフローチャートを示す図。
【図８】本発明の第２実施例を示すブロック図。
【符号の説明】
１  命令保持実行部
２  マニピュレ−タ制御部
３  接触状態検出部
４  マニピュレータ
５  仮想モデル算出部
６  先端位置への変換部
７  先端位置制御装置
８  重力補償部
９  制御点力への変換部
１０  マニピュレータ
１１  ベース座標系
１２  対象物座標系
１３  マニピュレータ先端座標系
１４  ピン
１５  ブロック
１６  ユーザインタフェース部
１７  上位命令翻訳部
１８  環境モデル部
１９  命令保持実行部
２０  接触状態検出部
２１  マニピュレ−タ制御部
２２  マニピュレータ
【図１】
0003580562.tif 000011
【図２】
0003580562.tif 000012
【図３】
0003580562.tif 000013
【図４】
0003580562.tif 000014
【図５】
0003580562.tif 000015
【図６】
0003580562.tif 000016
【図７】
0003580562.tif 000017
【図８】
0003580562.tif 000018