(57)【要約】【課題】  センサ座標系を気にせず、ワーク寸法の入力も必要とせずに、計測を含む把持動作の教示、再生をできるようにする。【解決手段】  ロボット制御装置１０２において、手動操作でグリッパ１０７を把持位置へ移動して教示位置として記録し、前記教示位置を中心としてあらかじめ決められた広さを計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状をマスタデータとして前記教示位置と関連づけて記録する。前記マスタデータと計測データを比較する。マスタデータに対する計測データの３次元の位置姿勢を計算して修正量とする。前記修正量にしたがって把持位置を修正し、グリッパ１０７を把持位置へ移動する指令を生成する。  【選択図】図１
2010172969.tif 000002
【特許請求の範囲】
【請求項１】
ワークを把持するエンドエフェクタと、前記ワークの形状計測センサと、を備えたロボットの制御装置において、
前記エンドエフェクタを把持位置へ移動させて教示位置として記録し、前記教示位置を中心としてあらかじめ決められた範囲を計測範囲とし、前記計測範囲をワークの形状計測手段により計測したワーク形状をマスタデータとして前記教示位置と関連づけて記録する教示ステップと、
前記教示位置を中心としてあらかじめ決められた範囲を計測範囲とし、前記計測範囲をワークの形状計測手段により計測したワーク形状を計測データとし、前記マスタデータと前記計測データを比較し、前記マスタデータに対する前記計測データの３次元の位置姿勢を計算して修正量とし、前記修正量にしたがって前記把持位置を修正し、前記エンドエフェクタを前記修正された把持位置へ移動する指令を生成する再生ステップと、を含むことを特徴とするロボット制御方法。
【請求項２】
前記再生ステップは、ロボットの動作プログラムの１命令で実行されることを特徴とする請求項１記載のロボット制御方法。
【請求項３】
前記計測範囲は、スキャン角度と、スキャン距離およびスキャン速度によって決定されることを特徴とする請求項１記載のロボット制御方法。
【請求項４】
前記マスタデータとして、計測したワーク形状から抽出された特徴データをメモリに記録することを特徴とする請求項１記載のロボット制御方法。
【請求項５】
ワークを把持するエンドエフェクタと、前記ワークの形状計測手段と、を備えたロボットの制御装置において、
前記ワーク把持後に移動する目標位置へ手動操作でエンドエフェクタを移動させて第１教示位置として記録し、前記第１教示位置を中心としてあらかじめ決められた範囲を計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状を第１マスタデータとして前記第１教示位置と関連づけて記録し、
エンドエフェクタを把持位置へ移動させて第２教示位置として記録し、前記第２教示位置を中心としてあらかじめ決められた広さを計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状を第２マスタデータとして前記第２教示位置と関連づけて記録する教示ステップと、
前記第１教示位置を中心としてあらかじめ決められた範囲を計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状を第１計測データとし、前記第１マスタデータと前記第１計測データを比較し、第１マスタデータに対する第１計測データの３次元の位置姿勢を計算して第１修正量とし、
前記第２教示位置を中心としてあらかじめ決められた範囲を計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状を第２計測データとし、前記第２マスタデータと前記第２計測データを比較し、前記第２マスタデータに対する前記第２計測データの３次元の位置姿勢を計算して第２修正量とし、
前記第２修正量にしたがって把持位置を修正し、エンドエフェクタを把持位置へ移動してワークを把持する指令を生成し、
前記第１修正量にしたがって把持後に移動する目標位置を修正し、ワークを把持したエンドエフェクタを修正した目標位置へ移動する指令を生成する再生ステップと、を含むことを特徴とするロボット制御方法。
【請求項６】
ワークを把持するエンドエフェクタと、前記ワークの形状計測手段と、を備えた双腕ロボットの制御装置において、
一方の腕のエンドエフェクタで把持した前記ワークに対して、前記ワークの形状計測手段を備えた他方の腕のエンドエフェクタをワーク計測位置へ移動させて教示位置として記録し、前記教示位置を中心としてあらかじめ決められた範囲を計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状をマスタデータとして前記教示位置と関連づけて記録する教示ステップと、
前記教示位置を中心としてあらかじめ決められた範囲を計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状を計測データとし、前記マスタデータと前記計測データを比較し、前記マスタデータに対する前記計測データの３次元の位置姿勢を計算して修正量とし、前記修正量にしたがってワークをマスタデータの位置へ移動するために、ワークを把持している腕の位置を修正して移動する指令を生成する再生ステップと、を含むことを特徴とするロボット制御方法。
【請求項７】
前記再生ステップは、前記教示位置を中心としてあらかじめ決められた範囲を第１計測範囲として決定し、前記第１計測範囲をワークの形状計測手段により第１回目の計測を行い、計測したワーク形状を第１回目の計測データとし、前記マスタデータと前記第１回目の計測データを比較し、前記第１回目の計測データをマスタデータに一致させるにはデータが足りない場合には、前記第１計測範囲からワークに対して姿勢を変更した第２計測範囲を生成し、前記第２計測範囲をワークの形状計測手段により第２回目の計測を実行し、第１回目の計測データと第２回目の計測データを重ね合わせて計測データとし、前記マスタデータに対する前記計測データの３次元の位置姿勢を計算して修正量とし、前記修正量にしたがって把持位置を修正し、前記エンドエフェクタを前記修正した把持位置へ移動する指令を生成するステップであることを特徴とする請求項１記載のロボット制御方法。
【請求項８】
アームにワークを把持するエンドエフェクタを備えた双腕ロボットであって、前記アームに前記ワークの形状を計測するためのセンサを備えた双腕ロボットと、
前記双腕ロボットを制御する制御装置であって、ロボットの動作が記述された動作プログラムが保存されるメモリと、前記動作プログラムを解析する解析部と、現在の位置を教示位置として動作プログラムに書き込む教示データ記憶部と、前記センサから前記ワーク形状の計測データを得る計測部と、教示時に得た前記計測データをマスタデータとしてメモリに記憶するマスタデータ記憶部と、前記マスタデータと計測データとを比較して、その比較結果である変化量を修正量として出力する比較部と、所定のスキャン条件および前記修正量に基づいて前記アームの軌道を生成する指令生成部と、前記生成された軌道に従ってモータを駆動するサーボ制御部と、を備えた制御装置と、
前記制御装置に接続される教示装置であって、前記双腕ロボットの動作指令を教示する教示装置と、を備えたことを特徴とするロボットシステム。
【請求項９】
前記マスタデータは、ヘリカルスキャンの軌道に沿って前記ワークの形状計測手段により計測したワーク形状の計測データであることを特徴とする請求項１記載のロボット制御方法。
【請求項１０】
前記再生ステップにおいて、前記ワークの形状計測手段は、ヘリカルスキャンの軌道に沿って前記ワーク形状を計測することを特徴とする請求項１記載のロボット制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、部品の組立等に用いられるロボットシステム及び制御方法に関する。
【背景技術】
【０００２】
車や家電の製造工場で行なわれる部品の組立工程には、ワーク同士の嵌め合い作業が含まれている。このような嵌め合い作業を含む組立工程を、ロボットを用いて自動化、省力化する場合には、何らかの方法でロボットに嵌め合い作業を教示する必要がある。この教示は、オンライン教示とオフライン教示に分けることができる。
オンライン教示の方法として、例えば、操作者がロボットを誘導して位置姿勢を作り、実際の作業の動作や手順を教示する方法がある。また、オフライン教示の方法として、あらかじめパソコンで作業プログラムを作成して、生成した作業プログラムをロボットコントローラに転送して再生運転する方法がある。
しかし、オンライン教示では、ワーク位置決め誤差により、実際に教示した位置と再生運転時に作業する位置がずれる可能性がある。オフライン教示では、教示データの原点と実際のロボットの原点がずれている場合があり、教示点位置の補正方法が難しい。
【０００３】
そこで、このような問題を解決するために、以下のような従来技術があった。
特許文献１には、オンライン教示で精度良く教示位置を教示する方法として、ロボットエンドエフェクタ上にワークの位置検出をする位置検出手段を設け、位置検出手段が位置検出領域に測定光を照射し、位置検出領域から帰還する乱反射光や正反射光に基づいてワークの位置情報を取得する技術が開示されている。
また、特許文献２には、作業動作を補正する方法として、測定用動作ファイルでロボットを動かして、距離センサにより基準ワークをセンシングして測定距離を記憶しておき、実ワークをセンシングして測定距離を求めて、基準ワークの測定距離との差から作業用動作を補正する技術が開示されている。
【０００４】
一方で近年、製造業において双腕ロボットを人による作業の置き換えに活用しようとする動きが活発になってきている。
特許文献３には、双腕ロボットの一方の腕が一つの部品を組み付け中に他方の腕で他の部品の組み付け準備を行ったり、双方のアームにて２つの部品を組み合わせたりする、双腕を交互にまたは同時に協調させて動作する技術が開示されている。
また、特許文献４には、２つのロボットアームを協調動作させて、ひとつの対象物を２つのアームで保持して移動させる技術が開示されている。
【０００５】
【特許文献１】特開平５−３０９５９０号公報（第１０頁、図１）
【特許文献２】特開平６−２１０５８０号公報（第４頁、図１、第５頁、図４）
【特許文献３】特開２００６−３５３４６号公報（第９頁、図６）
【特許文献４】特開平２００３−１５９６８３号公報（第８頁、図１）
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１および２については、ワークを十字方向又は一方向のみに走査する方法では、ワークの３次元の姿勢ずれを測定不可能である。また、ワーク毎に操作者がワークをどのように走査するかを教示することは面倒で時間がかかる問題もある。また、効率的な３次元の位置姿勢の計測動作自体が明確になっておらず、ワーク毎に操作者の試行錯誤が必要である。
【０００７】
また、特許文献３については、双腕で一つのワークを扱う場合については十分に説明されておらず、一つのワークの一方の端と他端の間が柔軟物である場合のワークを双腕で把持する方法が明らかでない。
また、特許文献４については、双方の腕がワークを把持する箇所が正確でなければうまく嵌合できないような場合には、双方の腕が正確にワークの正確な位置を把持しなければならないが、スレーブアームは重力方向のみ支持しているので、正確な把持位置を保つことができない。
本発明はこのような問題点に鑑みてなされたものであり、操作者がワーク毎に計測動作の位置姿勢教示をする必要がなく、３次元位置姿勢を高精度に取得して、ワークの位置や姿勢のずれを修正することを目的とし、さらに、双腕ロボットでの組み立て作業に適用できる方法とすることを目的とする。
【課題を解決するための手段】
【０００８】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、ワークを把持するエンドエフェクタと、前記ワークの形状計測センサと、を備えたロボットの制御装置において、前記エンドエフェクタを把持位置へ移動させて教示位置として記録し、前記教示位置を中心としてあらかじめ決められた範囲を計測範囲とし、前記計測範囲をワークの形状計測手段により計測したワーク形状をマスタデータとして前記教示位置と関連づけて記録する教示ステップと、前記教示位置を中心としてあらかじめ決められた範囲を計測範囲とし、前記計測範囲をワークの形状計測手段により計測したワーク形状を計測データとし、前記マスタデータと前記計測データを比較し、前記マスタデータに対する前記計測データの３次元の位置姿勢を計算して修正量とし、前記修正量にしたがって前記把持位置を修正し、前記エンドエフェクタを前記修正された把持位置へ移動する指令を生成する再生ステップと、を含むことを特徴とするものである。
請求項２に記載の発明は、前記再生ステップは、ロボットの動作プログラムの１命令で実行されることを特徴とするものである。
請求項３に記載の発明は、前記計測範囲は、スキャン角度と、スキャン距離およびスキャン速度によって決定されることを特徴とするものである。
請求項４に記載の発明は、前記マスタデータとして、計測したワーク形状から抽出された特徴データをメモリに記録することを特徴とするものである。
請求項５に記載の発明は、ワークを把持するエンドエフェクタと、前記ワークの形状計測手段と、を備えたロボットの制御装置において、前記ワーク把持後に移動する目標位置へ手動操作でエンドエフェクタを移動させて第１教示位置として記録し、前記第１教示位置を中心としてあらかじめ決められた範囲を計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状を第１マスタデータとして前記第１教示位置と関連づけて記録し、エンドエフェクタを把持位置へ移動させて第２教示位置として記録し、前記第２教示位置を中心としてあらかじめ決められた広さを計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状を第２マスタデータとして前記第２教示位置と関連づけて記録する教示ステップと、前記第１教示位置を中心としてあらかじめ決められた範囲を計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状を第１計測データとし、前記第１マスタデータと前記第１計測データを比較し、第１マスタデータに対する第１計測データの３次元の位置姿勢を計算して第１修正量とし、前記第２教示位置を中心としてあらかじめ決められた範囲を計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状を第２計測データとし、前記第２マスタデータと前記第２計測データを比較し、前記第２マスタデータに対する前記第２計測データの３次元の位置姿勢を計算して第２修正量とし、前記第２修正量にしたがって把持位置を修正し、エンドエフェクタを把持位置へ移動してワークを把持する指令を生成し、前記第１修正量にしたがって把持後に移動する目標位置を修正し、ワークを把持したエンドエフェクタを修正した目標位置へ移動する指令を生成する再生ステップと、を含むことを特徴とするものである。
請求項６に記載の発明は、ワークを把持するエンドエフェクタと、前記ワークの形状計測手段と、を備えた双腕ロボットの制御装置において、一方の腕のエンドエフェクタで把持した前記ワークに対して、前記ワークの形状計測手段を備えた他方の腕のエンドエフェクタをワーク計測位置へ移動させて教示位置として記録し、前記教示位置を中心としてあらかじめ決められた範囲を計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状をマスタデータとして前記教示位置と関連づけて記録する教示ステップと、前記教示位置を中心としてあらかじめ決められた範囲を計測範囲として決定し、前記計測範囲をワークの形状計測手段により計測したワーク形状を計測データとし、前記マスタデータと前記計測データを比較し、前記マスタデータに対する前記計測データの３次元の位置姿勢を計算して修正量とし、前記修正量にしたがってワークをマスタデータの位置へ移動するために、ワークを把持している腕の位置を修正して移動する指令を生成する再生ステップと、を含むことを特徴とするものである。
請求項７に記載の発明は、前記再生ステップは、前記教示位置を中心としてあらかじめ決められた範囲を第１計測範囲として決定し、前記第１計測範囲をワークの形状計測手段により第１回目の計測を行い、計測したワーク形状を第１回目の計測データとし、前記マスタデータと前記第１回目の計測データを比較し、前記第１回目の計測データをマスタデータに一致させるにはデータが足りない場合には、前記第１計測範囲からワークに対して姿勢を変更した第２計測範囲を生成し、前記第２計測範囲をワークの形状計測手段により第２回目の計測を実行し、第１回目の計測データと第２回目の計測データを重ね合わせて計測データとし、前記マスタデータに対する前記計測データの３次元の位置姿勢を計算して修正量とし、前記修正量にしたがって把持位置を修正し、前記エンドエフェクタを前記修正した把持位置へ移動する指令を生成するステップであることを特徴とするものである。
請求項８に記載の発明は、アームにワークを把持するエンドエフェクタを備えた双腕ロボットであって、前記アームに前記ワークの形状を計測するためのセンサを備えた双腕ロボットと、前記双腕ロボットを制御する制御装置であって、ロボットの動作が記述された動作プログラムが保存されるメモリと、前記動作プログラムを解析する解析部と、現在の位置を教示位置として動作プログラムに書き込む教示データ記憶部と、前記センサから前記ワーク形状の計測データを得る計測部と、教示時に得た前記計測データをマスタデータとしてメモリに記憶するマスタデータ記憶部と、前記マスタデータと計測データとを比較して、その比較結果である変化量を修正量として出力する比較部と、所定のスキャン条件および前記修正量に基づいて前記アームの軌道を生成する指令生成部と、前記生成された軌道に従ってモータを駆動するサーボ制御部と、を備えた制御装置と、前記制御装置に接続される教示装置であって、前記双腕ロボットの動作指令を教示する教示装置と、を備えたことを特徴とするものである。
請求項９に記載の発明は、前記マスタデータは、ヘリカルスキャンの軌道に沿って前記ワークの形状計測手段により計測したワーク形状の計測データであることを特徴とするものである。
請求項１０に記載の発明は、前記再生ステップにおいて、前記ワークの形状計測手段は、ヘリカルスキャンの軌道に沿って前記ワーク形状を計測することを特徴とするものである。
【発明の効果】
【０００９】
請求項１に記載の発明によると、操作者にとっては、ワークを把持するグリッパなどのエンドエフェクタの教示位置を中心として計測範囲を決めているように見えるため、計測のためのセンサ座標系や計測用レーザが当たる位置を気にせず、さらにワーク寸法の入力も必要としないので、計測動作を含む把持動作を簡単に教示することができる。双腕ロボットで一つのワークを把持する場合には、ワークの片方の端を左アームで正確に把持した後、ワークの別の端を右アームで計測して把持することで、両端とも正確な位置を把持して作業することができる。
また、請求項２に記載の発明によると、動作再生時には、計測動作が把持動作と別の動作ではなく、把持という作業に必要な動作として自動実行されるようになるので動作プログラムを見て作業がわかりやすくなる。
また、請求項３に記載の発明によると、複数点の計測位置を教示する必要がなく、ワーク寸法を入力することなく、教示位置を中心として計測範囲を決定することができ、自動的に計測動作を生成することができるので、教示時間が短くなる。
また、請求項４に記載の発明によると、マスタデータとして計測データのすべてを保存するよりもデータ量を小さくして保存することができる。
また、請求項５に記載の発明によると、把持位置だけでなく、把持した後の勘合位置などの目標位置を修正することができるので、把持から勘合までの作業動作のずれを補正することができる。
また、請求項６に記載の発明によると、双腕ロボットにおいて、ワークを保持している側のアームの動作を修正することができる。
また、請求項７に記載の発明によると、必要なデータを能動的に計測して取得することができるので、教示位置を増やさずに大きなずれを許容して再生動作を実行することができる。
請求項８に記載の発明によると、双腕ロボットにおいて、教示位置からずれた位置にあるワークを把持するための双腕ロボットの制御システムを構成することができる。
請求項９に記載の発明によると、多方面から見た計測データをマスタデータとすることができ、再生動作時に、教示位置からのワークの姿勢のずれが大きい場合にも、計測データと合致する部分を含んでいるので、修正量が算出できる。
請求項１０に記載の発明によると、再生動作時に多方面から見た計測データを得ることができ、姿勢を変えて再計測する必要がないので、計測時間を短くすることができる。
【発明を実施するための最良の形態】
【００１０】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【００１１】
図１は、本発明に係るロボットシステムの構成図である。このロボットシステムは、双腕ロボット１０１と、その動作を制御する制御装置１０２を備えている。
まず、双腕ロボット１０１について説明する。
図において、ロボットの左アーム１０４と右アーム１０５は、関節も含めて模式的に表されている。各関節は、図示しないモータによって、駆動される。
左グリッパ（エンドエフェクタ）１０６は、左アームの先端に配置されている。
右グリッパ（エンドエフェクタ）１０７は、右アームの先端に配置されている。
ワーク１０９は双腕ロボット１０１の把持対象物である。ワーク１０９は、本実施例においてはコネクタ付ケーブルであるが、詳細については後述する。
ワーク形状計測センサ１０８は、２次元レーザ変位センサである。このセンサは、右グリッパ１０７上に搭載されている。このワーク形状計測センサ１０８は、右アーム１０５が動くとともにワーク１０９のデータを取得することで、ワーク１０９の２次元の計測データを連続して取得することができる。この取得したデータを重ねることによって、３次元の計測データ（ワーク形状）が得られる。なお、このセンサは、右グリッパ１０７ではなく、左グリッパ１０６に搭載されていても良い。また、グリッパではなく、アームに搭載されていても良い。また、このセンサは、ワークの３次元形状を計測できるものであれば任意のセンサでよい。例えば、このセンサは、複数のカメラによるビジョンセンサであってもよい。
【００１２】
次に、制御装置１０２について説明する。
動作プログラム解析部１１０は、ロボットの動作が記述された動作プログラム（ＪＯＢ）１１８を解析して必要な処理を呼び出す。
教示データ記憶部１１３は、現在の位置１２２を教示位置１２３として、動作プログラム１１８に書き込む。この教示位置１２３は、操作者が教示作業時に教示装置１０３の所定のキーを入力すると動作プログラム１１８に書き込まれる。教示データ記憶部１１３は、後述するマスタデータ１２１の番号を対象物番号１２５として、動作プログラム１１８に書き込む。ここで、対象物番号１２５は、マスタデータ１２１を複数保存した場合に、マスタデータを特定するために付ける番号である。教示データ記憶部１１３は、教示位置１２３と対象物番号１２５を一組として動作プログラム１１８に書き込む。この動作プログラム１１８は、不揮発性メモリ１１７に保存される。
計測部１１４は、ワーク形状計測センサ１０８によりワークの計測データ１２０を得る。
マスタデータ記憶部１１５は、マスタデータ１２１として計測データ１２０に番号を付け、このマスタデータ１２１を不揮発性メモリ１１７に保存する。
位置姿勢比較部１１６は、マスタデータ１２１と計測データ１２０とを比較して、マスタデータ１２１に対する計測データ１２０の３次元の位置姿勢を計算する。位置姿勢比較部１１６は、この位置姿勢の変化量を修正量１２４として、指令生成部１１１に出力する。
指令生成部１１１は、教示装置１０３からの手動操作にしたがって軌道を生成する。また、指令生成部１１１は、動作プログラム１１８、制御装置のパラメータとして保存されているスキャン条件１１９、および修正量１２４に基づいて把持動作や計測動作を行うアームの軌道を生成する。
サーボ制御部１１２は、生成された軌道にしたがってモータを駆動する。
教示装置１０３は、制御装置１０２に接続され、双腕ロボット１０１の教示・操作をするための装置である。教示装置１０３上には、ロボットに対する指令やデータを入力するためのキーと情報表示用のディスプレイが設けられている。
【００１３】
次に、本発明に係るロボットシステムの動作について説明する。本実施例において、双腕ロボットが扱うワーク１０９は、図４（ａ）に示す複数のリード線の両端にコネクタを備えたケーブルである。図４（ａ）は、テーブル上のジグや供給装置によって位置が固定されたワークのコネクタ４０４を、予め教示されたように把持することで、左グリッパ１０６がコネクタ４０４を把持している状態を示している。コネクタ４０３の位置は固定されていない。コネクタ４０４とコネクタ４０３の間のリード線４０５は柔軟であるため、コネクタ４０３の位置は定まらない。
【００１４】
本発明に係るロボットシステムの動作は、図２に示す教示動作（ステップＳ２０１〜２０６）と、図３に示す再生動作（ステップＳ３０１〜Ｓ３０５）である。以下、各動作について説明する。
【００１５】
I．教示動作
まず、教示動作（ステップＳ２０１〜Ｓ２０６）について、図２に基づいて説明する。
（Ｓ２０１）
操作者が教示装置１０３のキーを操作する。操作者は右グリッパ１０７をワーク１０９の把持位置へ移動するようにキーを操作する。指令生成部１１１は、このキー操作にしたがってロボットが移動する軌道を生成する。
【００１６】
（Ｓ２０２）
操作者が教示装置１０３を操作して、スキャンコマンド（ＭＯＶＳＣＡＮ）を動作プログラムに登録する。このスキャンコマンド（ＭＯＶＳＣＡＮ）は、ワーク形状計測センサ１０８がワークの計測データ１２０を得るスキャン動作をさせるためのコマンドである。この動作プログラムは不揮発性メモリ１１７に保存される。
具体例を図５に基づいて説明する。図５は教示装置の動作プログラム表示画面５０１を示している。動作プログラム表示画面５０１中、教示位置５０３は、ロボットの教示位置を表示している。５０２は、現在編集中の動作プログラムを表示している。
操作者は図示しないメニューの命令一覧の画面からスキャンコマンド（ＭＯＶＳＣＡＮ）を選択する。選択後、操作者が教示装置１０３上のEnterキーを押すと、図に示すようにスキャンコマンド５０４が動作プログラム５０２に登録される。同時に、ロボットの現在位置が教示位置データ５０３として保存される。この教示位置データは、教示位置番号５０５（Ｃ００００１）として保存される。
スキャンコマンド５０４は、（ｉ）教示位置番号５０５（Ｃ００００１）、（ｉｉ）ステップＳ３０４において修正された教示位置が代入される位置変数５０６（Ｐ０１２）、（ｉｉｉ）スキャン動作の移動速度５０７（Ｖ＝１０．０）、および（ｉｖ）対象物番号１２５（ＯＢＪ＃（１））と関連づけられて、動作プログラムに記録される。記録後、デフォルトで設定された（ｉｉｉ）スキャン動作の移動速度５０７と（ｉｖ）対象物番号１２５を変更することもできる。
【００１７】
（Ｓ２０３）
操作者が、教示装置１０３を操作して、移動コマンド（ＭＯＶＬ）を動作プログラムに登録する。この移動コマンド６０１は、グリッパを把持位置へ移動させるためのコマンドである。
具体例を図６に基づいて説明する。図６は、教示装置の動作プログラム表示画面５０１を示している。ステップＳ２０２で実行したスキャンコマンドが既に表示されている。
移動コマンド（ＭＯＶＬ）６０１は、位置変数５０６（Ｐ０１２）および移動速度６０２（Ｖ＝１０．０）と関連付けられて、動作プログラムに登録される。なお、この位置変数５０６は、ステップＳ２０２において、スキャンコマンドで指定された変数である。
【００１８】
（Ｓ２０４）
操作者が教示装置１０３の画面上に表示されるカーソルをスキャンコマンドの行に合わせて、画面（図６）上のスキャン実行ボタン５０８を押す。スキャン実行ボタン５０８が押されると、指令生成部１１１は、動作プログラムの教示位置（Ｃ００００１に保存された教示位置）を基準として、所定のスキャン条件１１９に基づいて、所定の計測範囲を計測するための軌道を生成する。ここで、この所定のスキャン条件１１９は、スキャン角度４０６、スキャン距離４０７およびスキャン速度として、あらかじめ制御装置のパラメータに保存されている。ここで、スキャン角度は、対象物正面方向の軸まわりにワーク形状計測センサ１０８を傾ける角度である（図４（ｃ））。スキャン距離はワーク形状計測センサ１０８が移動する距離である（図４（ｃ））。スキャン速度はワーク形状計測センサ１０８が移動する速度である。
サーボ制御部１１２は、モータを駆動して、生成された軌道にしたがってロボットを動かす。ロボットが動くとともに、計測部１１４はワーク形状計測センサ１０８からの計測データを得る。
【００１９】
（Ｓ２０５）
教示装置のディスプレイに、得られた計測データが表示される。この計測データは、図７に示すように、３次元イメージ７０２で表示される。図において、１２５は対象物番号、７０３はコネクタの型名や通称などを表す対象物名である。この対象物名は、任意の名前でよい。７０４は登録ボタンである。
【００２０】
（Ｓ２０６）
操作者がマスタ登録操作を行う。このマスタ登録操作は、操作者が教示装置を使って対象物番号１２５を入力し、登録ボタン７０４を押す操作である。操作者がこのマスタ登録操作を行うと、計測データはマスタデータ１２１として不揮発性メモリ１１７に保存される。
【００２１】
以上説明したように、教示動作（ステップＳ２０１〜Ｓ２０６）によって、動作プログラム１１８とマスタデータ１２１が作成される。この動作プログラム１１８とマスタデータ１２１は、不揮発性メモリ１１７に保存される。
【００２２】
図４は教示動作時のワーク形状計測センサ１０８の動きを示している。図４（ａ）において、右グリッパ１０７の先にグリッパ座標系４０１が設定されている。ワーク形状計測センサ１０８のセンサ原点にセンサ座標系４０２が設定されている。ここで、把持する対象物は、左グリッパ１０６が把持していないコネクタ４０３である。
図４（ｂ）は、前記ステップＳ２０１およびＳ２０２を実行する際の様子を表している。教示装置１０３からの手動操作でロボットを動かし、右グリッパ１０７をコネクタの把持位置に位置決めして教示位置とする。
図４（ｃ）は、前記ステップＳ２０４を実行する際の様子を表している。制御装置１０２は、自動的にセンサ座標系４０２の原点を教示位置のグリッパ座標系４０１の原点へ合わせる。その後、制御装置１０２は、コネクタ４０３の正面方向の周りにスキャン角度４０６だけセンサ座標系４０２を回転させ、ワーク形状計測センサ１０８をスキャン距離４０７だけ移動させる。このとき、スキャン距離４０７の２分の１だけ移動方向の負方向へ移動した位置から計測を開始することで、図４（ｃ）のように教示位置が計測範囲の中心となるようにスキャンする。スキャンコマンド実行前の位置から計測開始位置へは、スキャンコマンドに指定した移動速度で移動する。計測中の速度は、ステップＳ２０２において、スキャンコマンドに指定した移動速度ではなく、スキャン条件として指定されたスキャン速度で移動する。
【００２３】
II．再生動作
次に、再生動作（ステップＳ３０１〜Ｓ３０５）について、図３に基づいて説明する。
この再生動作は、教示動作により作成された動作プログラムに書かれたスキャンコマンドを実行する動作（ステップＳ３０１〜Ｓ３０４）と移動コマンドを実行する動作（ステップ３０５）である。
【００２４】
（Ｓ３０１）
動作プログラム解析部１１０が、不揮発性メモリ１１７から動作プログラム１１８とスキャン条件１１９を呼び出す。動作プログラム解析部１１０は、動作プログラム１１８中に書かれたスキャンコマンドの実行を指令生成部１１１へ指令する。動作プログラム解析部１１０から指令されると、指令生成部１１１は、スキャンコマンドとともに登録された教示位置（Ｃ００００１に保存された教示位置）を基準としてスキャン条件１１９に従った所定の計測範囲を計測する軌道を生成する。サーボ制御部１１２は、生成された軌道にしたがって、モータを駆動してアームを動かす。ロボットが動作するとともに、計測部１１４は、ワーク形状計測センサ１０８からコネクタ４０３の３次元の位置姿勢の計測データを得る。
【００２５】
（Ｓ３０２）
位置姿勢比較部１１６は、ステップＳ２０２において登録された対象物番号１２５に対応するマスタデータ１２１を不揮発性メモリ１１７から読み出す。次に、位置姿勢比較部１１６は、ステップＳ３０１で得たコネクタ４０３の３次元の位置姿勢が、マスタデータ１２１に対してどの程度変化したかを計算する。位置姿勢比較部１１６は、この変化した位置姿勢の量を修正量１２４とする。
【００２６】
ここで、この修正量１２４の計算について、以下に説明する。図８において、８０１はマスタデータ１２１の３次元イメージ、８０２は計測データ１２０の３次元イメージである。
まず、特徴点の抽出を行う。図８の３次元イメージからエッジ点を抽出すると、図９（ａ）に示す点群が得られる。本図において、９０１はマスタデータから抽出したエッジ点であり、９０２は計測データから抽出したエッジ点を示している。このエッジ点を抽出する方法は、隣の点との差分がある閾値以上になった点をエッジ点とする公知の方法などを使用する。
エッジ点のデータから直線を抽出すると、図９（ｂ）のように、エッジ点をつなげた直線が得られる。本図において、９０３はマスタデータの直線であり、９０４は計測データの直線である。この直線を抽出する方法は、Ｈｏｕｇｈ変換による公知の方法などを使用する。
求める特徴点は、抽出した直線の交点である。この交点には、符号を付ける。このとき、スキャン方向において最初に位置する特徴点を１番とする。各特徴点は位置成分Ｘ、Ｙ、Ｚを持つ。図９（ｃ）において、９０５の４点はマスタデータの特徴点、９０６の４点は計測データの特徴点である。マスタデータについてはｐ１からｐ４、計測データについてはｑ１からｑ４という符号を特徴点に付けている。
【００２７】
修正量１２４はマスタデータ１２１に対する計測データの位置姿勢の変化量として求められる。
まず、図１０（ａ１）に示すマスタデータと図１０（ａ２）に示す計測データを同じセンサ座標系４０２から見たものとして重ねると、図１０（ｂ）に示すようになる。ただし、センサ座標系は計測時にはスキャン方向に平行移動するため、たとえば、スキャン開始点からスキャン距離の２分の１だけ進んだ位置をセンサ座標系の原点として計測データを重ねる。
次に、特徴点ｐ１とｑ１が一致するように、マスタデータ全体を移動する。ｐ１から見たｑ１の分だけセンサ座標系を並行移動すると図１０（ｃ）のようになる。図１０（ｃ）において、ｐ１、ｐ２、ｐ３、ｐ４を移動したものは、ｐ１ｂ、ｐ２ｂ、ｐ３ｂ、ｐ４ｂで表している。ｐ１ｂとｑ１の距離は０である。ｐ１ｂの位置にセンサ座標系Ｓ１と同じ姿勢の座標系を設定し、座標系Ｓ２とする。すなわち、Ｓ１から見たｑ１の分だけＳ１を平行移動した座標系がＳ２である。
次に、Ｓ２をΔＴrだけ回転させると同時に、ｐ２ｂからｐ４ｂもＳ２を基準に回転させ、それぞれｐ２ｃ、ｐ３ｃ、ｐ４ｃとする。つまり、ｐ１ｂ、ｐ２ｂ、ｐ３ｂ、ｐ４ｂを頂点とする四角形全体をＳ２とともに回転させる。ΔＴrは回転を表す４×４の同次変換行列である。回転後、符号の中の番号が同じ点同士の距離を求めて加算した式１のＪを評価関数として計算する。
【００２８】
【数１】
2010172969.tif 000003
【００２９】
ここで、ｉは１からｎまでの点の番号を表す添字、Ｓ２ｑｉは座標系Ｓ２から見た点ｑｉの座標、Ｓ２ｐｉｃは座標系Ｓ２から見た点ｐｉｃの座標である。
ΔＴｒは、次の（１）から（５）のようにして求める。
（１）ΔＴｒを、Ｒｏｌｌ、Ｐｉｔｃｈ、Ｙａｗから計算される回転行列とする。Ｐｉｔｃｈ角、Ｙａｗ角は固定で、Ｒｏｌｌ角を−４５度から４５度まで１度ずつ変化させて、評価関数Ｊが最小になるＲｏｌｌ角を求める。
（２）ΔＴｒを、Ｒｏｌｌ角を（１）で求めたＲｏｌｌ角にし、さらにＰｉｔｃｈ角を−４５度から４５度まで１度ずつ変化させて、評価関数Ｊが最小になるＰｉｔｃｈ角を求める。
（３）ΔＴｒを、Ｒｏｌｌ角を（１）で求めたＲｏｌｌ角にし、Ｐｉｔｃｈ角を（２）で求めたＰｉｔｃｈ角にし、さらにＹａｗ角を−４５度から４５度まで１度ずつ変化させて、評価関数Ｊが最小になるＹａｗ角を求める。
（４）ΔＴｒを、Ｐｉｔｃｈ角を（２）で求めたＰｉｔｃｈ角にし、Ｙａｗ角を（３）で求めたＹａｗ角にする。
（５）前記（１）から（４）を数回繰り返して、評価関数Ｊを最小とする変換行列ΔＴｒを求める。求めたΔＴｒを使って、修正量は次の式２の行列Ｓ１ＴＳＦで表される。
【００３０】
【数２】
2010172969.tif 000004
【００３１】
ここで、Ｔｒａｎｓ（ ）は平行移動を表す変換行列である。グリッパ座標系で表した修正量Ｇ１ＴＧＦは式３のようになる。
【００３２】
【数３】
2010172969.tif 000005
【００３３】
ここで、ＧＲＳはグリッパ座標系から見たセンサ座標系の姿勢を表す３×３の回転行列を表す。ＳＲＧはセンサ座標系から見たグリッパ座標系の姿勢を表す３×３の回転行列を表す。
【００３４】
（Ｓ３０３）
指令生成部１１１が、教示位置を修正量１２４にしたがって修正する。教示位置を同次変換行列の形にして、式３の行列を右からかけることによって変換したものが、修正した教示位置となる。すなわち、ロボット座標系から見た教示位置をRＴtとすると、修正した教示位置RＴt2は式４のようになる。
【数４】
2010172969.tif 000006
【００３５】
（Ｓ３０４）
修正された教示位置を、スキャンコマンドの引数として指定された位置変数５０６（Ｐ０１２）へ代入する。ただし、位置変数５０６には、修正した教示位置RＴt2の行列から計算したＸ、Ｙ、Ｚ、Ｒｏｌｌ、Ｐｉｔｃｈ、Ｙａｗの値を代入する。
【００３６】
（Ｓ３０５）
動作プログラム解析部１１０が動作プログラム１１８中に書かれた移動コマンドの実行を指令生成部１１１へ指令する。動作プログラム解析部１１０から指令されると、指令生成部１１１は、動作プログラム１１８の移動コマンドに指定された位置変数の位置への軌道を生成する。サーボ制御部１１２は、モータを駆動して、この生成された軌道に沿うようにアームを移動させる。
【００３７】
図１１は再生動作時のワーク形状計測センサ１０８の動きを示している。図１１（ａ）において、スキャンコマンドにしたがってスキャンし、計測データを得る。図１１（ｂ）はスキャンコマンドの結果として位置変数へ代入された、修正した教示位置へ移動コマンドで移動した様子を示している。
【００３８】
このように、操作者は、ステップＳ２０２において教示位置を１点だけ教示すればよいので、複数の教示点を使ってロボットの計測動作自体を教示する必要がない。その結果、操作者はグリッパを見ながらの手動操作による位置決めと、３次元データ表示による、教示操作ができる。すなわち、ワークの位置姿勢の計測を含む教示操作が簡単になる。計測動作の際、ワーク形状計測センサ１０８は教示位置を中心とした所定の計測範囲を動くため、操作者はセンサ座標系の位置を気にせず、対象物の寸法入力も必要とせずに教示することができる。
なお、スキャンコマンドと移動コマンドはそれぞれ独立したコマンドであるが、これらをまとめた把持移動コマンド（ＭＯＶＣＡＴＣＨ）としても良い。
【実施例２】
【００３９】
図１３は、第２実施例に係るロボットシステムの構成図である。本発明に係るロボットシステムは、双腕ロボット１０１と、その動作を制御する制御装置１０２を備えている。
双腕ロボット１０１については、図１に示したものと同一であるので、その説明は省略する。
以下、制御装置１０２について説明する。
スキャン条件入力部１３０１は、教示作業時に教示装置１０３から入力されたスキャン角度（図４（ｃ））、スキャン距離（図４（ｃ））、スキャン速度などのデータをスキャン条件１１９として不揮発性メモリ１１７に保存する。
その他の部分は実施例１と同じであるので、説明は省略する。
【００４０】
本実施例に係るロボットシステムの動作は、図１４に示すような教示動作（ステップＳ１４０１〜Ｓ１４０６）と、図１５に示すような再生動作（ステップＳ１５０１〜Ｓ１５０４）である。以下、各動作について説明する。
【００４１】
I．教示動作
まず、教示動作（ステップＳ１４０１〜Ｓ１４０６）について、図１４に基づいて説明する。
（Ｓ１４０１）
操作者が教示装置１０３のキーを操作する。操作者は右グリッパ１０７をワーク１０９の把持位置へ移動するようにキーを操作する。指令生成部１１１は、このキー操作にしたがってロボットが移動する軌道を生成する。
【００４２】
（Ｓ１４０２）
操作者が教示装置１０３を操作して、把持移動コマンド（ＭＯＶＣＡＴＣＨ）を動作プログラムに登録する。この把持移動コマンド（ＭＯＶＣＡＴＣＨ）は、ワーク形状計測センサ１０８がワークの計測データ１２０を得るスキャンコマンドと移動コマンドをまとめ多コマンドである。この動作プログラムは不揮発性メモリに保存される。
具体例を図１２に基づいて説明する。図１２は教示装置の動作プログラム表示画面５０１の例を示している。
操作者は図示しないメニューの命令一覧の画面から把持移動コマンド（ＭＯＶＣＡＴＣＨ）を選択する。選択後、操作者が教示装置１０３上のEnterキーを押すと、図に示すように把持移動コマンド１２０１が動作プログラム５０２に登録される。同時に、ロボットの現在位置が教示位置データ５０３として保存される。この教示位置データは、教示位置番号５０５（Ｃ００００１）として保存される。
把持移動コマンド１２０１は（ｉ）教示位置番号５０５（Ｃ００００１）、（ｉｉ）スキャン動作の移動速度５０７（Ｖ＝１０．０）、（ｉｉｉ）対象物番号５０７（ＯＢＪ＃（１））と関連づけられて動作プログラムに記録される。記録後、デフォルトで設定された移動速度５０７と対象物番号５０７を変更することもできる。
【００４３】
（Ｓ１４０３）
教示装置から入力されたスキャン角度、スキャン距離、およびスキャン速度をスキャン条件として不揮発性メモリに保存する。図１６は教示装置のスキャン条件入力画面１６０１を示している。操作者はスキャン角度１６０２、スキャン距離１６０３、およびスキャン速度１６０４の値を入力し、設定ボタン（ＳＥＴ）１６０５を押すことで、スキャン条件を設定できる。
【００４４】
（Ｓ１４０４）
操作者が教示装置のスキャン実行ボタン５０８を押すと、指令生成部１１１は動作プログラムの教示位置（Ｃ００００１に保存された教示位置）を基準として設定されたスキャン条件に従った計測範囲を計測する軌道を生成する。サーボ制御部１１２は、モータを駆動して、ロボットを動かす。ロボットが動くとともに、計測部１１４はワーク形状計測センサ１０８からの計測データを得る。
【００４５】
（Ｓ１４０５）
教示装置１０３のディスプレイに、得られた計測データが表示される。この計測データは、図７に示すように、３次元イメージ７０２で表示される。図において、１２５は対象物番号、７０３は対象物名、７０４は登録ボタンである。
【００４６】
（Ｓ１４０６）
次に、操作者がマスタ登録操作を行う。操作者がこのマスタ登録操作を行うと、前記計測データはマスタデータ１２１として不揮発性メモリ１１７に保存される。
【００４７】
以上説明したように、教示動作（Ｓ１４０１〜ステップＳ１４０６）によって、スキャン条件、動作プログラム、およびマスタデータが作成される。このスキャン条件、動作プログラム、およびマスタデータは、不揮発性メモリ１１７に保存されることになる。
【００４８】
II．再生動作
次に、再生動作（ステップＳ１５０１〜Ｓ１５０４）について、図１５に基づいて説明する。この再生動作は、動作プログラムに書かれた把持移動コマンドを実行する動作である。
【００４９】
（Ｓ１５０１）
動作プログラム解析部１１０が、不揮発性メモリ１１７から動作プログラム１１８とスキャン条件１１９を呼び出す。動作プログラム解析部１１０は、動作プログラム１１８中に書かれた把持移動コマンドの実行を指令生成部１１１へ指令する。動作プログラム解析部１１０から指令されると、指令生成部１１１は動作プログラム１１８の把持移動コマンドとともに登録された教示位置（Ｃ００００１に保存された教示位置）を基準としてスキャン条件１１９に従った所定の計測範囲を計測する軌道を生成する。サーボ制御部１１２は、生成された軌道にしたがって、モータを駆動してアームを動かす。ロボットが動作するとともに、計測部１１４はワーク形状計測センサ１０８からコネクタ４０３の３次元の位置姿勢の計測データを得る。
【００５０】
（Ｓ１５０２）
位置姿勢比較部１１６は、動作プログラム１１８中の把持移動コマンドとともに登録された対象物番号１２５に対応するマスタデータ１２１を不揮発性メモリ１１７から読み出す。次に、位置姿勢比較部１１６は、ステップＳ１５０１で得たコネクタ４０３の３次元の位置姿勢が、マスタデータ１２１に対してどの程度変化したかを計算する。位置姿勢比較部１１６は、この変化した位置姿勢の量を修正量１２４とする。
図８にマスタデータ１２１と計測データの３次元イメージの例を示す。
【００５１】
（Ｓ１５０３）
指令生成部１１１が、教示位置を修正量１２４にしたがって修正する。修正量１２４を求める方法は、実施例１と同様であるので省略する。
【００５２】
（Ｓ１５０４）
指令生成部１１１が、教示位置を修正量１２４にしたがって修正し、修正した位置への軌道を生成する。サーボ制御部１１２は、モータを駆動して、この生成された軌道に沿うように、アームを移動させる。なお、把持移動コマンド（ＭＯＶＣＡＴＣＨ）では、スキャンコマンド実行前の位置から計測開始位置への移動と、計測終了位置から把持位置への移動については、スキャンコマンドに指定した移動速度で移動する。計測中の速度は、スキャン条件として指定されたスキャン速度で移動する
【００５３】
図１１は動作再生の動作を示している。図１１（ａ）において、把持移動コマンドにしたがってスキャンし、計測データを得る。図１１（ｂ）は計測結果とマスタデータから修正量を算出し、修正した教示位置を計算し、教示位置から計算したアプローチ位置へ到達した様子を示し、図１１（ｃ）は修正した教示位置へグリッパを移動した様子を示している。
【００５４】
このように、ワーク寸法を入力することなく、スキャン条件によって、教示位置を中心として計測範囲を決定することができる。従って、自動的に計測動作を生成することができるので、教示時間が短くなる。
【実施例３】
【００５５】
図２のフローチャートにおけるマスタデータ保存時（Ｓ２０６）に、図９に示した特徴点の抽出を実行し、この抽出した特徴点データのみを保存する。また、図３のマスタデータ読み出し時（Ｓ３０２）には保存した特徴点データを読み出し、計測データから求めた特徴点と比較して図１０のように修正量の計算を実行する。
このようにすると、マスタデータとして計測データのすべてを保存するよりもデータ量を小さくして保存することができる。従って、マスタデータのための不揮発性メモリの使用量を減らすことができる。
【実施例４】
【００５６】
図１７は、第４実施例の動作を表す図である。左グリッパ１０６がケース１７０３を把持している。このケース１７０３からはアース線１７０２が出ている。本実施例は、このアース線１７０２の下端１７０１を右グリッパ１０７で把持し、アース線挿入タブ１７０４へ挿入する作業を行うものである。
本実施例においては、把持位置であるアース線１７０２の位置が不確定である。そこで、アース線１７０２の下端１７０１をワーク形状計測センサ１０８で計測して把持する。また、アース線１７０２の下端１７０１を把持した後にアース線下端を挿入するタブ１７０４の位置も不確定である。そこで、タブ１７０４をセンサで計測して、挿入の教示位置を修正して挿入動作を行う。
【００５７】
本実施例に係るロボットシステムの動作は、図１８に示すような教示動作（ステップＳ１８０１〜Ｓ１８１１）と、図１９に示すような再生動作（ステップＳ１９０１〜Ｓ１９１０）である。
【００５８】
I．教示動作
まず、教示動作（ステップＳ１８０１〜Ｓ１８１１）について、図１８に基づいて説明する。
（Ｓ１８０１）
タブへの挿入位置を計測する動作を教示するため、操作者が教示装置１０３のキーを操作する。操作者は右グリッパ１０７をタブ位置へ移動するようにキーを操作する。指令生成部１１１は、このキー操作にしたがってロボットが移動する軌道を生成する。
【００５９】
（Ｓ１８０２）
操作者が教示装置１０３を操作して、スキャンコマンド（ＭＯＶＳＣＡＮ２）を第１教示位置と対象物番号１２５とともに動作プログラムに登録する。このスキャンコマンド（ＭＯＶＳＣＡＮ２）は、ワーク形状計測センサ１０８がワークの計測データ１２０を得るスキャン動作をさせるためのコマンドさせるためのコマンドであり、ＭＯＶＳＣＡＮとは、引数に指定する位置変数に入る値の点で相違する。この動作プログラムは不揮発性メモリ１１７に保存される。ここで、スキャンコマンド実行後に、スキャンコマンドの引数に指定した位置変数に入る値は、修正した教示位置ではなく、修正量であり、マスタデータをロボット座標系１２６まわりにどれだけ動かすと計測データに一致するかを表す変換量である。
具体例を図２０に基づいて説明する。図２０は教示装置の動作プログラム表示画面２００１を示している。操作者は図示しないメニューの命令一覧の画面からスキャンコマンドを選択する。選択後、操作者が教示装置１０３上のEnterキーを押すと、動作プログラム２００２にＬ００１に示すように登録される。同時に、ロボットの現在位置が第１教示位置データとして保存される。この第１教示位置データは、教示位置番号（Ｃ００００１）として保存される。
Ｌ００１に示すスキャンコマンド（ＭＯＶＳＣＡＮ２）は（ｉ）教示位置番号（Ｃ００００１）、（ｉｉ）スキャン結果である修正量の成分Ｘ、Ｙ、Ｚ、Ｒｏｌｌ、Ｐｉｔｃｈ、Ｙａｗが代入される位置変数（Ｐ０１８）、（ｉｉｉ）スキャン動作の移動速度（Ｖ＝５．０）、（ｉｖ）対象物番号（ＯＢＪ＃（３））と関連づけられて動作プログラムに記録される。
【００６０】
（Ｓ１８０３）
操作者が教示装置１０３の画面上に表示されるカーソルをスキャンコマンドの行に合わせて、画面（図２０）上のスキャン実行ボタン（ＳＣＡＮ）５０８を押す。スキャン実行ボタン５０８が押されると、指令生成部１１１は、動作プログラムの第１教示位置（Ｃ００００１に保存された教示位置）を基準として、所定のスキャン条件に基づいて、所定の計測範囲を計測する軌道を生成する。この所定のスキャン条件は、スキャン角度、スキャン距離、およびスキャン速度として、あらかじめ制御装置のパラメータに保存されている。  
サーボ制御部１１２は、モータを駆動し、生成された軌道にしたがってロボットの右アームを動かす。ロボットが動くとともに、計測部１１４は、ワーク形状計測センサ１０８からコネクタ４０３の３次元の位置姿勢の計測データを得る。
【００６１】
（Ｓ１８０４）
教示装置１０３のディスプレイに得られた計測データが３次元イメージで表示される。
【００６２】
（Ｓ１８０５）
操作者がマスタ登録操作を行う。このマスタ登録操作は、操作者が教示装置１０３を使って対象物番号１２５を入力し、登録ボタンを押す操作である。操作者がこのマスタ登録操作を行うと、前記計測データは第１マスタデータとして不揮発性メモリ１１７に保存される。
【００６３】
（Ｓ１８０６）
アース線下端を把持する動作を教示するため、操作者が教示装置１０３のキーを操作する。操作者は右グリッパ１０７を把持位置へ移動するようにキーを操作する。指令生成部１１１は、このキー入力にしたがってロボットが移動する軌道を生成する。
【００６４】
（Ｓ１８０７）
操作者が、教示装置１０３を操作して、スキャンコマンド（ＭＯＶＳＣＡＮ２）を第２教示位置と対象物番号１２５とともに動作プログラムに登録する。この動作プログラム１２１は不揮発性メモリ１１７に保存される。
図２０において、操作者は図示しないメニューの命令一覧の画面からスキャンコマンドを選択する。選択後、操作者がEnterキーを押すと動作プログラム２００２にＬ００２のように登録される。同時に、ロボットの現在位置が第２教示位置データ２００３として保存される。この第２教示位置データは、教示位置番号（Ｃ００００２）として保存される。
Ｌ００２に示すスキャンコマンド（ＭＯＶＳＣＡＮ２）は（ｉ）教示位置番号（Ｃ００００２）、（ｉｉ）スキャン結果である修正量の成分Ｘ、Ｙ、Ｚ、Ｒｏｌｌ、Ｐｉｔｃｈ、Ｙａｗが代入される位置変数（Ｐ０１０）、（ｉｉｉ）スキャン動作の移動速度（Ｖ＝５．０）、（ｉｖ）対象物番号（ＯＢＪ＃（２））と関連づけられて動作プログラムに記録される。
【００６５】
（Ｓ１８０８）
操作者が教示装置１０３の画面上のカーソルをスキャンコマンドの行に合わせて、教示装置の画面（図２０）上のスキャン実行ボタン５０８を押す。スキャン実行ボタン５０８が押されると、指令生成部１１１は動作プログラムの第２教示位置（Ｃ００００２に保存された教示位置）を基準として、所定のスキャン条件に基づいて、所定の計測範囲を計測する軌道を生成する。この所定のスキャン条件は、スキャン角度、スキャン距離、およびスキャン速度として、あらかじめ制御装置のパラメータに保存されている。
サーボ制御部１１２は、モータを駆動し、生成された軌道にしたがってロボットの右アームを動かす。ロボットが動くとともに、計測部１１４はワーク形状計測センサ１０８からの計測データを得る。
【００６６】
（Ｓ１８０９）
教示装置１０３のディスプレイに前記計測データが３次元イメージで表示される。
【００６７】
（Ｓ１８１０）
操作者がマスタ登録操作を行う。操作者がこのマスタ登録操作を行うと、前記計測データは第２マスタデータとして不揮発性メモリ１１７に保存される。
【００６８】
（Ｓ１８１１）
操作者が、教示装置１０３のキーを操作して、スキャンコマンド（ＭＯＶＳＣＡＮ２）に指定した前記位置変数（Ｐ０１８、Ｐ０１０）に再生ステップで入る修正量を使って、教示位置修正の演算をするコマンドと移動コマンドを動作プログラムに登録する。
図２０において、Ｌ００３はアース線下端を右グリッパで把持するための移動プログラムが記述されたサブルーチンの呼び出しコマンド（ＣＡＬＬ）である。
Ｌ００４は、アース線をタブの上まで移動させる移動コマンド（ＭＯＶＬ）であり、教示位置（Ｃ００００３からＣ００００６）と移動速度（Ｖ＝１０．０）と関連づけられて保存される。
Ｌ００５は、ロボット座標系１２６から見た右アームの現在位置を取得するコマンド（ＧＥＴＳ）であり、現在位置を意味するシステム変数（＄ＰＸ００１）と、システム変数の内容のコピーを代入する位置変数（Ｐ０１１）と関連づけられて保存される。
Ｌ００６は、Ｐ０１８を同次変換行列にしたものとＰ０１１を同次変換行列にしたものの積を計算し、計算結果をＰ０１２に入れる演算コマンド（ＭＵＬＭＡＴ）である。Ｌ００６において、Ｐ０１８はＬ００１で得られるタブの修正量、Ｐ０１１は現在位置である。
Ｌ００７はＰ０１２の位置への移動コマンド（ＭＯＶＬ）であり、位置変数（Ｐ０１２）と移動速度（Ｖ＝１０．０）と関連付けられて保存される。Ｌ００７によってタブに挿入するための位置は修正されることになる。
Ｌ００８は現在位置からＰ０１９に設定されている量だけ移動する相対移動コマンド（ＩＭＯＶ）であり、位置変数（Ｐ０１９）と移動速度（Ｖ＝１０．０）と座標系指定（ＲＦ）と関連付けられて保存される。Ｐ０１９には、挿入方向の移動量があらかじめ設定されており、ＲＦはロボット座標系上の相対移動量として動くことを意味する。
【００６９】
以上説明したように、教示操作によって、動作プログラムと第１マスタデータおよび第２マスタデータが作成されて不揮発性メモリ１１７に保存されることになる。
【００７０】
II．再生動作
次に、再生動作（ステップＳ１９０１〜Ｓ１９１０）について、図１９に基づいて説明する。この再生動作は、動作プログラムに書かれたコマンドを実行する動作である。
【００７１】
（Ｓ１９０１）
動作プログラム解析部１１０が不揮発性メモリ１１７から動作プログラムとスキャン条件を呼び出す。動作プログラム解析部１１０は、動作プログラム中のＬ００１に書かれたスキャンコマンド（ＭＯＶＳＣＡＮ２）の実行を指令生成部へ指令する。指令生成部１１１は動作プログラムのスキャンコマンドとともに登録された第１教示位置（Ｃ００００１に保存された教示位置）を基準としてスキャン条件に従った所定の計測範囲を計測する軌道を生成する。サーボ制御部１１２は、生成された軌道にしたがって、モータを駆動してロボットを動かす。ロボットが動作するとともに、計測部１１４はワーク形状計測センサ１０８から第１計測データを得る。第１計測データは、アース線挿入タブ１７０４の計測データである。
【００７２】
（Ｓ１９０２）
位置姿勢比較部１１６は、動作プログラム中のＬ００１のスキャンコマンド（ＭＯＶＳＣＡＮ２）とともに登録された対象物番号に対応するマスタデータ１２１を不揮発性メモリ１１７から読み出す。位置姿勢比較部１１６は、第１マスタデータに対する前記第１計測データの３次元の位置姿勢を計算して、その変化量を第１修正量とする。ただし、修正量は、マスタデータをロボット座標系１２６まわりにどれだけ動かすと計測データに一致するかを表す変換量とする。つまり、マスタデータから決まる対象物の位置姿勢ＲＴＣ２と計測データから決まる対象物の位置姿勢ＲＴＣ１の関係が式５の関係になるような変換行列Ｔａを求めて、修正量とする。
【００７３】
【数５】
2010172969.tif 000007
【００７４】
（Ｓ１９０３）
動作プログラム解析部１１０は、動作プログラム中のＬ００２に書かれたスキャンコマンド（ＭＯＶＳＣＡＮ２）の実行を指令生成部へ指令する。指令生成部１１１は動作プログラムのスキャンコマンドとともに登録された第２教示位置（Ｃ００００２に保存された教示位置）を基準としてスキャン条件に従った所定の計測範囲を計測する軌道を生成する。サーボ制御部１１２は、生成された軌道にしたがって、モータを駆動してロボットを動かす。ロボットが動作するとともに、計測部１１４はワーク形状計測センサ１０８から第２計測データを得る。第２計測データは、アース線下端１７０１の計測データである。
【００７５】
（Ｓ１９０４）
位置姿勢比較部１１６は、動作プログラム中のＬ００２のスキャンコマンド（ＭＯＶＳＣＡＮ２）とともに登録された対象物番号に対応するマスタデータ１２１を不揮発性メモリ１１７から読み出す。位置姿勢比較部１１６は、第２マスタデータに対する前記第２計測データの３次元の位置姿勢を計算して、その変化量を第２修正量とする。
【００７６】
（Ｓ１９０５）
動作プログラム解析部１１０は、動作プログラム中のＬ００３に書かれたサブルーチン（ＣＡＴＣＨ）を呼び出す。Ｌ００３は第２修正量（Ｐ０１０）を使ってアース線下端の把持位置である第２教示位置修正し、アース線下端を右グリッパで把持するための移動を行う。修正した教示位置ＲＴＧ２は、アース線把持位置であるグリッパ教示位置ＲＴＧ１と修正量Ｔａを使って式６のように計算する。
【００７７】
【数６】
2010172969.tif 000008
【００７８】
（Ｓ１９０６）
Ｌ００４に書かれた移動コマンド（ＭＯＶＬ）を実行し、アース線をタブの上まで移動させる。
【００７９】
（Ｓ１９０７）
Ｌ００５に書かれた、ロボット座標系１２６から見た右アームの現在位置を取得するコマンド（ＧＥＴＳ）を実行して、現在位置を位置変数Ｐ０１１に代入する。
【００８０】
（Ｓ１９０８）
Ｌ００６に書かれた行列の積の計算コマンド（ＭＵＬＭＡＴ）を実行して、現在位置にタブの修正量をかけた結果を位置変数Ｐ０１２に代入する。
【００８１】
（Ｓ１９０９）
Ｌ００７に書かれたＰ０１２の位置への移動コマンド（ＭＯＶＬ）を実行し、現在位置を修正した位置である、正しいタブ上の位置へ移動する。これは、現在位置である教示位置をタブ修正量をＴａとして、式６を使って修正した位置への移動となる。
【００８２】
（Ｓ１９１０）
Ｌ００８に書かれた移動コマンド（ＩＭＯＶ）を実行し、アース線をタブへ挿入する。
【００８３】
このように、スキャンコマンドと演算コマンド、移動コマンドを組み合わせた動作プログラムを記述することで、同じスキャンの教示の仕方で、把持動作および挿入動作を修正することができる。
【実施例５】
【００８４】
図２１は、第５実施例の動作を表す図である。左グリッパ１０６は、コネクタ４０４を把持している。コネクタ４０４は柔軟なリード線４０５とつながっており、もう一方の端にコネクタ４０３が接続されている。ケース１７０３上部には穴が開いている。本実施例は、コネクタ４０３をケース１７０３の穴に通す作業を行うものである。
コネクタ４０３の位置が不確定であるため、センサで計測し、左アームの移動によってコネクタ４０３をケース１７０３の穴の上へ移動させて、コネクタを穴へ通す動作を行う。
【００８５】
その動作は、図２２に示すような教示動作（Ｓ２２０１〜Ｓ２２０６）と、図２３に示すような再生動作（Ｓ２３０１〜Ｓ２３０７）になる。
【００８６】
I．教示動作
まず、教示動作（Ｓ２２０１〜Ｓ２２０６）について、図２２に基づいて説明する。
（Ｓ２２０１）
まず、コネクタ形状を計測する動作を教示するため、操作者が教示装置１０３のキーを操作する。操作者は、右グリッパ１０７をコネクタ４０３の位置へ移動するようにキーを操作する。指令生成部１１１はこのキー操作にしたがってロボットが移動する軌道を生成する。
【００８７】
（Ｓ２２０２）
操作者が教示装置１０３を操作して、スキャンコマンド（ＭＯＶＳＣＡＮ２）を教示位置と対象物番号とともに動作プログラムに登録する。この移動スキャンコマンド（ＭＯＶＳＣＡＮ２）は、ワーク形状計測センサ１０８がワークの計測データ１２０を得るスキャン動作をさせるためのコマンドさせるためのコマンドであり、させるためのコマンドであり、ＭＯＶＳＣＡＮとは、引数に指定する位置変数に入る値の点で相違する。動作プログラムは不揮発性メモリに保存される。ここで、スキャンコマンド実行後に、スキャンコマンドの引数に指定した位置変数に入る値は、修正した教示位置ではなく、修正量であり、マスタデータをロボット座標系１２６まわりにどれだけ動かすと計測データに一致するかを表す変換量である。
具体例を図２４に基づいて説明する。図２４は教示装置の動作プログラム表示画面２４０１を示している。操作者は図示しないメニューの命令一覧の画面からスキャンコマンドを選択する。選択後、操作者が教示装置１０３上のEnterキーを押すと動作プログラム２４０２に図２４のＬ００１に示すように登録される。同時に、ロボットの現在位置が教示位置データ２４０３として保存される。教示位置データ２４０３は、教示位置番号（Ｃ００００３）として保存される。
スキャンコマンド（ＭＯＶＳＣＡＮ２）は、（ｉ）教示位置番号（Ｃ００００３）、（ｉｉ）スキャン結果である修正量の成分Ｘ、Ｙ、Ｚ、Ｒｏｌｌ、Ｐｉｔｃｈ、Ｙａｗが代入される位置変数（Ｐ０１０）、（ｉｉｉ）スキャン動作の移動速度（Ｖ＝５．０）、（ｉｖ）対象物番号（ＯＢＪ＃（１））と関連づけられて動作プログラムに記録される。
【００８８】
（Ｓ２２０３）
操作者が教示装置１０３の画面上に表示されるカーソルをスキャンコマンドの行に合わせて、画面（図２４）上のスキャン実行ボタン（ＳＣＡＮ）５０８を押す。スキャン実行ボタン５０８が押されると、指令生成部１１１は、動作プログラムの教示位置（Ｃ００００３に保存された教示位置）を基準として、所定のスキャン条件に基づいて、所定の計測範囲を計測する軌道を生成する。
サーボ制御部１１２は、モータを駆動し、生成された軌道にしたがってロボットの右アームを動かす。この所定のスキャン条件は、たとえば、スキャン角度、スキャン距離、およびスキャン速度として、あらかじめ制御装置のパラメータに保存されている。ロボットが動くとともに、計測部１１４は、ワーク形状計測センサ１０８からの計測データを得る。
【００８９】
（Ｓ２２０４）
次に、教示装置のディスプレイに、得られた計測データが３次元イメージで表示される。
【００９０】
（Ｓ２２０５）
操作者がマスタ登録操作を行う。このマスタ登録操作については、再度の説明は省略する。操作者がこのマスタ登録操作を行うと、前記計測データはマスタデータとして不揮発性メモリに保存される。
【００９１】
（Ｓ２２０６）
操作者が、教示装置１０３のキーを操作して、スキャンコマンド（ＭＯＶＳＣＡＮ２）に指定した前記位置変数（Ｐ０１０）に再生ステップで入る修正量を使って、教示位置修正の演算をするコマンドと移動コマンドを動作プログラムに登録する。
Ｌ００２はスキャンコマンドの結果である修正量を同次変換行列にして逆行列を計算し、結果をＰ０１１に代入するコマンド（ＩＮＶＭＡＴ）である。変換元の位置変数（Ｐ０１０）と、計算した逆行列を代入する変数（Ｐ０１１）と関連づけられて保存される。
Ｌ００３は、ロボット座標系１２６から見た左アームの現在位置を取得するコマンド（ＧＥＴＳ）であり、現在位置を意味するシステム変数（＄ＰＸ００１）と、システム変数の内容のコピーを代入する位置変数（Ｐ０１２）と関連づけられて保存される。
Ｌ００４は、Ｐ０１１を同次変換行列にしたものとＰ０１２を同次変換行列にしたものの積を計算し、結果をＰ０１３に入れる演算コマンド（ＭＵＬＭＡＴ）である。Ｌ００４において、Ｐ０１１はコネクタの修正量の逆行列、Ｐ０１２は現在位置である。
Ｌ００５は、Ｐ０１３の位置への移動コマンド（ＭＯＶＬ）であり、位置変数（Ｐ０１３）と移動速度（Ｖ＝１０．０）と関連付けられて保存される。Ｌ００５によって、ずれている位置にあるコネクタをマスタ登録されている位置へ修正することになる。
Ｌ００６、はＰ０２０に設定されている量だけ移動する相対移動コマンド（ＩＭＯＶ）であり、位置変数（Ｐ０２０）と移動速度（Ｖ＝１０．０）と関連付けられて保存される。Ｐ０２０には、ケースの穴を通すためのコネクタの垂直方向の移動量があらかじめ設定されており、ＲＦはロボット座標系上の相対移動量として動くことを意味する。
【００９２】
以上説明したように、教示操作によって、動作プログラムとマスタデータが作成されて不揮発性メモリ１１７に保存されることになる。
【００９３】
II．再生動作
次に、再生動作（Ｓ２３０１〜Ｓ２３０７）について、図２３に基づいて説明する。この再生動作は、動作プログラムに書かれたコマンドを実行する動作である。
【００９４】
（Ｓ２３０１）
動作プログラム解析部１１０が不揮発性メモリから動作プログラムとスキャン条件を呼び出す。動作プログラム解析部１１０は、動作プログラム中のＬ００１に書かれたスキャンコマンド（ＭＯＶＳＣＡＮ２）の実行を指令生成部へ指令する。指令生成部１１１は動作プログラムのスキャンコマンドとともに登録された教示位置（Ｐ０１０）を基準としてスキャン条件に従った所定の計測範囲を計測する軌道を生成する。サーボ制御部１１２は、生成された軌道にしたがって、モータを駆動してロボットを動かす。計測部１１４はワーク形状計測センサ１０８から計測データを得る。
【００９５】
（Ｓ２３０２）
位置姿勢比較部１１６は、動作プログラム中のＬ００１のスキャンコマンド（ＭＯＶＳＣＡＮ２）とともに登録された対象物番号に対応するマスタデータ１２１を不揮発性メモリ１１７から読み出す。位置姿勢比較部１１６は、マスタデータに対する前記計測データの３次元の位置姿勢を計算して、その変化量を修正量とする。ただし、修正量は、マスタデータをロボット座標系１２６まわりにどれだけ動かすと計測データに一致するかを表す変換量とする。つまり、マスタデータから決まる対象物の位置姿勢ＲＴＣ２と計測データから決まる対象物の位置姿勢ＲＴＣ１の関係が式５の関係になるような変換行列Ｔａを求めて、修正量とする。
【００９６】
（Ｓ２３０３）
Ｌ００２に書かれた逆行列コマンド（ＩＮＶＭＡＴ）を実行し、修正量の逆行列を計算してＰ０１１に代入する。
【００９７】
（Ｓ２３０４）
Ｌ００３に書かれた、ロボット座標系１２６から見た左アームの現在位置を取得するコマンド（ＧＥＴＳ）を実行して、現在位置をＰ０１２に代入する。
【００９８】
（Ｓ２３０５）
Ｌ００４に書かれた行列の積の計算コマンド（ＭＵＬＭＡＴ）を実行して、左アームの現在位置に修正量の逆行列をかけた結果をＰ０１３に代入する。この計算は、左グリッパの現在位置を、計測したコネクタのずれ分だけ逆方向に動かすことで、コネクタの現在位置を理想の位置へ修正するためのものであり、グリッパ教示位置ＲＴＧ１と修正量Ｔａを使って式７のように計算する。
【００９９】
【数７】
2010172969.tif 000009
【０１００】
（Ｓ２３０６）
Ｌ００５に書かれた位置変数Ｐ０１３の位置への移動コマンド（ＭＯＶＬ）を実行し、ずれた位置にあるコネクタを、正しい位置であるマスタデータの位置へ移動する。
【０１０１】
（Ｓ２３０７）
Ｌ００６に書かれた移動コマンド（ＩＭＯＶ）を実行し、コネクタをケースの穴へ通す。
【０１０２】
このように、スキャンコマンドと演算コマンド、移動コマンドを組み合わせた動作プログラムを記述することで、不確定な対象の位置を正しい位置へ修正することができる。
【実施例６】
【０１０３】
本実施例は、必要な計測データが足りない場合に、自動的に再計測を行って能動的にデータを取得し、教示位置を修正するものである。
図２５は、第６実施例の動作を示すフローチャートである。教示動作については前述の図２の手順と同様であるので、その説明は省略する。
以下、再生動作について説明する。図２５の再生動作（Ｓ２５０１〜Ｓ２５０７）は、動作プログラムに書かれたスキャンコマンドと移動コマンドを実行する動作である。なお、動作プログラム表示画面は、図６に示したものと同一である。
【０１０４】
（Ｓ２５０１）
動作プログラム解析部１１０が不揮発性メモリ１１７から動作プログラム１１８とスキャン条件１１９を呼び出す。動作プログラム解析部１１０は、動作プログラム中に書かれたスキャンコマンドの実行を指令生成部１１１へ指令する。指令生成部１１１は、動作プログラム１１８のスキャンコマンドとともに登録された教示位置を基準としてスキャン条件１１９に従った所定の計測範囲を計測する軌道を生成する。サーボ制御部１１２は、生成された軌道にしたがって、モータを駆動してロボットを動かす。計測部１１４は、ワーク形状計測センサ１０８から計測データを得る。
【０１０５】
（Ｓ２５０２）
位置姿勢比較部１１６は、動作プログラム中のスキャンコマンドとともに登録された対象物番号１２５に対応するマスタデータを不揮発性メモリ１１７から読み出す。位置姿勢比較部１１６は、マスタデータ１２１に対する前記計測データの３次元の位置姿勢を計算する。
（Ｓ２５０３）
位置姿勢比較部１１６は、計測データの中にマスタデータ１２１と一致する部分が存在しない場合（例えば、マスタデータ１２１から抽出した角の点を結んでできる直方体の辺の長さが、計測データから抽出した角の点を結んでできる直方体のどの辺にも一致しない場合）、計測データの中にマスタデータ１２１と一致する部分が存在しないと判断する。一方、位置姿勢比較部１１６は、計測データの中にマスタデータ１２１と一致する部分が存在すると判断した場合は、マスタデータ１２１に対する前記計測データの３次元の位置姿勢を計算して修正量とする。
（Ｓ２５０４）
位置姿勢比較部１１６は、計測データの中にマスタデータ１２１と一致する部分が存在しないと判断した場合は、前回の計測姿勢とは異なる姿勢の計測動作の実行を指令生成部へ指令する。
図２６はマスタデータ８０１と計測データ２６０１の３次元イメージの例である。計測データ２６０１は、マスタデータに対して姿勢が大きくずれている場合のものである。
【０１０６】
位置姿勢比較部１１６は、修正量１２４の計算において、まず、特徴点の抽出を行う。図２６の３次元イメージからエッジ点を抽出すると、図２７（ａ）のような点が得られる。
２７０１はマスタデータ８０１から抽出したエッジ点を示している。２７０２は計測データ２６０１から抽出したエッジ点を示している。
【０１０７】
エッジ点のデータから直線を抽出すると、図２７（ｂ）のように、エッジ点をつなげた直線が得られる。
図において、２７０３はマスタデータの直線であり、２７０４は計測データの直線である。
【０１０８】
直線の交点を特徴点とし、図２７（ｃ）のように直線の交点に符号を付ける。このとき、スキャン方向において最初に位置する特徴点を１番とする。図２７（ｃ）では、マスタデータについては符号ｐ１〜ｐ４を付している。計測データについては符号ｑ１〜ｑ４を付している。２７０７はマスタデータの特徴点である。２７０８は計測データの特徴点である。各特徴点は位置成分Ｘ、Ｙ、Ｚを持つ。
【０１０９】
ところが、図２７（ｃ）のマスタデータと計測データでは対象物を見ている面が異なるため、形が一致するところがない。そのため、位置姿勢比較部１１６は、計測データの中にマスタデータと一致する部分が存在しないと判断する。
【０１１０】
図２８は再計測実行の例を示している。１回目の計測姿勢とは異なる姿勢で２回目の計測を行う。このとき、教示位置とは異なる位置へ動くことになるので衝突を防ぐため、１回目の計測データから、物体が存在しない範囲を２回目の計測姿勢の候補として選ぶ。
【０１１１】
図２９は同一の対象物の１回目の計測データ２６０１と２回目の計測データ３００１を表している。
図３０は図２９の３次元イメージからエッジ点を抽出し（図３０（ａ））、直線を抽出し（図３０（ｂ））、直線の交点を特徴点として番号を付ける（図３０（ｃ））手順を示している。このとき、計測データについては１回目の計測データと２回目の計測データを重ねて使用している。図３０（ｃ）において、点ｐ１、ｐ２、ｐ３、ｐ４で囲まれるマスタデータの四角形と、点ｑ１、ｑ２、ｑ３、ｑ４で囲まれる計測データの四角形が相似で、各辺の長さもほぼ一致するため、２つの四角形の姿勢の違いを修正量とする。もしも、２回目の計測データにもマスタデータと一致する部分がない場合には、ステップＳ２５０２へ戻って繰り返す。
【０１１２】
（Ｓ２５０５）
修正量が計算された場合には、指令生成部１１１は教示位置を修正量にしたがって修正する。
（Ｓ２５０６）
指令生成部１１１は、修正した教示位置をスキャンコマンドに指定された位置変数へ代入する。
【０１１３】
（Ｓ２５０７）
動作プログラム解析部が動作プログラム中に書かれた移動コマンドの実行を指令生成部へ指令し、指令生成部１１１は動作プログラムの移動コマンドに指定された位置変数の位置への軌道を生成する。
【０１１４】
このように、必要なデータが足りない場合には、自動的に再計測を行って能動的にデータを取得する。この取得したデータに基づいて修正量を計算するので、教示位置を修正することができる。
【実施例７】
【０１１５】
本実施例は、教示操作において、螺旋軌道で計測するヘリカルスキャンを行って計測したデータをマスタデータとして登録し、また、再生動作において、姿勢を変えて複数回再計測するのではなく、螺旋軌道で計測するヘリカルスキャンを１回行って計測データとする方法である。
本発明に係る方法は、教示動作については前述の図２の手順と同様であり、再生動作については図３の手順と同様である。ただし、教示動作および再生動作における計測動作を、教示位置を基準にしてスキャン条件に従ったヘリカルスキャンで行う。このスキャン条件は、スキャン角度４０６、スキャン距離４０７およびスキャン速度として、あらかじめ制御装置のパラメータに保存されている。
【０１１６】
図３１はスキャン条件から作成する螺旋軌道を示している。図３１（ａ）のように、スキャン角度４０６だけ傾けてスキャン距離だけ移動する軌道を、垂直軸３２０２まわりに、あらかじめ決められた回転角３２０３だけ回転させると、図３１（ｂ）の螺旋軌道３１０１になる。このとき、垂直方向の移動距離３２０４は、スキャン角度とスキャン距離から
（垂直方向移動距離）＝（スキャン距離）・ｃｏｓ（スキャン角度）
である。図３１（ｂ）では、教示位置を中心に、ワーク形状計測センサ１０８を教示位置の垂直軸３２０２まわりに１８０度の回転角３２０３でまわしながら、垂直軸の上方向にも同時に移動することで、螺旋軌道３１０１を描いて計測データを得る様子を示している。
このようにすることで、一回の計測で複数方向から計測したデータを得ることができるので、ワークの教示位置からの姿勢ずれが大きい場合にも対応できる。また、再生動作時においては、姿勢を変えて複数回再計測するよりも、計測時間が短くなる。
【図面の簡単な説明】
【０１１７】
【図１】本発明の第１実施例を示すロボット制御装置を含むロボットシステムの構成図
【図２】本発明の第１実施例のロボット制御装置の教示動作を示すフローチャート
【図３】本発明の第１実施例のロボット制御装置の再生動作を示すフローチャート
【図４】本発明の教示動作を示す図
【図５】本発明の第１実施例の動作プログラム表示画面の例（その１）
【図６】本発明の第１実施例の動作プログラム表示画面の例（その２）
【図７】本発明の計測データ表示画面の例
【図８】本発明のマスタデータと計測データの３次元イメージの例
【図９】本発明の特徴点抽出の例
【図１０】本発明のマスタデータから見た計測データの位置姿勢の求め方の例
【図１１】本発明の再生動作を示す図
【図１２】本発明の第２実施例の動作プログラム表示画面の例
【図１３】本発明の第２実施例のロボット制御装置を含むロボットシステムの構成図
【図１４】本発明の第２実施例のロボット制御装置の教示動作を示すフローチャート
【図１５】本発明の第２実施例のロボット制御装置の再生動作を示すフローチャート
【図１６】本発明のスキャン条件入力画面の例
【図１７】本発明の第４実施例の作業における座標系の関係図
【図１８】本発明の第４実施例のロボット制御装置の教示動作を示すフローチャート
【図１９】本発明の第４実施例のロボット制御装置の再生動作を示すフローチャート
【図２０】本発明の第４実施例の動作プログラム表示画面の例
【図２１】本発明の第５実施例の作業における座標系の関係図
【図２２】本発明の第５実施例のロボット制御装置の教示動作を示すフローチャート
【図２３】本発明の第５実施例のロボット制御装置の再生動作を示すフローチャート
【図２４】本発明の第５実施例の動作プログラム表示画面の例
【図２５】本発明の第６実施例のロボット制御装置の再生動作を示すフローチャート
【図２６】本発明の第６実施例のマスタデータと計測データの３次元イメージの例
【図２７】本発明の第６実施例の１回目の特徴点抽出の例
【図２８】本発明の第６実施例の２回目の計測動作を示す図
【図２９】本発明の第６実施例の１回目と２回目の計測データの３次元イメージの例
【図３０】本発明の第６実施例の２回目の特徴点抽出の例
【図３１】本発明の第７実施例のヘリカルスキャンの軌道の例
【符号の説明】
【０１１８】
１０１  ロボット
１０２  制御装置
１０３  教示装置
１０４  左アーム
１０５  右アーム
１０６  左グリッパ（エンドエフェクタ）
１０７  右グリッパ（エンドエフェクタ）
１０８  ワーク形状計測センサ
１０９  ワーク
１１０  動作プログラム解析部
１１１  指令生成部
１１２  サーボ制御部
１１３  教示データ記憶部
１１４  計測部
１１５  マスタデータ記憶部
１１６  位置姿勢比較部
１１７  不揮発性メモリ
１１８  動作プログラム（ＪＯＢ）
１１９  スキャン条件
１２０  計測データ
１２１  マスタデータ
１２２  現在位置
１２３  教示位置
１２４  修正量
１２５  対象物番号
４０１  グリッパ座標系
４０２  センサ座標系
４０３  コネクタ
４０４  コネクタ（左グリッパ把持）
４０５  コネクタ間リード線
４０６  スキャン角度
４０７  スキャン距離
５０１  動作プログラム表示画面
５０２  動作プログラム
５０３  教示位置データ
５０４  スキャンコマンド
５０５  教示位置番号
５０６  位置変数
５０７  スキャン動作の移動速度
５０８  スキャン実行ボタン
６０１  移動コマンド
６０２  把持動作の移動速度
７０１  計測データ表示画面
７０２  ３次元イメージ
７０３  対象物名
７０４  登録ボタン
８０１  マスタデータの３次元イメージ
８０２  計測データの３次元イメージ
９０１  マスタデータからのエッジ点抽出のイメージ
９０２  計測データからのエッジ点抽出のイメージ
９０３  マスタデータからの直線抽出のイメージ
９０４  計測データからの直線抽出のイメージ
９０５  マスタデータからの直線の交点抽出のイメージ
９０６  計測データからの直線の交点抽出のイメージ
９０７  エッジ点
９０８  直線
１２０１  把持移動コマンド
１３０１  スキャン条件入力部
１６０１  スキャン条件入力画面
１６０２  スキャン角度
１６０３  スキャン距離
１６０４  スキャン速度
１６０５  スキャン条件設定ボタン
１７０１  アース線下端
１７０２  アース線
１７０３  ケース
１７０４  アース線下端挿入タブ
２００１  動作プログラム表示画面
２００２  動作プログラム
２００３  教示位置データ
２４０１  動作プログラム表示画面
２４０２  動作プログラム
２４０３  教示位置データ
２６０１  １回目の計測データ
２７０１  マスタデータからのエッジ点抽出のイメージ
２７０２  計測データからのエッジ点抽出のイメージ
２７０３  マスタデータからの直線抽出のイメージ
２７０４  計測データからの直線抽出のイメージ
２７０５  マスタデータからの直線の交点抽出のイメージ
２７０６  計測データからの直線の交点抽出のイメージ
２７０７  エッジ点
２７０８  直線
２９０１  ２回目の計測データ
３００１  マスタデータからのエッジ点抽出のイメージ
３００２  計測データからのエッジ点抽出のイメージ
３００３  マスタデータからの直線抽出のイメージ
３００４  計測データからの直線抽出のイメージ
３００５  マスタデータからの直線の交点抽出のイメージ
３００６  計測データからの直線の交点抽出のイメージ
３００７  エッジ点
３００８  直線
３１０１  螺旋軌道
３１０２  垂直軸
３１０３  垂直軸まわりの回転角
３１０４  垂直方向の移動距離
【図１】
2010172969.tif 000010
【図２】
2010172969.tif 000011
【図３】
2010172969.tif 000012
【図４】
2010172969.tif 000013
【図５】
2010172969.tif 000014
【図６】
2010172969.tif 000015
【図７】
2010172969.tif 000016
【図８】
2010172969.tif 000017
【図９】
2010172969.tif 000018
【図１０】
2010172969.tif 000019
【図１１】
2010172969.tif 000020
【図１２】
2010172969.tif 000021
【図１３】
2010172969.tif 000022
【図１４】
2010172969.tif 000023
【図１５】
2010172969.tif 000024
【図１６】
2010172969.tif 000025
【図１７】
2010172969.tif 000026
【図１８】
2010172969.tif 000027
【図１９】
2010172969.tif 000028
【図２０】
2010172969.tif 000029
【図２１】
2010172969.tif 000030
【図２２】
2010172969.tif 000031
【図２３】
2010172969.tif 000032
【図２４】
2010172969.tif 000033
【図２５】
2010172969.tif 000034
【図２６】
2010172969.tif 000035
【図２７】
2010172969.tif 000036
【図２８】
2010172969.tif 000037
【図２９】
2010172969.tif 000038
【図３０】
2010172969.tif 000039
【図３１】
2010172969.tif 000040