JP3580562B2 - robot - Google Patents

Description

により（ｄ２／ｄｔ２）△ｘを算出する。（１）式右辺の△ｘ、（ｄ／ｄｔ）△ｘの値はひとつ前の制御周期での値を用いる。（ｄ２／ｄｔ２）△ｘが算出されたら、数値積分により順次（ｄ／ｄｔ）△ｘ、△ｘの値を求めることにより△ｘ、（ｄ／ｄｔ）△ｘの値を更新する。
【００２２】
ｂ）ｍが０の場合
（ｄ／ｄｔ）△ｘ＝（△ｆ−ｋ・△ｘ）／ｃ （２）
により（ｄ／ｄｔ）△ｘを算出する。（２）式右辺の△ｘの値はひとつ前の制御周期での値を用いる。（ｄ／ｄｔ）△ｘが算出されたら、やはり数値積分により△ｘの値を求め、更新する。なお、ｍ＝ｋ＝０の場合に、目標の力ｆｐが考えられると、マニピュレータがどことも接触していない場合には、ｆｐ／ｃの一定速度で移動し、対象物と接触すると一定の力ｆｐで力制御されるという、ダンピング制御が実現される。
【００２３】
先端位置への変換部６では、命令保持実行部から転送されたマニピュレータ先端座標での制御点への変位量ｘｅおよびベース座標系から対象物座標系への変位量ｘｏの値をもとに、対象物座標系での制御点の仮想モデル目標位置・速度の値を実現するマニピュレータ先端のベース座標系での位置・速度の目標値を算出する。先端位置制御装置７では、マニピュレータ先端の位置・速度目標値を実現するようにフィードバック制御を行う。重力補償部８では、マニピュレータの手首部に取り付けてある６軸力覚センサ信号の値から力覚センサより先に取り付けてあるハンド・把持物体の重力成分を除去する。重力成分は、力覚センサの姿勢をマニピュレータ各軸に取り付けてあるエンコーダの値により算出することにより求める。制御点力への変換部９では、命令保持実行部から転送されたマニピュレータ先端から制御点への変位量およびベース座標系から対象物座標への変位量をもとに、力覚センサに加わっている重力成分を除いた力／トルクの値を対象物座標系で表した制御点での力／トルクの値に静的なつりあいの関係式をもとに変換する。従って、本マニピュレ−タ制御部により対象物座標系での目標位置へ対象物座標系で表したインピーダンス係数で制御点の位置がインピーダンス制御される。
【００２４】
また、図３は、座標系の定義を示した図である。ベース座標系１１は作業場に固定された場所に定義され、物体座標系１２は各物体のそれぞれに固定されて定義される。またマニピュレータ先端座標系１３はマニピュレータ１０先端に定義され、マニピュレータ先端の動きに伴い移動する。
【００２５】
以下に、上記構成のロボットにおける直線移動により対象物との接触を実現する接触命令、すでに対象物と接触している状態で回転運動をすることにより稜線接触を実現するならい命令、および対象物と接触している状態で回転運動をすることにより対象物との内側外側拘束の状態を実現するはめ合い命令をオペレータがあらかじめ入力したときのロボットの動作を図１を参照しつつ説明する。
【００２６】
接触命令、ならい命令、およびはめ合い命令が表１に示すように定義されている。
【００２７】
【表１】

接触命令ｔｏｕｃｈが入力されると、その命令が命令保持実行部１に送られる。
【００２８】
命令保持実行部１では、引き数に含まれる値から
ｃ＝ｆｏｒｃｅ／ｖｅｌ （３）
のようにダンパ係数を計算し、マニピュレ−タ制御部２に
・“ａｘｉｓ”軸のインピーダンス係数として、
ｍ＝ｋ＝０、ｃ＝ｆｏｒｃｅ／ｖｅｌ
・マニピュレータ先端座標系での制御点までの変位量として“ｘｅ”
・対象物座標系のベース座標系からの変位量として“ｘｏ”
・目標の“ａｘｉｓ”軸方向の目標力として“ｆｏｒｃｅ”
・目標位置の“ａｘｉｓ”軸座標値として現在の、対象物座標系での制御点位置を転送する。すると、接触前には対象物座標系の“ａｘｉｓ”軸方向に“ｖｅｌ”の速度で移動し、接触後には“ａｘｉｓ”方向に力“ｆｏｒｃｅ”で押しつけるダンピング制御を行うように設定される。
【００２９】
一方、命令保持実行部１から接触状態検出部３には、オペレータから入力された命令および、ロボットの動作開始時点となる命令転送時の制御点の対象物座標系での座標値ｘ０が送られる。
【００３０】
接触状態検出部３の状態判定処理を図４に従って説明する。状態判定処理は毎制御周期実行される。まずステップｓ１１において、対象物座標系での制御点の“ａｘｉｓ”軸座標値の値の０．５秒間の最大値と最小値を計算し、その差をＭａ×Ｍｉｎとする。次に、ステップｓ１２において、命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を

のように比較し、現在の位置が許容範囲を逸脱して行き過ぎである場合には、動作が失敗したと判定する。もし、行き過ぎでない場合には、ステップｓ１３において命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を

のように比較し、現在の位置が許容範囲を逸脱して手前である場合には、ステップｓ１４に進む。ステップｓ１４では、０．５秒前から変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、許容範囲より手前で停止してしまっていると判断し、失敗したと判定する。一方、△ｖより大きい場合には、まだ運動中であると判断し、実行中であると判定する。ステップｓ１３で手前ではないと判定された場合には、ステップｓ１５に進む。ステップｓ１５では、命令で指定された目標の力ｆｐと現在の対象物座標系で表した制御点での力ｆとの差を計算し、許容範囲外ならば、衝突直後、あるいはまだ接触前であると考えられるので、まだ実行中であると判定する。一方、許容範囲内ならばステップｓ１６へ進む。ステップｓ１６では、０．５秒前からの変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、命令の中で指定された位置付近で目標の力で接触して安定に静止しているとみなせるので、目標の接触が実現されたと判定する。一方、変動量Ｍａ×Ｍｉｎの値が△ｖより大きい場合にはまだ衝突直後だとみなして実行中であると判定する。
【００３１】
以上の手順により、接触状態検出部３は実行中・失敗・完了のいずれかの結果を命令保持実行部１に応答する。命令保持実行部１は、完了の応答が返ってきたときには、あらかじめストアされていた次の命令に従い、再びマニピュレ−タ制御部２および接触状態検出部３へ値を転送する。また、失敗の応答が返ってきたときには、あらかじめストアされていた、失敗時の命令がある場合にはその命令に従い、マニピュレ−タ制御部２および接触状態検出部３へ値を転送し、特にストアされていた命令がない場合にはデフォルトで設定されている動作、例えば基準姿勢への位置制御での移動動作などを行う。
【００３２】
ならい命令ｆｉｔが入力されると、その命令が命令保持実行部１に送られる。命令保持実行部１では、マニピュレ−タ制御部２に
・“ａｘｉｓ”軸のインピーダンス係数として、
ｍ＝ｋ＝０、ｃ＝Ｃｆ
・マニピュレータ先端座標系での制御点までの変位量として“ｘｅ”
・対象物座標系のベース座標系からの変位量として“ｘｏ”
・目標の“ａｘｉｓ”軸方向の目標力として ０
・目標位置の“ａｘｉｓ”軸座標値として現在の、対象物座標系での制御点位置を転送する。Ｃｆはデフォルトで設定されているならい命令時のインピーダンス制御のダンパ係数である。なお制御点位置は、稜線接触を実現する稜線の中心位置が設定されるのが望ましい。また本ならい命令が実行される前に稜線接触を実現する稜線の両端点のうち一方がすでに接触している必要がある。すると、対象物座標系での“ａｘｉｓ”方向にトルク０の制御を行うように設定されるので、稜線接触が制御的に実現される。
【００３３】
一方、命令保持実行部１から接触状態検出部３には、オペレータから入力された命令および、ロボットの動作開始時点となる命令転送時の制御点の対象物座標系での座標値ｘ０が送られる。
【００３４】
接触状態検出部３の状態判定処理を図５に従って説明する。状態判定処理は毎制御周期実行される。まずステップｓ２１において、対象物座標系での制御点の“ａｘｉｓ”軸座標値の値の０．５秒間の最大値と最小値を計算し、その差をＭａ×Ｍｉｎとする。次に、ステップｓ２２において、命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を

のように比較し、現在の位置が許容範囲を逸脱して行き過ぎである場合には、動作が失敗したと判定する。もし、行き過ぎでない場合には、ステップｓ２３において命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を

のように比較し、現在の位置が許容範囲を逸脱して手前である場合には、ステップｓ２４に進む。ステップｓ２４では、０．５秒前から変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、許容範囲より手前で停止してしまっていると判断し、失敗したと判定する。一方、△ｖより大きい場合には、まだ運動中であると判断し、実行中であると判定する。ステップｓ２３で手前ではないと判定された場合には、ステップｓ２５に進む。ステップｓ２５では、現在の対象物座標系で表した制御点での力ｆの大きさを計算し、許容範囲外ならば、衝突直後、あるいはまだ接触前であると考えられるので、まだ実行中であると判定する。一方、許容範囲内ならばステップｓ２６へ進む。ステップｓ２６では、０．５秒前からの変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、命令の中で指定された位置付近で目標の力で接触して安定に静止しているとみなせるので、目標の接触が実現されたと判定する。一方、変動量Ｍａ×Ｍｉｎの値が△ｖより大きい場合にはまだ衝突直後だとみなして実行中であると判定する。
【００３５】
以上の手順により、接触状態検出部３は実行中・失敗・完了のいずれかの結果を命令保持実行部１に応答する。命令保持実行部１は、接触命令のときと同様に応答結果に従った動作を行う。
【００３６】
はめ合い命令ｐａｒａｌｌｅｌが入力されると、その命令が命令保持実行部１に送られる。
【００３７】
命令保持実行部１では、マニピュレ−タ制御部２に
・“ａｘｉｓ”軸のインピーダンス係数として、
ｍ＝ｋ＝０
ｃ＝ｆｏｒｃｅ／ｖｅｌ
・マニピュレータ先端座標系での制御点までの変位量として“ｘｅ”
・対象物座標系のベース座標系からの変位量として“ｘｏ”
・目標の“ａｘｉｓ”軸方向の目標力として“ｆｏｒｃｅ”
・目標位置の“ａｘｉｓ”軸座標値として現在の、対象物座標系での制御点位置を転送する。なお本はめ合い命令が実行される前にすでに内側外側の拘束を実現する対象物と接触している必要がある。また“ｆａｘｉｓ”方向に押し付ける制御をしている必要がある。通常は“ｆａｘｉｓ”方向にすでに接触命令を実現している必要がある。
【００３８】
すると、対象物座標系の“ａｘｉｓ”軸方向に“ｖｅｌ”の角速度で回転することにより内側外側の拘束状態を実現しようとしているふたつの面が平行になった時点で、“ｆａｘｉｓ”方向に制御的に押し付けているので、図６（ｂ）のような乗り越え動作をした後に図６（ｃ）のような内側外側の拘束状態に移行する。その後も回転運動を続け、図６（ｄ）のような２ケ所での接触状態になると回転運動が停止し、“ｆｏｒｃｅ”のトルクでの接触状態となる。
一方、命令保持実行部１から接触状態検出部３には、オペレータから入力された命令および、ロボットの動作開始時点となる命令転送時の制御点の対象物座標系での座標値ｘ０が送られる。
【００３９】
接触状態検出部３の状態判定処理を図７に従って説明する。状態判定処理は毎制御周期実行される。まずステップｓ３１において、対象物座標系での制御点の“ａｘｉｓ”軸座標値の値の０．５秒間の最大値と最小値を計算し、その差をＭａ×Ｍｉｎとする。次に、ステップｓ３２において、０．２秒間の“ｆａｘｉｓ”方向の目標の力の絶対値から実際の力の値の絶対値を引いた最小値を計算し、ｆｆｒｅｅとする。次に、ステップｓ３３において、命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を

のように比較し、現在の位置が許容範囲を逸脱して行き過ぎである場合には、動作が失敗したと判定する。もし、行き過ぎでない場合には、ステップｓ３４において命令で指定された接触点での“ａｘｉｓ”軸座標値の値ｘｐと現在の“ａｘｉｓ”軸座標値ｘの値を

のように比較し、現在の位置が許容範囲を逸脱して手前である場合には、ステップｓ３５に進む。ステップｓ３５では、０．５秒前から変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、許容範囲より手前で停止してしまっていると判断し、失敗したと判定する。一方、△ｖより大きい場合には、まだ運動中であると判断し、実行中であると判定する。ステップｓ３４で手前ではないと判定された場合には、ステップｓ３６に進む。ステップｓ３６では、“ｆａｘｉｓ”方向の力が目標の力の値よりどの程度小さいかどうかを計算したｆｆｒｅｅの値と力の制御誤差の最大量としてデフォルトで設定してある△ｆｒｅｅの値との比較を行い、ｆｆｒｅｅが△ｆｒｅｅより大きい場合には、内側外側の拘束状態に移行した時点で１点のみの接触状態になり（図６（ｃ））、“ｆａｘｉｓ”方向に移動できるようになったと判定し、動作の完了と判定する。０．２秒間というある程度の長さの時間の間、１点接触とみなせる力の値を継続したことによりはじめて内側外側の拘束が実現されたとみなしていることになる。このような判定手続きとすることにより、力覚センサのノイズや接触による衝撃、制御による振動等による誤認識を防ぐことができる。一方、ステップｓ３６でｆｆｒｅｅの値が△ｆｒｅｅより小さい場合には、図６（ｄ）に相当する２ケ所での接触が安定に実現されているかどうかの判定であるステップｓ３７からステップｓ３８に進む。ステップｓ３７では命令で指定された目標のトルクｆｐと現在の対象物座標系で表した制御点での力トルクとの差を計算し、許容範囲外ならば、衝突直後、あるいはまだ２点接触前であると考えられるので、まだ実行中であると判定する。一方、許容範囲内ならばステップｓ３８へ進む。ステップｓ３８では、０．５秒前からの変動量Ｍａ×Ｍｉｎの値が許容値△ｖより小さいかどうかの判定を行い、小さい場合には、命令の中で指定された位置付近で目標の力で接触して安定に静止しているとみなせるので、目標の接触が実現されたと判定する。一方、変動量Ｍａ×Ｍｉｎの値が△ｖより大きい場合にはまだ衝突直後だとみなして実行中であると判定する。
【００４０】
以上の手順により、接触状態検出部３は実行中・失敗・完了のいずれかの結果を命令保持実行部１に応答する。命令保持実行部１は、接触・ならい命令のときと同様に応答結果に従った動作を行う。
【００４１】
以上のような働きにより、あらかじめオペレータから入力され、命令保持実行部にストアされていた一連の命令をそれぞれ確実に実行するとともに、その動作の完了・失敗を検出することが可能になる。従って、オペレータはあらかじめ、目的とする組み立てを実現するための手順を、直線移動により対象物との接触を実現する接触命令、すでに対象物と接触している状態で回転運動をすることにより稜線接触を実現するならい命令、および対象物と接触している状態で回転運動をすることにより対象物との内側外側拘束の状態を実現するはめ合い命令および、通常の位置制御による移動、インピーダンス制御による移動の組み合わせで表現しておくことにより、治具や部品供給装置により十分な程度で位置決めされていない場合や、治具や部品供給装置で位置決めされてはいるが、現場でのロボットの据え付け誤差やロボットの関節部でのたわみなどによる誤差により現場での教示やキャリブレーションを行わないとオフラインの教示による位置制御のみでは作業の実行が不可能な場合や、ロボットの精度以上に高精度なはめ合い作業などを行う場合に、現場での教示やキャリブレーションを行うことなく、オフラインの教示のみにより確実な動作で作業を実行することが可能となる。
【００４２】
図８は、本発明の第２実施例を示すブロック図である。ロボットは、ユーザインタフェース１６、上位命令翻訳部１７、環境モデル部１８、命令保持実行部１９、マニピュレ−タ制御部２０、接触状態検出部２１、およびマニピュレータ２２を備えている。環境モデル部１８は、ロボットの作業環境およびロボット自身の幾何情報を３次元で有し、各物体の位置の値と実際との間の誤差として起こり得る最大値の値も内部に有する。データ表現は境界表現で、多面体近似で表現されている。
【００４３】
以下に、上記構成のロボットにおける直線移動により指定されたふたつの面の点接触を実現する上位接触命令、すでに対象物と接触している状態で回転運動をすることにより指定された面と稜線との稜線接触を実現する上位ならい命令、および対象物と接触している状態で回転運動をすることにより指定されたふたつの面を平行にし対象物との内側外側拘束の状態を実現する上位はめ合い命令をオペレータがあらかじめ入力したときのロボットの動作を図８を参照しつつ説明する。
【００４４】
上位接触命令、上位ならい命令、および上位はめ合い命令が表２に示すように定義されている。
【００４５】
【表２】

上位接触命令がオペレータにより入力されると、ユーザインタフェース部１６から上位命令翻訳部１７へその命令が転送される。上位命令翻訳部１７では、環境モデル部１８の値を利用しながら命令保持実行部へ転送する接触命令の引き数の値を計算により求める。まず、命令の中で指定された対象物の座標系“Σｏ”のベース座標系からの変位量ｘｏの値を環境モデルから読み込む。“ａｘｉｓ”、“ｖｅｌ”、“ｆｏｒｃｅ”、の値はそのまま接触命令の引き数とする。また、対象物座標系“Σｏ”の位置・姿勢の値に含まれる誤差量の値△ｘを環境モデルから読み込む。制御点のマニピュレータ先端座標系での変位量ｘｅの値は、本命令が実行される時点で設定されている値を接触命令の引き数とする。ｘｅの値は、後に説明する上位ならい命令その他の命令による自動設定も行われるが、ｘｅの値を設定・変更するための専用の命令もシステムに含まれるとする。最後に、命令の中で指定された面同士の接触が起きる位置を以下の手順により求める。
【００４６】
まず、命令が実行されるときの対象物座標系“Σｏ”でのマニピュレータ制御点位置をｘ０とする。制御点の位置をｘ０から“ａｘｉｓ”軸方向に“ｖｅｌ”の値が正ならば正方向に、“ｖｅｌ”の値が負ならば負の方向に一定きざみ幅で移動させてゆき、各点で面“ｆａ”と面“ｆｂ”の接触の有無を環境モデル部１８の値をもとに計算し、はじめて接触が起きるときの対象物座標系“Σｏ”での“ａｘｉｓ”座標値の値をｘｐとする。
【００４７】
以上の処理により求めた値を引き数として接触命令を命令保持実行部１９に転送する。以後は第１の実施例と同様の処理が行われることにより、面“ｆａ”と面“ｆｂ”の接触の実現およびその完了・失敗の検出が行われる。
【００４８】
上位ならい命令がオペレータにより入力されると、ユーザインタフェース部１６から上位命令翻訳部１７へその命令が転送される。上位命令翻訳部１７では、環境モデル部１８の値を利用しながら命令保持実行部の転送する接触命令の引き数の値を研鑽により求める。まず、命令の中で指定された対象物の座標系“Σｏ”のベース座標系からの変位量ｘｏの値を環境モデルから読み込む、“ａｘｉｓ”の値はそのまま接触命令の引き数とする。また、対象物座標系“Σｏ”の位置・姿勢の値に含まれる誤差量の値△ｘを環境モデルから読み込む。制御点のマニピュレータ先端座標系での変位量ｘｅの値は、命令の中で指定された稜線“ｅｂ”の中心点のマニピュレータ先端座標での値を求め、ならい命令の引き数とする。最後に、命令の中で指定された面と稜線の接触が起きる位置を上位接触命令の時と同様に環境モデル部１８の値を利用することにより求め、ならい命令の引き数“ｘｐ”とする。以上の処理により求めた値を引き数としてならい命令を命令保持実行部１９に転送し、やはり第１実施例と同様の処理が行われることにより、面“ｆａ”と稜線“ｅｂ”の稜線接触の実現およびその完了・失敗の検出が行われる。なお本命令はマニピュレータで把持している物体の稜線“ｅｂ”と対象物の面“ｆａ”との稜線接触を実現する。
【００４９】
上位はめ合い命令がオペレータにより入力されると、ユーザインタフェース部１６から上位命令翻訳部１７へその命令が転送される。上位命令翻訳部１７では、環境モデル部１８の値を利用しながら命令保持実行部へ転送する接触命令の引き数の値を計算により求める。まず、命令の中で指定された対象物の座標系“Σｏ”のベース座標系からの変位量ｘｏの値を環境モデルから読み込む。“ａｘｉｓ”，“ｖｅｌ”，“ｆｏｒｃｅ”の値はそのままはめ合い命令の引き数とする。また、対象物座標系“Σｏ”の位置・姿勢の値に含まれる誤差量の値△ｘを環境モデルから読み込む。制御点のマニピュレータ先端座標系での変位量ｘｅの値は、本命令が実行される時点で設定されている値を接触命令の引き数とする。はめ合い命令の“ｘｐ”の値は、上位はめ合い命令の指定された“ｆａ”、“ｆｂ”のふたつの面が平行、又は“ａｘｉｓ”軸まわりの回転運動により最も平行に近付くときの制御点の位置・姿勢の値を環境モデル部１８の値を利用することにより求め、その“ａｘｉｓ”軸まわりの姿勢の値を設定する。はめ合い命令の“ｆａｘｉｓ”の値は、ｘ，ｙ，ｚのいずれかであるが、本上位はめ合い命令が実行される時点で接触命令が完了している軸の中から、面“ｆｂ”の法線方向と最も大きく方向が異なる軸として設定する。つまり、押し付けの制御が行われている方向の中で、面ｆｂと最も平行に近い、従って１点接触になったときに並進移動を起こしやすい方向を“ｆａｘｉｓ”に設定する。以上の処理により求めた値を引き数としてはめ合い命令を命令保持実行部１９に転送し、やはり第１の実施例と同様の処理が行われることにより、面“ｆａ”と面“ｆｂ”がほぼ平行になることにより実現される内側外側の拘束状態の実現およびその完了・失敗の検出が行われる。なお、本命令では面“ｆａ”はマニピュレータで把持している物体の面であり、面“ｆｂ”は対象物の面である。
【００５０】
以上のような働きにより、あらかじめオペレータから入力された、第２に示すような命令をそれぞれ確実に実行するとともに、その動作の完了・失敗を検出することが可能になる。上位接触命令、上位ならい命令、上位はめ合い命令は、それぞれ環境モデル部１８で定義された物体、それらの物体の面・稜線の間の接触の実現を指定する命令であり、３次元のソリッドモデルを有する環境モデル、環境モデルデータをもとに作業環境を両面表示するシミュレータ等と組み合わせることにより、オペレータは容易にロボットの動作をオフラインで教示することが可能になる。
【００５１】
また治具や部品供給装置により十分な精度で位置決めされていない場合や、治具や部品供給装置で位置決めされてはいるが、現場でのロボットの据え付け誤差やロボットの関節部でのたわみなどによる誤差により現場での教示やキャリブレーションを行わないとオフラインの教示による位置制御のみでは作業の実行が不可能な場合や、ロボットの精度以上に高精度なはめ合い作業などを行う場合でも、確実な動作を行うように教示することが可能となる。
【００５２】
【発明の効果】
以上説明したように、本発明の構成によれば、各基本命令により確実に目標とする動作を実行するとともに、目標とする動作の完了または失敗を検出しつつ動作をすることが可能となり、治具や部品供給装置により十分な精度で位置決めされていない場合や、治具部品供給装置で位置決めされてはいるが現場でのロボットの据え付け誤差やロボットの関節部でのたわみなどによる誤差により現場での教示やキャリブレーションを行わないとオフラインの教示による位置制御のみでは作業の実行が不可能な場合や、ロボットの精度以上に高精度ははめ合い作業などを行う場合において、現場での教示やキャリブレーションを行うことなく、オフラインの教示のみにより確実な動作で作業を実行することができる。
【図面の簡単な説明】
【図１】本発明の第１実施例の概略構成を示すブロック図。
【図２】図１に示すマニピュレ−タ制御部の一構成例を示すブロック図。
【図３】マニピュレータおよび作業環境における座標系の定義を示す図。
【図４】接触命令を実行している場合の接触状態判定部での判定アルゴリズムの詳細を示すフローチャートを示す図。
【図５】ならい命令を実行している場合の接触状態判定部での判定アルゴリズムの詳細を示すフローチャートを示す図。
【図６】はめ合い作業における接触状態の変化を示す説明図。
【図７】はめ合い命令を実行している場合の接触状態判定部での判定アルゴリズムの詳細を示すフローチャートを示す図。
【図８】本発明の第２実施例を示すブロック図。
【符号の説明】
１ 命令保持実行部
２ マニピュレ−タ制御部
３ 接触状態検出部
４ マニピュレータ
５ 仮想モデル算出部
６ 先端位置への変換部
７ 先端位置制御装置
８ 重力補償部
９ 制御点力への変換部
１０ マニピュレータ
１１ ベース座標系
１２ 対象物座標系
１３ マニピュレータ先端座標系
１４ ピン
１５ ブロック
１６ ユーザインタフェース部
１７ 上位命令翻訳部
１８ 環境モデル部
１９ 命令保持実行部
２０ 接触状態検出部
２１ マニピュレ−タ制御部
２２ マニピュレータ[0001]
[Object of the invention]
[Industrial applications]
The present invention relates to a robot having a force sensor for detecting a force / moment applied to a hand and a control device for controlling the position and force of the robot.
[0002]
[Prior art]
The willingness to introduce industrial robots into manufacturing factories has never diminished, and in recent years, in particular, the use of robots for assembling electronic components has become popular. Not only the assembling work but also the industrial robot, by accurately positioning the parts by the part supply device, enables the robot to execute the work only by repeating the operation taught in advance every time. In addition, as the application of small force sensors that measure the force / torque applied to the tip of the robot becomes widespread, only the position of the robot, such as an example in which compliance control achieves a more accurate fit than the positioning accuracy of the robot, has been realized. In some cases, high-precision work has been realized by controlling force instead of force.
[0003]
Generally, force control, compliance control, impedance control, damping control, hybrid control, and the like are typical methods for controlling the force of a robot.
[0004]
Further, in the prior art described in Japanese Patent Application Laid-Open No. 4-141386, a force control is performed so that the fitting of the convex fitting portion and the concave fitting portion and the fitting and fastening of the bolt and nut can be reliably performed in one trial. A method, a method of detecting a state, and a work procedure are described.
[0005]
In the prior art described in Japanese Patent Application Laid-Open No. 4-141387, when assembling a truss structure, the speed of the hand is calculated based on the force received by the force sensor based on a motion equation including a viscous term excluding an inertia term and a spring term. Then, the position of the hand is controlled, and the assembling operation is performed by the damping control, so that the assembling operation can be performed with a small amount of calculation.
[0006]
In addition, Journal of the Robotics Society of Japan, Vol. 8, No. In the "manipulation system based on skills" described in 3, the skills are defined as basic operations for realizing complicated tasks by the manipulator, and the data inside the computer and the real environment are controlled by force control and the use of sensors for each skill. The difference between the two is absorbed to achieve a reliable operation.
[0007]
[Problems to be solved by the invention]
However, in compliance control and impedance control, if there is a large error between the target position and the actual component position, an excessive force is applied to the component, or the spring, viscosity, mass, etc. In addition, there is a problem that it is difficult to set the value of the coefficient, and there is a limit that in the fitting operation, the operation cannot be successful unless the convex fitting portion can be directly brought into contact with the concave fitting portion to the inside and outside contact state. In addition, since a method for detecting a contact state with a part at the time of performing a work has not been established, when performing a complicated work by combining a series of works, not only is it difficult to determine when to switch the work, However, it was not always easy to detect that the operation failed. There is an example in which, as in the prior art described in Japanese Patent Application Laid-Open No. 4-141386, a specific work is limited to a specific work, a series of work procedures for realizing the work are specified, and a method for detecting the realization of each work is devised. However, it was only a method applicable to limited tasks.
[0008]
Journal of the Robotics Society of Japan, Vol. 8, No. In the “manipulation system based on skills” described in 3 above, skills are defined as basic tasks for realizing a complex task by a combination thereof, and a method of realizing the method, a method of detecting the realization, and a detection of a change in a contact state are defined. Methods have also been devised. However, the contact state detection method described in this document detects a change in the movement amount of the manipulator tip as a change in the instantaneous rotation center, and uses a speed reducer as used in the experiments in this document. Direct drive manipulators are effective because the absolute position of the manipulator tip can be accurately measured and controlled.However, manipulators that use commonly used decelerators are relatively effective due to the force applied due to contact or collision with the object. It is not always easy to accurately detect a change in the instantaneous rotation center as described in this document due to the occurrence of deflection in the low-rigidity speed reducer.
[0009]
Accordingly, an object of the present invention is to provide a case where positioning is not performed with sufficient accuracy by a jig or a component supply device, or a robot that is positioned by a jig or a component supply device but has a mounting error or a joint of the robot on site. In cases where it is not possible to execute work only with position control based on offline teaching without teaching or calibration at the site due to errors due to deflection in the part, or for fitting work that is more accurate than the robot's accuracy, etc. An object of the present invention is to provide a robot capable of performing a work with a reliable operation only by off-line teaching without performing on-site teaching or calibration.
[0010]
[Means for Solving the Problems]
In order to achieve the above object, a robot according to the present invention is a robot that places a workpiece held by a hand of a manipulator in a predetermined relationship with an object, wherein the manipulator control unit controls the position and force of the hand. A contact state detection unit that determines whether or not the work is placed in a predetermined relationship with the target object, and holds a previously input command, interprets this command, and refers to the determination result by the contact state detection unit. An instruction holding and executing unit for calculating a control coefficient of the manipulator control unit and transferring the control coefficient to the manipulator control unit; The command includes a contact command for causing the work to move linearly and contact the object, and the control coefficient moves the work at a specified speed in a direction specified in the command in the contact command; After the contact with the target object, the contact command includes a contact command control coefficient for maintaining the contact by pressing with a specified force or torque, and the contact state detecting unit includes, in the contact command, the contact specified in the command. The difference between the position and the actual position and the value of the allowable position error specified in this command, the difference between the pressing force specified in this command and the actual force, and the manipulator tip position within a certain time It is determined whether or not the target contact has been realized based on the variation amount, It is characterized by the following.
[0011]
Also, In a robot that places a work held by a hand of a manipulator in a predetermined relationship with an object, a manipulator control unit that controls the position and force of the hand, and whether the work is placed in a predetermined relationship with the object. A contact state detection unit for determining the input state, holding a command input in advance, interpreting the instruction, and calculating a control coefficient of the manipulator control unit with reference to a determination result by the contact state detection unit. An instruction holding and executing unit for transferring to the manipulator control unit; The command includes a tracing command for rotating the work in a state where the work is in contact with the target and causing the work to make ridge contact with the target, and the control coefficient is set in the direction specified in the command in the tracing command. The profile state control unit includes a profile command control coefficient for realizing and maintaining a ridge line contact with the target object by rotating the workpiece, and the contact state detection unit determines the actual position of the contact with the contact position specified in the command in the profile command. The target is determined by the difference from the position of the actuator, the value of the allowable position error specified in this command, the value of the moment around the rotation axis specified in this command, and the amount of change in the manipulator tip position within a certain time. Is determined whether or not the contact has been realized.
Also, In a robot that places a work held by a hand of a manipulator in a predetermined relationship with an object, a manipulator control unit that controls the position and force of the hand, and whether the work is placed in a predetermined relationship with the object. A contact state detection unit for determining the input state, holding a command input in advance, interpreting the instruction, and calculating a control coefficient of the manipulator control unit with reference to a determination result by the contact state detection unit. An instruction holding and executing unit for transferring to the manipulator control unit; The command includes a fitting command for rotating the work in contact with the object to make the surface of the work and the surface of the object constrained in a state of contact with the object, and the control coefficient includes the fitting command of the fitting command. Rotating the work at the specified angular velocity in the direction specified in, includes a fitting command control coefficient for realizing and maintaining the constraint state between the work surface and the target object surface, the contact state detection unit includes In the fitting command, the difference between the contact position specified in this command and the actual position, the value of the allowable position error specified in this command, the rotation axis specified in this command, The target restraint condition is determined by the difference between the actual moment and the actual moment, the amount of change in the manipulator tip position within a certain period of time, and the value of the force in the direction in which movement is possible as a result of the fit specified in this command. There it is determined whether or not realized, characterized in that.
Also, an environment model having the robot's working environment and the robot's own three-dimensional geometric information, and having information on the maximum value that can occur as an error between a predetermined positional relationship between the workpiece and the object and the actual positional relationship. Unit, a user interface unit capable of interpreting a higher-order contact command higher than the contact command, and a work designated in the upper-order contact command when moved in a direction designated in the command. The position at which the contact between the surface and the object occurs is derived by using the value inside the environment model, and the argument is determined by the value of the position error included in the environment model part and the argument included in the upper-level contact instruction. And a higher-level instruction translating unit that generates the value of the contact instruction and transfers the contact instruction to the instruction holding and executing unit.
Also, an environment model having the robot's working environment and the robot's own three-dimensional geometric information, and having information on the maximum value that can occur as an error between a predetermined positional relationship between the workpiece and the object and the actual positional relationship. Unit, a user interface unit capable of interpreting a higher-order copying command which is higher than the above-mentioned copying command, and a higher-order copying command specified in this command when the work is rotated in the direction specified by this command. The posture at which the contact between the surface and the ridge line occurs is calculated by using the value inside the environment model, and the value of the argument is determined by the value of the position error included in the environment model portion and the argument included in the higher-order copying instruction. A high-order instruction translating unit that generates a value and transfers the copying instruction to the instruction holding / executing unit.
Also, an environment model having the robot's working environment and the robot's own three-dimensional geometric information, and having information on the maximum value that can occur as an error between a predetermined positional relationship between the workpiece and the object and the actual positional relationship. Unit, a user interface unit capable of interpreting a higher-order fitting command that is above the fitting instruction, and a higher-order fitting instruction specified in this command when rotated in the direction specified in the command. The calculated value of the posture in which the two surfaces of the workpiece surface and the target object surface are parallel to each other, and a command from among directions in which the contact command is executed before the higher-level fitting command is executed. From the directions of the two surfaces specified in the above, the direction in which the movement is possible is obtained, and the position error value included in the environment model unit and the argument included in the higher-level fitting instruction are obtained. A high instruction translation unit for transferring instructions fit said generated values of the argument to the instruction holding execution unit, and further comprising a.
[0012]
[Action]
According to the present invention, when the operator has previously input a contact command off-line, the command holding and executing unit transfers a value in accordance with the input command to the manipulator control unit, so that the manipulator can be operated. It moves in a specified direction at a specified speed, and after the contact between the work held by the hand of the manipulator and the object, the contact is maintained by the specified force. On the other hand, the contact state detector detects that the difference between the contact position specified in the command and the actual position is smaller than the value of the position error specified in the command, and that the pressing force specified in the command and the actual It is only after confirming that the force difference is smaller than the default value of the force error, and that the amount of change in the manipulator tip position within a certain period of time is smaller than the reference value and that the manipulator is almost stationary. Then, by transferring the result to the instruction holding and executing unit, it is possible to reliably determine whether or not the target contact has been realized by the linear motion. If the command has been input in advance, the command holding and executing unit transfers the value according to the input command to the control unit of the robot. To achieve and maintain ridge line contact.
[0013]
On the other hand, the difference between the contact position specified in the command and the actual position is smaller than the value of the position error specified in the command, and the moment about the rotation axis specified in the command. Is smaller than the default value of the force error, and furthermore, the manipulator tip position is less stationary than the reference value. Is transferred to the instruction holding and executing unit, it is possible to reliably determine whether or not the target contact has been realized by the rotational movement. When a fitting command is input in advance, the command holding and executing unit transfers a value according to the input command to the control unit of the robot, so that the robot rotates in the designated direction and thereby the target object is rotated. Realize and maintain the state of inner and outer restraint with. On the other hand, the contact state detector detects that the difference between the contact position specified in the command and the actual position becomes smaller than the value of the position error specified in the command, and the rotation around the rotation axis specified in the command. Make sure that the moment is smaller than the value of the moment specified in the command and the default moment error value, and that the amount of change in the manipulator tip position within a certain time is smaller than the reference value and that it is almost stationary. , Or that the force value in the direction in which movement is possible as a result of the fit specified in the command is less than the reference value for a certain period of time, and then it is determined that the target contact has been achieved. Is transferred to the instruction holding and executing unit, it is possible to reliably determine whether or not the target contact has been realized by the rotational movement.
[0014]
As described above, the operation of realizing the point contact, the operation of realizing the ridge line contact, and the operation of the mating operation can be reliably realized by the contact command, the copying command, the fitting command, and the operation according to the situation. Even if there is a case where it is not possible to realize, it is possible to detect the state, if it is not positioned with sufficient accuracy by the jig or component supply device, or if it is positioned by the jig or component supply device, Due to errors such as robot installation errors at robots and deflections at robot joints, it is impossible to perform work only with position control using offline teaching without teaching or calibration at the site, Even when performing fitting work with higher precision as described above, without performing on-site teaching or calibration, It is possible to perform a variety of tasks in a reliable operation by teaching only, it is possible to perform an operation corresponding the situation.
[0015]
Further, according to the present invention, when the operator inputs a higher-level contact command, the user interface unit transfers the command to the higher-level command translator, and the higher-level command translator moves in the direction specified in the command. At this time, the position where the contact between the two surfaces specified in the instruction occurs is calculated by using the value inside the environmental model, and is included in the value of the allowable position error included in the environmental model part and the upper contact instruction. The contact command is transferred to the command holding execution unit by generating the value of the argument by the argument to be executed. When the operator inputs a high-order command, the user interface unit also transfers the command to the high-order command translation unit. Calculates the posture at which the contact between the surface and the ridge line specified by the above occurs by using the value inside the environment model, and the argument by the value of the allowable position error included in the environment model part and the argument included in the upper-level copying instruction Is transferred to the instruction holding and executing unit. When the operator inputs a higher-order fitting command, the user interface unit also transfers the command to the higher-order command translating unit, and the upper-order command translating unit rotates in the direction specified in the command when the higher-order fitting command is issued. Calculate the posture value where the two surfaces specified in the command are parallel when the command is executed, and select the direction from the direction in which the contact command is executed before executing the upper fitting command. The movable direction is determined from the directions of the two designated surfaces, and the value of the allowable position error included in the environment model portion and the value of the argument included in the upper-level fitting instruction are generated, and the fitting instruction is generated. Transfer to the instruction holding execution unit.
[0016]
As a result, when the upper contact command is input, contact between the surfaces specified in the command is realized, and when the upper copy command is input, the ridge line and the surface specified in the command are When the upper fitting command is input, the inner and outer restraint states realized by the planes specified in the command being parallel are reliably realized. The operator can easily teach off-line work by specifying the name or number of the surface or ridgeline of the object defined inside the environment model of the system by the upper contact command, upper copy command, and upper fitting command as described above. Can be performed.
[0017]
【Example】
Hereinafter, embodiments will be described with reference to the drawings. FIG. 1 shows a schematic configuration of a first embodiment of the present invention.
[0018]
In this figure, the robot includes a command holding and executing unit 1, a manipulator control unit 2, a contact state detecting unit 3, and a manipulator 4. The manipulator 4 is provided with an encoder for measuring a joint angle on each joint axis, and the wrist is provided with a six-axis force sensor for detecting a force / torque applied to the hand. The manipulator control unit 2 receives the position of the control point and the value of the force / torque at the control point transferred from the command holding and executing unit 1 as input, and also inputs the impedance coefficient transferred from the command holding and executing unit 1. Impedance control is performed according to the value.
[0019]
FIG. 2 shows a schematic configuration of the manipulator controller 2. The manipulator controller 2 executes command holding and execution so that the target position in the object coordinate system transferred from the command holding and executing unit 1 and the target force / torque at the control point expressed in the object coordinate system are obtained. The control is performed based on the impedance coefficient transferred from the unit. The virtual model calculation unit 5 calculates the position and speed of a virtual model composed of a spring, a mass, and a damper based on the target position, force, and impedance coefficient input from the outside, and calculates the virtual model target position of the control point.・ Calculate the speed. In addition, the virtual model independently calculates each axis of x, y, z, roll, pitch, and yaw based on each impedance coefficient. Roll, pitch, and yaw are rotations around the x-axis, y-axis, and z-axis, respectively. The method of calculating the position and velocity values of the virtual model will be described below.
[0020]
The impedance coefficient is k, m, and c for each of the spring, the mass, and the damper. Also, the target position xp of the control point, the target force fp, and the value of the force / torque subjected to control point conversion and gravity compensation are set to f, and the virtual model target position is set to xm.
[0021]
a) When m is not 0

Then, (d2 / dt2) △ x is calculated. (1) The value of {x, (d / dt)} x on the right side of the expression uses the value in the immediately preceding control cycle. When (d2 / dt2) △ x is calculated, the values of (x / d / dt) △ x are updated by sequentially calculating the values of (d / dt) △ x and △ x by numerical integration.
[0022]
b) When m is 0
(D / dt) △ x = (△ fk △ x) / c (2)
Then, (d / dt) △ x is calculated. (2) The value of Δx on the right side of the equation uses the value in the immediately preceding control cycle. When (d / dt) △ x is calculated, the value of △ x is also obtained and updated by numerical integration. When the target force fp is considered when m = k = 0, the manipulator moves at a constant speed of fp / c when not in contact with any part, and when the manipulator comes into contact with the object, a constant force is applied. Damping control in which force control is performed at fp is realized.
[0023]
In the conversion unit 6 to the tip position, the displacement xe to the control point in the manipulator tip coordinates and the displacement xo from the base coordinate system to the object coordinate system transferred from the instruction holding and executing unit, A target value of the position / velocity of the tip of the manipulator in the base coordinate system that realizes the value of the virtual model target position / velocity of the control point in the object coordinate system is calculated. The tip position control device 7 performs feedback control so as to achieve the target position / velocity value of the manipulator tip. The gravity compensator 8 removes the gravity component of the hand / gripped object attached earlier than the force sensor from the value of the 6-axis force sensor signal attached to the wrist of the manipulator. The gravitational component is obtained by calculating the attitude of the force sensor using the values of encoders attached to each axis of the manipulator. The control point force conversion unit 9 applies a force sensor based on the displacement from the manipulator tip to the control point and the displacement from the base coordinate system to the object coordinates transferred from the command holding execution unit. The force / torque value excluding the existing gravity component is converted into a force / torque value at a control point expressed in the object coordinate system based on a static balance relational expression. Therefore, the position of the control point is impedance-controlled by the manipulator controller to the target position in the object coordinate system by the impedance coefficient expressed in the object coordinate system.
[0024]
FIG. 3 is a diagram showing a definition of a coordinate system. The base coordinate system 11 is defined at a place fixed to the workplace, and the object coordinate system 12 is defined to be fixed to each object. The manipulator tip coordinate system 13 is defined at the tip of the manipulator 10 and moves with the movement of the tip of the manipulator.
[0025]
Hereinafter, a contact command for realizing contact with the object by linear movement in the robot having the above configuration, a command for realizing ridge line contact by performing a rotary motion while already in contact with the object, and The operation of the robot when the operator inputs in advance a fitting command for realizing a state of inner and outer restraint with the object by performing a rotary motion while in contact with the robot will be described with reference to FIG.
[0026]
The contact, follow, and fit commands are defined as shown in Table 1.
[0027]
[Table 1]

When the contact command touch is input, the command is sent to the command holding and executing unit 1.
[0028]
In the instruction holding execution unit 1, the value included in the argument is
c = force / vel (3)
Calculate the damper coefficient as shown below and send it to the manipulator controller 2.
・ As the impedance coefficient of the “axis” axis,
m = k = 0, c = force / vel
"Xe" as the displacement to the control point in the manipulator tip coordinate system
"Xo" as the displacement of the object coordinate system from the base coordinate system
"Force" as the target force in the "axis" axis direction of the target
Transfer the current control point position in the object coordinate system as the "axis" axis coordinate value of the target position. Then, it is set so as to perform a damping control of moving at a speed of “vel” in the “axis” axis direction of the object coordinate system before the contact, and performing a force “force” in the “axis” direction after the contact.
[0029]
On the other hand, the command input from the operator and the coordinate value x0 in the object coordinate system of the control point at the time of command transfer at the time of starting the operation of the robot are sent from the command holding execution unit 1 to the contact state detection unit 3. .
[0030]
The state determination process of the contact state detection unit 3 will be described with reference to FIG. The state determination process is executed every control cycle. First, in step s11, the maximum value and the minimum value of the value of the "axis" axis coordinate value of the control point in the object coordinate system for 0.5 seconds are calculated, and the difference is defined as Ma * Min. Next, in step s12, the value xp of the “axis” axis coordinate value at the contact point designated by the command and the current value of the “axis” axis coordinate value x are

When the current position deviates from the allowable range and goes too far, it is determined that the operation has failed. If it is not excessive, in step s13, the value xp of the “axis” axis coordinate value at the contact point designated by the command and the value of the current “axis” axis coordinate value x are

When the current position is out of the allowable range and closer to the front, the process proceeds to step s14. In step s14, it is determined whether or not the value of the variation amount Ma × Min is smaller than the permissible value Δv from 0.5 seconds before. If it is smaller, it is determined that the vehicle has stopped just before the permissible range. Is determined to have failed. On the other hand, if it is larger than △ v, it is determined that the exercise is still in progress, and it is determined that the exercise is in progress. If it is determined in step s13 that it is not the near side, the process proceeds to step s15. In step s15, the difference between the target force fp specified by the command and the force f at the control point represented by the current object coordinate system is calculated. Since it is considered that there is, it is determined that it is still being executed. On the other hand, if it is within the allowable range, the process proceeds to step s16. In step s16, it is determined whether or not the value of the variation amount Ma × Min from 0.5 seconds before is smaller than the allowable value 、 v. If smaller, the target force near the position specified in the instruction is determined. It can be considered that the contact is made and the vehicle is stably stationary, so that it is determined that the target contact has been realized. On the other hand, if the value of the variation amount Ma × Min is larger than △ v, it is determined that it is still immediately after the collision and it is determined that the operation is being performed.
[0031]
According to the above procedure, the contact state detection unit 3 responds to the instruction holding execution unit 1 with any result of execution, failure, or completion. When a response indicating completion is returned, the instruction holding execution unit 1 transfers the value to the manipulator control unit 2 and the contact state detection unit 3 again in accordance with the next instruction stored in advance. When a failure response is returned, the value is transferred to the manipulator control unit 2 and the contact state detection unit 3 according to the previously stored instruction at the time of failure, if any. If no command has been issued, an operation set by default, for example, a movement operation by position control to a reference posture is performed.
[0032]
When the copying instruction fit is input, the instruction is sent to the instruction holding and executing unit 1. In the instruction holding execution unit 1, the manipulator control unit 2
・ As the impedance coefficient of the “axis” axis,
m = k = 0, c = Cf
"Xe" as the displacement to the control point in the manipulator tip coordinate system
"Xo" as the displacement of the object coordinate system from the base coordinate system
・ 0 as the target force in the target “axis” axis direction
Transfer the current control point position in the object coordinate system as the "axis" axis coordinate value of the target position. Cf is a damper coefficient for impedance control at the time of a copy command set by default. It is desirable that the control point position is set to the center position of the ridge line for realizing the ridge line contact. In addition, one of the two end points of the edge line for realizing the edge line contact must be in contact before the copy command is executed. Then, it is set so as to control the torque 0 in the “axis” direction in the object coordinate system, so that the ridge line contact is realized in a controlled manner.
[0033]
On the other hand, the command input from the operator and the coordinate value x0 in the object coordinate system of the control point at the time of command transfer at the time of starting the operation of the robot are sent from the command holding execution unit 1 to the contact state detection unit 3. .
[0034]
The state determination process of the contact state detection unit 3 will be described with reference to FIG. The state determination process is executed every control cycle. First, in step s21, the maximum value and the minimum value of the value of the "axis" axis coordinate value of the control point in the object coordinate system for 0.5 second are calculated, and the difference is defined as Ma * Min. Next, in step s22, the value xp of the “axis” axis coordinate value at the contact point designated by the command and the current value of the “axis” axis coordinate value x are

When the current position deviates from the allowable range and goes too far, it is determined that the operation has failed. If it is not excessive, in step s23, the value xp of the “axis” axis coordinate value at the contact point designated by the command and the value of the current “axis” axis coordinate value x are

When the current position deviates from the allowable range and is on the near side, the process proceeds to step s24. In step s24, it is determined whether or not the value of the variation amount Ma × Min is smaller than the permissible value Δv from 0.5 seconds before. If it is smaller, it is determined that the vehicle has stopped just before the permissible range. Is determined to have failed. On the other hand, if it is larger than △ v, it is determined that the exercise is still in progress, and it is determined that the exercise is in progress. If it is determined in step s23 that it is not the near side, the process proceeds to step s25. In step s25, the magnitude of the force f at the control point expressed in the current object coordinate system is calculated, and if it is out of the allowable range, it is considered that it is immediately after the collision or just before the contact, so it is still being executed. It is determined that there is. On the other hand, if it is within the allowable range, the process proceeds to step s26. In step s26, it is determined whether or not the value of the variation amount Ma × Min from 0.5 seconds ago is smaller than the allowable value 、 v. If smaller, the target force near the position specified in the command is determined. It can be considered that the contact is made and the vehicle is stably stationary, so that it is determined that the target contact has been realized. On the other hand, if the value of the variation amount Ma × Min is larger than △ v, it is determined that it is still immediately after the collision and it is determined that the operation is being performed.
[0035]
According to the above procedure, the contact state detection unit 3 responds to the instruction holding execution unit 1 with any result of execution, failure, or completion. The command holding execution unit 1 performs an operation according to the response result as in the case of the contact command.
[0036]
When the fitting instruction parallel is input, the instruction is sent to the instruction holding and executing unit 1.
[0037]
In the instruction holding execution unit 1, the manipulator control unit 2
・ As the impedance coefficient of the “axis” axis,
m = k = 0
c = force / vel
"Xe" as the displacement to the control point in the manipulator tip coordinate system
"Xo" as the displacement of the object coordinate system from the base coordinate system
"Force" as the target force in the "axis" axis direction of the target
Transfer the current control point position in the object coordinate system as the "axis" axis coordinate value of the target position. Note that it is necessary for the book fitting instruction to be already in contact with the object for realizing the inside / outside constraint before being executed. In addition, it is necessary to perform control for pressing in the “faxis” direction. Normally, it is necessary to have already realized the contact command in the “faxis” direction.
[0038]
Then, when the two surfaces for which the inner and outer restraint states are to be realized by rotating at an angular velocity of “vel” in the “axis” axis direction of the object coordinate system become parallel, control is performed in the “fax” direction. 6 (b), the state shifts to the inner / outer restraining state as shown in FIG. 6 (c). After that, the rotational movement is continued, and when the contact state is reached at two places as shown in FIG. 6D, the rotational movement is stopped and the contact state is established with the torque of "force".
On the other hand, the command input from the operator and the coordinate value x0 in the object coordinate system of the control point at the time of command transfer at the time of starting the operation of the robot are sent from the command holding execution unit 1 to the contact state detection unit 3. .
[0039]
The state determination process of the contact state detection unit 3 will be described with reference to FIG. The state determination process is executed every control cycle. First, in step s31, the maximum value and the minimum value of the value of the "axis" axis coordinate value of the control point in the object coordinate system for 0.5 seconds are calculated, and the difference is set to Ma * Min. Next, in step s32, the minimum value obtained by subtracting the absolute value of the actual force value from the absolute value of the target force in the "fax" direction for 0.2 seconds is calculated as fffree. Next, in step s33, the value xp of the “axis” axis coordinate value at the contact point designated by the command and the value of the current “axis” axis coordinate value x are

When the current position deviates from the allowable range and goes too far, it is determined that the operation has failed. If it is not excessive, in step s34, the value xp of the “axis” axis coordinate value at the contact point designated by the command and the value of the current “axis” axis coordinate value x are

When the current position deviates from the allowable range and is on the near side, the process proceeds to step s35. In step s35, it is determined whether or not the value of the variation amount Ma × Min is smaller than the allowable value ０．５v from 0.5 seconds before. If it is smaller, it is determined that the vehicle has stopped just before the allowable range. Is determined to have failed. On the other hand, if it is larger than △ v, it is determined that the exercise is still in progress, and it is determined that the exercise is in progress. If it is determined in step s34 that it is not the near side, the process proceeds to step s36. In step s36, a comparison is made between the value of ffree calculated as to how much the force in the "faxis" direction is smaller than the target force value and the value of $ free, which is set by default as the maximum amount of force control error. When ffree is larger than △ free, only one point is brought into contact at the time of transition to the inner / outer restraint state (FIG. 6C), and it is possible to move in the “faxis” direction. It is determined that the operation is completed. Only when the force value that can be regarded as one-point contact is continued for a certain length of time of 0.2 seconds, it is considered that the inner and outer restraints are realized only for the time being. With such a determination procedure, it is possible to prevent erroneous recognition due to noise of the force sensor, impact due to contact, vibration due to control, and the like. On the other hand, if the value of ffree is smaller than △ free in step s36, the process proceeds from step s37, which is a determination as to whether or not the contact at two places corresponding to FIG. In step s37, the difference between the target torque fp specified by the command and the force torque at the control point represented by the current object coordinate system is calculated. If the difference is outside the allowable range, immediately after the collision or before the two-point contact. Therefore, it is determined that the program is still being executed. On the other hand, if it is within the allowable range, the process proceeds to step s38. In step s38, it is determined whether or not the value of the variation amount Ma × Min from 0.5 seconds ago is smaller than the allowable value 、 v. If smaller, the target force near the position specified in the command is determined. It can be considered that the contact is made and the vehicle is stably stationary, so that it is determined that the target contact has been realized. On the other hand, if the value of the variation amount Ma × Min is larger than △ v, it is determined that it is still immediately after the collision and it is determined that the operation is being performed.
[0040]
According to the above procedure, the contact state detection unit 3 responds to the instruction holding execution unit 1 with any result of execution, failure, or completion. The instruction holding execution unit 1 performs an operation according to the response result as in the case of the contact / tracing instruction.
[0041]
With the above-described operation, a series of instructions input in advance by the operator and stored in the instruction holding and executing unit can be executed without fail, and completion / failure of the operation can be detected. Therefore, the operator has previously performed a procedure for realizing the target assembly, a contact command for realizing contact with the object by linear movement, and a ridge line contact by performing a rotary motion while already in contact with the object. And a fitting command to achieve a state of inner and outer restraint with the object by rotating while in contact with the object, and movement by ordinary position control and movement by impedance control When the robot is not positioned to a sufficient degree by a jig or component supply device, or is positioned by a jig or component supply device, Due to errors due to bending at the joints of the robot, teaching on-site and off-line teaching unless calibration is performed When it is not possible to perform work only with positioning control, or when performing fitting work that is more accurate than the robot's accuracy, reliable teaching can be performed only by offline teaching without performing on-site teaching or calibration. The operation can be performed by the operation.
[0042]
FIG. 8 is a block diagram showing a second embodiment of the present invention. The robot includes a user interface 16, a higher-level command translator 17, an environment model 18, a command holding and executing unit 19, a manipulator control unit 20, a contact state detecting unit 21, and a manipulator 22. The environment model unit 18 has the robot's work environment and the robot's own geometric information in three dimensions, and also has therein the maximum value that can occur as an error between the position value of each object and the actual value. The data representation is a boundary representation, which is represented by a polyhedral approximation.
[0043]
In the following, a higher-level contact command that realizes point contact between two surfaces specified by linear movement in the robot having the above configuration, a surface and a ridgeline specified by performing a rotary motion while already in contact with the target object Upper-level instruction to achieve edge contact with the object, and upper-level fit that achieves a state of inner and outer restraint with the object by making two specified surfaces parallel by rotating while in contact with the object The operation of the robot when the operator inputs a command in advance will be described with reference to FIG.
[0044]
The upper contact command, the higher-order command, and the higher-fit command are defined as shown in Table 2.
[0045]
[Table 2]

When the upper contact command is input by the operator, the command is transferred from the user interface unit 16 to the upper command translator 17. The upper-level instruction translating unit 17 calculates the value of the argument of the contact instruction to be transferred to the instruction holding and executing unit while using the value of the environment model unit 18. First, the value of the displacement xo of the target object specified in the command from the base coordinate system of the coordinate system “Σo” is read from the environment model. The values of “axis”, “vel”, and “force” are directly used as arguments of the contact command. Further, a value Δx of the error amount included in the position / posture value of the object coordinate system “Δo” is read from the environment model. As the value of the displacement xe of the control point in the manipulator tip coordinate system, the value set when this command is executed is used as an argument of the contact command. Although the value of xe is automatically set by a higher-level copying command or other commands described later, it is assumed that the system also includes a dedicated command for setting and changing the value of xe. Finally, the position at which contact between the surfaces specified in the command occurs is obtained by the following procedure.
[0046]
First, the position of the manipulator control point in the object coordinate system “系 o” when the command is executed is x0. The position of the control point is moved from x0 in the “axis” axis direction in the positive direction if the value of “vel” is positive, and in the negative direction if the value of “vel” is negative in the direction of the axis. The presence / absence of contact between the surface “fa” and the surface “fb” is calculated based on the value of the environment model unit 18, and the value of the “axis” coordinate value in the object coordinate system “” o ”when contact first occurs Xp.
[0047]
The contact command is transferred to the command holding and executing unit 19 using the value obtained by the above processing as an argument. Thereafter, the same processing as in the first embodiment is performed, thereby realizing the contact between the surface “fa” and the surface “fb” and detecting the completion / failure thereof.
[0048]
When a higher-order command is input by the operator, the command is transferred from the user interface section 16 to the higher-order command translation section 17. The upper-level instruction translator 17 uses the values of the environment model 18 to determine the value of the argument of the contact instruction to be transferred by the instruction holding and executing unit. First, the value of the displacement xo of the target object specified in the command from the base coordinate system of the coordinate system “Σo” is read from the environment model. The value of “axis” is directly used as an argument of the contact command. Further, a value Δx of the error amount included in the position / posture value of the object coordinate system “Δo” is read from the environment model. The value of the displacement xe of the control point in the manipulator tip coordinate system is obtained by calculating the value of the center point of the ridge line "eb" specified in the command at the manipulator tip coordinate, and is used as an argument of the copying command. Lastly, the position where the contact between the surface and the ridge line specified in the instruction occurs is obtained by using the value of the environment model unit 18 in the same manner as in the case of the upper contact instruction, and is set as the argument “xp” of the copying instruction. . An instruction following the value obtained by the above processing as an argument is transferred to the instruction holding and executing unit 19, and the same processing as that of the first embodiment is performed, so that the ridge line contact between the surface “fa” and the ridge line “eb” is performed. Is realized and its completion / failure is detected. This instruction realizes ridge line contact between the ridge line “eb” of the object held by the manipulator and the surface “fa” of the object.
[0049]
When a higher-level fit command is input by the operator, the command is transferred from the user interface unit 16 to the higher-level command translator 17. The upper-level instruction translating unit 17 calculates the value of the argument of the contact instruction to be transferred to the instruction holding and executing unit by using the value of the environment model unit 18. First, the value of the displacement xo of the target object specified in the command from the base coordinate system of the coordinate system “Σo” is read from the environment model. The values of “axis”, “vel”, and “force” are used as arguments of the fitting instruction as they are. Further, a value Δx of the error amount included in the position / posture value of the object coordinate system “Δo” is read from the environment model. As the value of the displacement xe of the control point in the manipulator tip coordinate system, the value set when this command is executed is used as an argument of the contact command. The value of “xp” of the fitting instruction is controlled when the two specified faces “fa” and “fb” of the upper-level fitting instruction are parallel or approach the most parallel due to the rotation about the “axis” axis. The value of the position / posture of the point is obtained by using the value of the environment model unit 18, and the value of the posture about the "axis" axis is set. The value of “faxis” of the fitting command is any of x, y, and z. However, from the axes for which the contact command has been completed at the time when the upper-level fitting command is executed, the surface “fb” Is set as the axis whose direction is most different from the normal direction of. In other words, among the directions in which the pressing control is being performed, the direction closest to parallel to the surface fb, that is, the direction in which the translation is likely to occur when one-point contact is established, is set to “faxis”. The fitting instruction is transferred to the instruction holding and executing unit 19 using the value obtained by the above processing as an argument, and the same processing as that of the first embodiment is performed, so that the surface “fa” and the surface “fb” are changed. Realization of the inner and outer restraint states realized by being substantially parallel and detection of completion / failure thereof are performed. In this instruction, the surface “fa” is the surface of the object held by the manipulator, and the surface “fb” is the surface of the object.
[0050]
With the above-described operation, it is possible to reliably execute each of the commands as shown in the second example input in advance by the operator, and to detect the completion / failure of the operation. The upper-level contact instruction, the higher-level copying instruction, and the higher-level fitting instruction are instructions that specify the objects defined by the environment model unit 18 and the realization of contact between the surfaces and ridges of these objects, respectively. By combining this with an environment model having the above, a simulator or the like that displays the working environment on both sides based on the environment model data, the operator can easily teach the operation of the robot offline.
[0051]
In addition, when positioning is not performed with sufficient accuracy by a jig or component supply device, or when positioning is performed by a jig or component supply device, due to robot installation errors at the site or bending at the robot joints, etc. Even if it is not possible to execute work only by position control based on off-line teaching without teaching or calibration at the site due to errors, or when performing fitting work that is more accurate than the accuracy of the robot, reliable It is possible to teach the user to perform an operation.
[0052]
【The invention's effect】
As described above, according to the configuration of the present invention, it is possible to execute a target operation by each basic instruction without fail, and to operate while detecting completion or failure of the target operation. If the positioning is not performed with sufficient accuracy by the fixture or component supply device, or if the robot is positioned by the jig component supply device but there is a robot installation error at the site or an error due to bending at the robot joint, etc. If it is not possible to execute work only by position control using offline teaching without teaching or calibration of the robot, or when fitting work that is more accurate than the robot's accuracy, teaching and calibration at the site The operation can be performed with a reliable operation only by teaching offline without performing any operation.
[Brief description of the drawings]
FIG. 1 is a block diagram showing a schematic configuration of a first embodiment of the present invention.
FIG. 2 is a block diagram showing a configuration example of a manipulator controller shown in FIG. 1;
FIG. 3 is a diagram showing definitions of a manipulator and a coordinate system in a work environment.
FIG. 4 is a flowchart showing details of a determination algorithm in a contact state determination unit when a contact command is executed.
FIG. 5 is a diagram showing a flowchart illustrating details of a determination algorithm in a contact state determination unit when a copying instruction is being executed.
FIG. 6 is an explanatory diagram showing a change in a contact state in a fitting operation.
FIG. 7 is a flowchart showing details of a determination algorithm in a contact state determination unit when a fitting instruction is executed.
FIG. 8 is a block diagram showing a second embodiment of the present invention.
[Explanation of symbols]
1 Instruction holding execution unit
2 Manipulator controller
3 Contact state detector
4 Manipulator
5 Virtual model calculator
6 Conversion part to tip position
7 Tip position control device
8 Gravity compensation unit
9 Conversion unit to control point force
10 Manipulator
11 Base coordinate system
12 Object coordinate system
13 Manipulator tip coordinate system
14 pin
15 blocks
16 User interface section
17 Higher level instruction translator
18 Environmental Model Department
19 Instruction execution unit
20 Contact state detector
21 Manipulator control unit
22 Manipulator

Claims (6)

In a robot that places a workpiece held by a manipulator hand in a predetermined relationship with respect to an object,
A manipulator controller for controlling the position and force of the hand;
A contact state detection unit that determines whether the work is placed in a predetermined relationship with the target object,
A command for storing a command input in advance, interpreting the command, calculating a control coefficient of the manipulator control unit with reference to the determination result by the contact state detecting unit, and transferring the control coefficient to the manipulator control unit. And a holding execution unit ,
The command includes a contact command to move the work linearly and contact the target,
The control coefficient is such that in the contact command, the work is moved at a specified speed in a direction specified in the command, and after contact with the object, the contact is maintained by pressing with a specified force or torque. Including the contact command control factor for
In the contact command, the contact state detection unit includes a difference between a contact position specified in the command and an actual position, a value of an allowable position error specified in the command, and a value specified in the command. The difference between the applied pressing force and the actual force, and the amount of change in the manipulator tip position within a certain period of time determine whether or not the target contact has been realized,
A robot characterized in that: In a robot that places a workpiece held by a manipulator hand in a predetermined relationship with respect to an object,
A manipulator controller for controlling the position and force of the hand;
A contact state detection unit that determines whether the work is placed in a predetermined relationship with the target object,
A command for storing a command input in advance, interpreting the command, calculating a control coefficient of the manipulator control unit with reference to the determination result by the contact state detecting unit, and transferring the control coefficient to the manipulator control unit. Holding execution unit and
With
The command includes a command for rotating the work in a state where the work is in contact with the object and causing the work to make ridge line contact with the object,
The control coefficient includes, in the copying command, a copying command control coefficient for rotating the work in a direction designated in the command, realizing and maintaining ridge line contact with the object,
In the copying command, the contact state detection unit includes a difference between a contact position specified in the command and an actual position, a value of an allowable position error specified in the command, and a value specified in the command. It is determined whether or not the target contact has been realized based on the value of the moment about the rotation axis and the variation amount of the manipulator tip position within a predetermined time.
A robot characterized in that: In a robot that places a workpiece held by a manipulator hand in a predetermined relationship with respect to an object,
A manipulator controller for controlling the position and force of the hand;
A contact state detection unit that determines whether the work is placed in a predetermined relationship with the target object,
A command for storing a command input in advance, interpreting the command, calculating a control coefficient of the manipulator control unit with reference to the determination result by the contact state detecting unit, and transferring the control coefficient to the manipulator control unit. Holding execution unit and
With
The instruction includes a fitting instruction for rotating the work in a state where the work is brought into contact with the object and bringing the surface of the work and the surface of the object into a constrained state,
The control coefficient is used in the fit command to rotate the work at a specified angular velocity in a direction specified in the command, and to realize and maintain a constraint state between the work surface and the object surface. Including the fit command control coefficient,
The contact state detection unit, in the fitting command, the difference between the contact position and the actual position specified in this command and the value of the allowable position error specified in this command, The difference between the moment about the specified rotation axis and the actual moment, the amount of change in the manipulator tip position within a certain period of time, and the value of the force in the direction that can move as a result of the fit specified in this command Determine whether the target restraint state has been achieved,
A robot characterized in that: An environment model unit having the robot's work environment and the robot's own three-dimensional geometric information, and having information of a maximum value that can occur as an error between a predetermined positional relationship between the workpiece and the target object and an actual positional relationship; ,
A user interface unit capable of interpreting a higher-order contact command higher than the contact command;
In the upper contact command, a position in the environment model is used as a position at which a contact between the surface of the work specified in the command and the object occurs when moving in the direction specified in the command. Upper-level instruction translation that derives a value, generates an argument value based on a position error value included in the environment model unit and an argument included in the higher-level contact instruction, and transfers the contact instruction to the instruction holding and executing unit. Department and
The robot according to claim 1 , further comprising: An environment model unit having the robot's work environment and the robot's own three-dimensional geometric information, and having information of a maximum value that can occur as an error between a predetermined positional relationship between the workpiece and the target object and an actual positional relationship; ,
A user interface unit capable of interpreting a higher-order copying command which is higher than the above-mentioned copying command;
In the high-order copying command, by rotating the work in the direction specified by this command, the posture at which the contact between the surface and the ridge line specified in the command occurs by using a value inside the environment model. A high-level instruction translating unit that calculates and generates an argument value based on a position error value included in the environment model unit and an argument included in the high-level copying instruction, and transfers the copying instruction to the instruction holding / executing unit When,
The robot according to claim 2 , further comprising: An environment model unit having the robot's work environment and the robot's own three-dimensional geometric information, and having information of a maximum value that can occur as an error between a predetermined positional relationship between the workpiece and the target object and an actual positional relationship; ,
A user interface unit capable of interpreting a higher-level fit command that is higher than the fit command;
In the upper fitting instruction, a value of a posture in which two surfaces of a work surface and an object surface specified in the instruction are parallel when rotated in a direction specified in the instruction is calculated. In addition, before the upper fitting instruction is executed, a direction that can be moved from directions of two surfaces specified in the instruction from among directions in which the contact instruction is executed is obtained from the direction in which the contact instruction is executed, and the environment model unit is obtained. A high-level instruction translating unit that generates a value of an argument based on a position error value included in the high-level fitting instruction and an argument included in the high-level fitting instruction and transfers the fitting instruction to the instruction holding and executing unit;
The robot according to claim 3 , further comprising:

Images