(57)【特許請求の範囲】
【請求項１】
相対的に運動する対象物を把持するロボット装置において、
前記対象物を把持する把持機構を有し、前記把持機構が移動可能に構成された把持手段と、
前記把持手段の把持機構と共に移動可能に固定され、対象物を撮像する撮像手段と、
前記撮像手段が撮像した対象物の画像から１以上の特徴量の座標を抽出する特徴量抽出手段と、
前記把持機構に前記対象物を把持させたときの把持機構の位置を第１の手先位置として記憶し、前記対象物を開放させて前記撮像手段が対象物を撮像することができる範囲内において前記把持機構を移動させたときの前記把持機構の位置を第２の手先位置として記憶し、前記第２の手先位置において前記撮像手段が撮像して前記特徴量抽出手段が抽出した前記対象物の各特徴量の座標を記憶する記憶手段と、
前記第２の手先位置から前記１の手先位置まで移動するための移動行列を算出する算出手段と、
前記特徴量抽出手段によって抽出される運動中の対象物の各特徴量の座標が、前記記憶手段に記憶されている各特徴量の座標に一致するように前記把持手段を移動させることで、前記運動中の対象物に前記把持機構を追従させる制御をする追従制御手段と、
前記把持機構が運動中の対象物に追従している際に、前記算出手段が算出した移動行列に基づいて前記把持機構を前記運動中の対象物に近付け、前記対象物を把持させるように前記把持手段を制御する把持制御手段と、
を備えたロボット装置。
【請求項２】
前記記憶手段は、
前記追従制御手段によって前記把持機構が前記運動中の対象物に対して所定距離を隔てながら追従している際に、前記特徴量抽出手段が抽出した前記対象物の各特徴量の座標を新たに記憶し、
前記追従制御手段が新たに記憶された各特徴量の座標を用いて、前記第２の手先位置から前記把持機構を対象物に完全に追従させたときの前記把持機構の位置を新たな第２の手先位置として記憶する請求項１記載のロボット装置。
【請求項３】
移動可能に構成された把持機構で相対的に運動する対象物を把持するロボット装置の制御方法において、
前記把持機構に前記対象物を把持させたときの把持機構の位置を第１の手先位置として記憶し、
前記対象物を開放させて、対象物を撮像することができる範囲内において前記把持機構を移動させたときの前記把持機構の位置を第２の手先位置として記憶し、
前記第２の手先位置において前記対象物を撮像し、
前記撮像した対象物の画像から１以上の特徴量の座標を抽出し、
前記抽出した各特徴量の座標を記憶し、
前記第２の手先位置から前記１の手先位置まで移動するための移動行列を算出し、
前記把持機構と共に移動可能に固定された状態で、運動中の対象物を撮像し、
前記撮像した運動中の対象物の画像から１以上の特徴量の座標を抽出し、
前記抽出された運動中の対象物の各特徴量の座標が、前記記憶されている各特徴量の座標に一致するように前記把持機構を移動させることで、前記運動中の対象物に前記把持機構を追従させ、
前記把持機構が運動中の対象物に追従している際に、前記算出した移動行列に基づいて前記把持機構を前記運動中の対象物に近付け、前記把持機構に前記対象物を把持させるロボット装置の制御方法。
【請求項４】
前記運動中の対象物に前記把持機構を追従させ、
前記把持機構が前記運動中の対象物に対して所定距離を隔てながら追従している際に、前記把持機構と共に移動可能に固定された状態で、運動中の対象物を撮像し、
前記撮像した運動中の対象物の画像から１以上の特徴量の座標を抽出し、
前記抽出した前記対象物の各特徴量の座標を新たに記憶し、
前記新たに記憶された各特徴量の座標を用いて、前記第２の手先位置から対象物に前記把持機構が完全に追従するように前記把持機構を移動させ、
完全に追従した際に、前記把持機構の位置を新たな第２の手先位置として記憶する請求項３記載のロボット装置の制御方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ロボット装置及びその制御方法に係り、特に、運動中の物体を把持するロボット装置及びその制御方法に関する。
【０００２】
【従来の技術】
ロボットを利用して対象物に作業（例えば物体の把持）を行う場合、対象物の位置、姿勢などの情報をセンサにより取得して、そのセンサ情報をもとに作業する例が数多く実用化されている。このような装置構成としては、大別して、一般的に次の２つの方式がある。
【０００３】
例えば特開平８−６３２１４号公報に記載されているように、コンベア上を搬送される対象物を固定されたカメラで撮像し、得られた画像から対象物の位置、姿勢を検出してその情報をロボットに送信するものがある。このロボットは、対象物の位置情報から把持などの作業を行っている。
【０００４】
また、ロボットの手先にカメラなどのセンサを取り付け、前述のようにカメラからの画像から対象の位置姿勢を検出して、その情報に基づいて移動する対象物に追従して把持などの作業を行うものもある。
【０００５】
【発明が解決しようとする課題】
しかし、前者のようにロボットと別に設置したカメラを用いて対象物の把持などの作業を実現する場合、ロボットとカメラの設置位置を予め正確に求めておく作業が必要になる。また、カメラの焦点距離やレンズひずみ係数などを予め算出しておくカメラキャリブレーションを行う必要がある。
【０００６】
加えて、特開平８−６３２１４号公報などにおける例では、一般的にカメラの光軸がコンベア平面に垂直に、さらに画面中のｘ軸あるいはｙ軸がコンベアの搬送方向に平行になるように設置しておく必要がある。また、対象物がベルトコンベアなどで動いているため、カメラからの画像により検出した対象物の位置姿勢情報に加えてコンベアの速度情報などを別の手段（例えばコンベアのエンコーダ）により検出し、両者の情報に基づいてロボットへの指令値を生成する必要がある。さらに、カメラとロボットの設置位置情報、対象物の位置情報、コンベアの速度情報からロボットが対象物を把持する際の軌道を生成しなければならない。したがって、教示作業、軌道生成のための処理手順が非常に煩雑になるなどの問題がある。
【０００７】
このような問題は、従来技術におけるロボットの手先にカメラを設置した後者の例についても同じことがいえる。つまり、カメラからの画像から検出した対象物の位置姿勢情報に基づいてロボットの手先に取り付けたグリッパなどで対象物を把持する場合、カメラから得られた対象物の位置姿勢情報をロボットの基準座標系からみた位置姿勢情報に変換する必要があり、ロボット手先とカメラとの位置関係を予め精密に求めておく、いわゆるハンドアイキャリブレーションが必要となる。特に、ロボット手先にカメラなどのセンサを設置した場合、作業中の誤作動などによりロボット手先とカメラ位置関係が変化することが頻繁にあり、その場合はキャリブレーション作業を再度行う必要がある。また、前者の例と同様に対象物が動いている場合、ロボット基準座標からみた対象物の位置姿勢情報は刻々と変化するため、把持を行うための軌道生成が非常に複雑となる。
【０００８】
さらに、従来では、前者及び後者の方法とも、構成装置（コンベアの設置状況や搬送速度、あるいはカメラとロボットの設置位置関係）や把持すべき対象物の形状が変わらなくても、使用するロボット及びセンサが変わる毎に、前述の高精度なキャリブレーション、煩雑な教示作業を行う必要があった。
【０００９】
本発明は、上述した問題点を解消するために提案されたものであり、煩雑な教授作業やキャリブレーションを行う手間を省き、対象物に容易に所定の作業を行わせることができるロボット装置及びその制御方法を提供することを目的とする。
【００１０】
【課題を解決するための手段】
請求項１記載の発明は、対象物を把持する把持機構を有し、前記把持機構が移動可能に構成された把持手段と、前記把持手段の把持機構と共に移動可能に固定され、対象物を撮像する撮像手段と、前記撮像手段が撮像した対象物の画像から１以上の特徴量の座標を抽出する特徴量抽出手段と、前記把持機構に前記対象物を把持させたときの把持機構の位置を第１の手先位置として記憶し、前記対象物を開放させて前記撮像手段が対象物を撮像することができる範囲内において前記把持機構を移動させたときの前記把持機構の位置を第２の手先位置として記憶し、前記第２の手先位置において前記撮像手段が撮像して前記特徴量抽出手段が抽出した前記対象物の各特徴量の座標を記憶する記憶手段と、前記第２の手先位置から前記１の手先位置まで移動するための移動行列を算出する算出手段と、前記特徴量抽出手段によって抽出される運動中の対象物の各特徴量の座標が、前記記憶手段に記憶されている各特徴量の座標に一致するように前記把持手段を移動させることで、前記運動中の対象物に前記把持機構を追従させる制御をする追従制御手段と、前記把持機構が運動中の対象物に追従している際に、前記算出手段が算出した移動行列に基づいて前記把持機構を前記運動中の対象物に近付け、前記対象物を把持させるように前記把持手段を制御する把持制御手段と、を備えたものである。
【００１１】
請求項１記載のロボット装置は、当該ロボット装置に対して相対的に運動する対象物を把持するものである。したがって、前記ロボット装置が固定され前記対象物が運動する場合に限らず、前記ロボット装置が運動し前記対象物が固定されている場合や、前記ロボット装置と前記対象物がそれぞれ異なる速度で運動している場合も適用することができる。さらに、前記対象物は、前記ロボット装置に対して相対的に等速直線運動、等速円運動、等加速度直線運動していてもよいが、相対的に静止していてもよい。
【００１２】
最初に、オペレータは、教示作業として、次の２つのことを行う必要がある。第１に、把持機構を第１の手先位置に移動させ、ここで対象物を把持させる。第２に、把持機構から対象物をリリースさせ、撮像手段が対象物を撮像することができる範囲内において把持機構を第２の手先位置まで移動させる。このような教示作業を行い、第１及び第２の手先位置を記憶手段に記憶させる。さらに、第２の手先位置において、撮像手段が撮像した画像から抽出される１以上の特徴量の座標も記憶手段に記憶させる。撮像手段は、把持機構のセンサとなるものであり、把持手段と共に移動するように固定されている。また、第２の手先位置から第１の手先位置まで移動するための移動行列が算出される。
【００１３】
このような教示作業が完了すると、運動中の対象物を把持する把持作業を行うことができる。把持機構は、運動中の対象物の各特徴量の座標が、前記記憶手段に記憶されている各特徴量の座標に一致するように移動される。すなわち、把持機構が運動中の対象物を追従するように、いわゆるビジュアルサーボが行われる。このときの対象物と把持機構の相対的な位置関係は、教示作業時の対象物と把持機構の相対的な位置関係と同じである。前記把持機構が運動中の対象物に追従すると、前記算出手段が算出した第１の移動行列に基づいて前記把持機構を前記運動中の対象物に近付け、そして前記対象物を把持させる。
【００１４】
ビジュアルサーボを行っている場合では、運動中の対象物に把持機構が完全に追従していることが望ましい。しかし、運動中の対象物に所定距離を隔てながら把持機構が追従することがある。このような場合は、請求項２に記載されるような再教示作業が行われるのが好ましい。具体的には、前記記憶手段は、前記追従制御手段によって前記把持機構が前記運動中の対象物に対して所定距離を隔てながら追従している際に、前記特徴量抽出手段が抽出した前記対象物の各特徴量の座標を新たに記憶し、前記追従制御手段が新たに記憶された各特徴量の座標を用いて、前記第２の手先位置から前記把持機構を対象物に完全に追従させたときの前記把持機構の位置を新たな第２の手先位置として記憶すればよい。
【００１５】
請求項３記載の発明は、移動可能に構成された把持機構で対象物を把持するロボット装置の制御方法であり、前記把持機構に前記対象物を把持させたときの把持機構の位置を第１の手先位置として記憶し、前記対象物を開放させて、対象物を撮像することができる範囲内において前記把持機構を移動させたときの前記把持機構の位置を第２の手先位置として記憶し、前記第２の手先位置において前記対象物を撮像し、前記撮像した対象物の画像から１以上の特徴量の座標を抽出し、前記抽出した各特徴量の座標を記憶し、前記第２の手先位置から前記１の手先位置まで移動するための移動行列を算出し、前記把持機構と共に移動可能に固定された状態で、運動中の対象物を撮像し、前記撮像した運動中の対象物の画像から１以上の特徴量の座標を抽出し、前記抽出された運動中の対象物の各特徴量の座標が、前記記憶されている各特徴量の座標に一致するように前記把持機構を移動させることで、前記運動中の対象物に前記把持機構を追従させ、前記把持機構が運動中の対象物に追従している際に、前記算出した移動行列に基づいて前記把持機構を前記運動中の対象物に近付け、前記把持機構に前記対象物を把持させるものである。
【００１６】
この発明は、請求項１に記載されたロボット装置を制御するための方法であり、オペレータは、教示作業として上述した２つのことを行うだけでよい。すなわち、オペレータが２つの簡単な教示作業を行うだけで、運動している対象物に把持機構を追従させて、その対象物を把持することができる。
【００１７】
ビジュアルサーボを行っている場合では、運動中の対象物に把持機構が完全に追従していることが望ましい。しかし、運動中の対象物に所定距離を隔てながら把持機構が追従することがある。このような場合は、請求項４に記載されるような再教示作業が行われるのが好ましい。具体的には、前記運動中の対象物に前記把持機構を追従させ、前記把持機構が前記運動中の対象物に対して所定距離を隔てながら追従している際に、前記把持機構と共に移動可能に固定された状態で、運動中の対象物を撮像し、前記撮像した運動中の対象物の画像から１以上の特徴量の座標を抽出し、前記抽出した前記対象物の各特徴量の座標を新たに記憶し、前記新たに記憶された各特徴量の座標を用いて、前記第２の手先位置から対象物に前記把持機構が完全に追従するように前記把持機構を移動させ、完全に追従した際に、前記把持機構の位置を新たな第２の手先位置として記憶すればよい。
【００１８】
【発明の実施の形態】
以下、本発明の実施の形態について図面を参照しながら詳細に説明する。
【００１９】
（第１の実施の形態）
図１に示すように、本実施の形態に係るロボット装置は、固定された状態で、コンベア１０で搬送されている対象物１１を把持するものである。なお、本発明は、このような実施の形態に限定されるものではなく、対象物１１がロボット装置に対して相対的に運動していれば適用することができる。すなわち、ロボット装置が運動し対象物１１が静止している場合や、ロボット装置と対象物１１が異なる速度で運動している場合であってもよい。さらに、第２及び第３の実施の形態でも同様である。
【００２０】
ロボット装置は、対象物１１を撮像するＣＣＤカメラ１と、ＣＣＤカメラ１が撮像した画像から特徴量を抽出するセンサコントローラ２と、対象物１１を把持するグリッパ３と、グリッパ３を３次元方向に移動させるロボットアーム４と、座標変換や軌道生成等を行い、図示しないサーボ回路、アンプなどを有し、グリッパ３やロボットアーム４の動作を制御するロボットコントローラ５とを備えている。
【００２１】
ＣＣＤカメラ１は、ロボットアーム４の先端であるグリッパ３の近傍に固着されている。ロボットコントローラ５は、センサコントローラ２で得られた画像特徴量Ｆiに基づいて、誤差ベクトルＥ（ｔ）や速度指令値ベクトルＶ（ｔ）を算出し、これらの値に従ってロボットアーム４の移動や、グリッパ３による対象物１１の把持を制御する。なお、コンベア１０は、対象物１１を一定の速度で搬送する。
【００２２】
このような構成のロボット装置は、搬送されている対象物１１を実際に把持する前に、対象物１１を把持するための教示作業が行われる。
【００２３】
（教示作業）
最初に、オペレータは、ロボット装置に対象物１１を把持させてロボット手先位置Ｐを教示する。なお、グリッパ３などの把持機構及び対象物１１の形状によって、把持すべき位置が制限される場合がある。また、後の処理（例えば、対象物１１に他の部品を組み付けたり、部品箱に投入すること）のために、把持すべき位置を予め規定しておくこともある。
【００２４】
ここでは、対象物１１は、例えば図２に示すように、７角柱状に形成され、７つの側面１１１〜１１７を備えている。対象物１１の上面１１８には、図示しない部品を取り付けるための６つの組み付け穴（例えばねじ穴）が形成されている。なお、対象物１１の下面１１９には、このような組み付け穴は形成されていない。対象物１１の把持される部分は、把持されたときの安定性を考慮して、７つの側面のうち相対する側面１１１，１１５、または側面１１３，１１６であることが好ましい。なお、本実施の形態では上記の形状の対象物１１を用いたが、本発明はこの形状に限定されるものではなく、対象物１１は他の形状であってもよい。そして、教示作業は、具体的には図３に示すステップＳＴ１からステップＳＴ６の処理に従って行われる。
【００２５】
ステップＳＴ１において、オペレータは、図示しないティーチングペンダントなどを用いてグリッパ３が対象物１１の側面１１１，１１５または側面１１３，１１６を把持するようにして、ステップＳＴ２に移行する。
【００２６】
ステップＳＴ２において、ロボットコントローラ５は、図４に示すようにグリッパ３が対象物１１を把持した状態で、ロボット基準座標系におけるグリッパ３の位置、すなわちロボット手先位置Ｐdを記憶して、ステップＳＴ３に移行する。ここにいう「位置」は、姿勢を含む６自由度のパラメータで表される。以下同様に、姿勢を含む６自由度のパラメータを「位置」という。
【００２７】
ステップＳＴ３において、ロボットコントローラ５は、グリッパ３が把持した対象物１１をリリースさせ、そして、任意の位置にグリッパ３を移動させ、ステップＳＴ４に移行する。
【００２８】
グリッパ３が移動する位置は、ＣＣＤカメラ１の視野範囲内であることを要する。すなわち、ＣＣＤカメラ１で撮像した画像から対象物１１の特徴量が抽出されるような位置である。具体的には、対象物１１がＣＣＤカメラ１の視野内に収まっていない位置や、ＣＣＤカメラ１が対象物１１を真横から撮像する位置などではなく、抽出すべき特徴量が好適に画像から得られるような位置である。
【００２９】
ステップＳＴ４において、ロボットコントローラ５は、図５に示すように現在のロボット基準座標系でのグリッパ３の位置、すなわちロボット手先位置Ｐsを記憶し、ステップＳＴ５に移行する。
【００３０】
ここで、ロボット手先位置ＰsからＰdに変換するための位置姿勢変換行列をＰOとすると、以下の式が成り立つ。
【００３１】
Ｐｓ・ＰO＝Ｐd
∴ＰO＝Ｐｓ-1・Ｐd
ロボットコントローラ５は、このとき位置姿勢変換行列ＰOを算出し、これを記憶する。なお、ロボットコントローラ５は、後述する把持作業の際に、位置姿勢変換行列ＰOを算出してもよい。
【００３２】
ステップＳＴ５において、ＣＣＤカメラ１は、ロボット手先位置Ｐsにおいて対象物１１を撮像し、この画像をセンサコントローラ２に供給する。センサコントローラ２は、ＣＣＤカメラ１からの画像に基づいて画像特徴量を抽出して、ステップＳＴ６に移行する。ここで、ＣＣＤカメラ１は、例えば図６に示すような対象物１１の画像を得る。
【００３３】
ステップＳＴ６において、センサコントローラ２は、対象物１１の上面１１８に加工されている６つの組み付け穴を画像特徴量Ｆi（ｉ＝１〜６）として抽出し、各画像特徴量Ｆiの中心位置の座標値（Ｘi，Ｙi）記憶して、教示作業の処理を終了する。
【００３４】
なお、画像中の組み付け穴の中心位置の計測方法としては、二値化画像処理やエッジ処理などの公知の手法を用いればよい。また、ここでは組み付け穴の中心位置を画像特徴量Ｆiとしたが、線分端点、コーナ、二値化画像の孤立領域の面積重心など、他の画像特徴量を用いてもよい。さらに、教示時は、オペレータが画像特徴量Ｆiをマウスやキーボード等の図示しない入力手段により人為的に定義してもよい。
【００３５】
（把持作業）
ロボット装置は、教示作業が終了すると、その作業の際に記憶した画像特徴量Ｆiの座標や、ロボット手先位置Ｐd，Ｐsを用いて、搬送中の対象物１１を把持する。この把持作業では、図７に示すステップＳＴ１１からステップＳＴ１８までの処理が実行される。
【００３６】
ステップＳＴ１１において、ＣＣＤカメラ１は、図８に示すように、対象物１１の搬送領域、すなわちコンベア１０上を撮像し、ステップＳＴ１２に移行する。
【００３７】
ステップＳＴ１２において、センサコントローラ２は、対象物１１を検出したかを判定する。対象物１１を検出し、その画像から特徴量を抽出するためには、対象物１１全体がＣＣＤカメラ１の視野の範囲内に入ることが必要である。ここでは、対象物１１の検出のために画像の輝度が用いられる。例えば、対象物１１がコンベア１０の上流にあってＣＣＤカメラ１の視野範囲内にないときは、ＣＣＤカメラ１で得られる画像の輝度はほとんど変化しない。一方、対象物１１がコンベアにより搬送されＣＣＤカメラ１の視野範囲内に入ると、その輝度は変化する。そこで、センサコントローラ２は、画像中の適当な位置に図示しないウィンドウを設け、そのウィンドウ内の輝度が所定の閾値以上の場合は対象物１１が視野範囲内にあると判定し、輝度が所定の閾値未満の場合はその視野の範囲内にないと判定する。そして、センサコントローラ２が対象物１１を検出したときはステップＳＴ１３に移行し、検出しなかったときはステップＳＴ１１に戻る。
【００３８】
ステップＳＴ１３において、センサコントローラ２は、ＣＣＤカメラ１が撮像した画像から時刻Ｔにおける画像特徴量ｆi（ｔ）（i＝１，２・・・）を抽出して、ステップＳＴ１４に移行する。ここで、図９は、把持作業の実行時において撮像した画像を示す図である。把持作業時には、時刻ｔに撮像された画像から画像特徴量ｆi（ｔ）を抽出する必要がある。しかし、何らかの原因で本来抽出すべきでない例えば画像特徴量ｆ３（ｔ）が抽出されることがある。そこで、特徴量ｆ１（ｔ）〜ｆ７（ｔ）が、図６に示すＦ１〜Ｆ６のいずれに対応しているのかを求める対応点探索を行う必要がある。
【００３９】
この対応点探索は、さまざまな方法が提案され、例えば”ロボットビジョン”（谷内田正彦著、昭晃堂、１９９０）に記載されている動的計画法や最大派閥法など、いわゆる照合法により行うことができる。なお、一旦、ｆ１〜ｆ７の中からＦ１〜Ｆ６に対応するものが得られれば、次の画像特徴量は容易に求めることができる。
【００４０】
具体的には、ステップＳＴ１３からステップＳＴ１６までの繰り返し処理のサイクルタイムをΔｔとすると、時刻ｔの次の時刻（ｔ＋Δｔ）における画像特徴量ｆi（ｔ＋Δｔ）は先の画像中での画像特徴量ｆi（ｔ）の近傍から抽出すればよい。これにより、特徴量抽出の安定性が増し、かつ、計算処理の負担を軽減することができる。
【００４１】
ステップＳＴ１４において、センサコントローラ２は、時刻ｔにおける誤差ベクトルＥ（ｔ）が十分小さいか、具体的には｜｜Ｅ（ｔ）｜｜＜εの判定基準を満たすかを判定し、十分小さいときはステップＳＴ１７に移行し、十分小さくないときはステップＳＴ１５に移行する。
【００４２】
ここでは、現在のＣＣＤカメラ１の画像から抽出された画像特徴量ｆi（ｔ）と上述したステップＳＴ６で記憶された画像特徴量Ｆiとに基づいて、誤差ベクトルＥ（ｔ）を求める。
【００４３】
本発明では、文献B.Espiauら、A New Approach to Visual Servoing in Robotics,IEEE Trans.Robotics and Automation,8(3),313-326,1992や、文献F.Bensalhaら、Real Time Visual Tracking using the Generalized Likelihood Ratio Test,Int.Conf.Automation,Robotics and Computer Vision,1379-1383,1994に記載されている手法に基づいて、誤差ベクトルＥ（ｔ）と、後述する速度指令値ベクトルＶ（ｔ）とを算出する。
【００４４】
教示時の画像特徴量Ｆi＝（Ｘi，Ｙi）および時刻ｔで撮像した画像から得られた画像特徴量ｆi（ｔ）＝（Ｘi（ｔ），Ｙi（ｔ））を用いて、特徴量偏差ベクトルｅ（ｔ）を以下の式（１）で求める。
【００４５】
【数１】
0004265088.tif 000002
【００４６】
（ｘi，ｙi）および（ｘi（ｔ），ｙi（ｔ））は、画素値で表される（Ｘi，Ｙi）および（Ｘi（ｔ），Ｙi（ｔ））を、それぞれカメラ座標系での値に変換したものである。また、Ｎは画像特徴量の個数を表す。
【００４７】
上記特徴量偏差ベクトルｅ（ｔ）を用いて誤差ベクトルＥ（ｔ）は、式（２）で表される。
【００４８】
【数２】
0004265088.tif 000003
【００４９】
ここで、ＬT+は、画像特徴量Ｆ，Ｆ（ｔ）から求められる画像ヤコビアンＬTの疑似逆行列である。
【００５０】
また、センサコントローラ２は、｜｜Ｅ（ｔ）｜｜＜εの判定基準を満たしたときに、運動する対象物１１に追従したと判定する。εは、オペレータによって予め与えられるスカラー量である。あるいは、
Ｅ＝（Ｅ1 Ｅ2 Ｅ3 Ｅ4 Ｅ5 Ｅ6 ）T
と表せば、式（３）に示す判定基準を用いてもよい。
【００５１】
【数３】
0004265088.tif 000004
【００５２】
ステップＳＴ１５において、ロボットコントローラ５は、速度指令値ベクトルＶ（ｔ）を算出し、ステップＳＴ１６に移行する。なお、速度指令値ベクトルＶは、ロボット手先位置Ｐの並進速度（ｔx，ｔy，ｔz）および回転速度（ωx，ωy，ωz）を含んだものであり、
Ｖ＝（ｔx ｔy ｔz ωx ωy ωz）T
と表される。そして、時刻（ｔ＋Δｔ）における速度指令値ベクトルＶ（ｔ＋Δｔ）は、式（４）から求められる。
【００５３】
【数４】
0004265088.tif 000005
【００５４】
なお、λは速度ゲインである。式（４）の右辺の最右項は、対象物１１の運動速度の推定値であり、式（５）から求められる。
【００５５】
【数５】
0004265088.tif 000006
【００５６】
ロボットコントローラ５は、以上のようにして、誤差ベクトルＥ（ｔ）及び速度指令値ベクトルＶ（ｔ）を算出する。
【００５７】
ステップＳＴ１６において、ロボットコントローラ５は、算出された速度指令値ベクトルＶ（ｔ）に基づいて、軌道生成、関節角速度・角加速度を生成し、これらに従ってロボットアーム４を制御し、ステップＳＴ１３に戻る。
【００５８】
ステップＳＴ１４でＥ（ｔ）が十分に小さいと判定したときのステップＳＴ１７において、ロボットコントローラ５は、グリッパ３が対象物１１に追従したと判定して、対象物１１に対してアプローチを開始する。なお、以下の説明では、図７に示すフローチャートの処理を行うことによってグリッパ３を対象物に追従させることをビジュアルサーボという。
【００５９】
ここで、ステップＳＴ１４に移行した時刻を時刻ｔ1とすると、例えば図１０に示すように、速度指令値ベクトルはＶactual（ｔ1）、ロボット手先位置の位置ベクトルはＰ（ｔ1）となる。
【００６０】
時刻ｔ1においては、ロボット手先位置Ｐは対象物１１に追従しているため、そのときの速度指令値ベクトルＶactual（ｔ1）は対象物１１の速度ベクトルに等しい。したがって、ロボットコントローラ５は、手先位置Ｐ（ｔ1）からＰO分だけ移動するための速度パターンに、上述した速度指令値ベクトルＶactual（ｔ1）を重畳させた速度パターンを生成する。なお、ＰOは、教示作業で示した位置姿勢変換行列である。また、ＰO分だけ移動するための速度パターンは、例えば台形速度パターンなど従来の手法により算出することができる。
【００６１】
また、ロボットコントローラ５は、ＰO分相対移動するのに時間ＴOを要するとして、以下の式に従ってＰaを算出する。
【００６２】
Ｐa＝Ｐ（ｔ1）＋ＰO＋Ｖactual（ｔ1）・ＴO
そして、ロボットコントローラ５は、算出したＰaに基づいてロボットアーム４を制御する。これにより、図１１に示すように、手先位置Ｐ（ｔ1）から対象物１１を把持すべき位置Ｐaにロボット手先（グリッパ３）を移動させることができる。このようなアプローチが終了すると、ステップＳＴ１８に移行する。
【００６３】
ステップＳＴ１８において、ロボットコントローラ５は、対象物１１をグリッパ３で把持して、処理を終了する。
【００６４】
以上のように、ロボット装置は、教示作業時におけるロボット手先位置Ｐs，Ｐdを記憶し、ビジュアルサーボによって搬送中の対象物１１に追従し、そしてロボット手先位置Ｐs，Ｐdを用いて対象物１１の把持作業を行うことができる。これにより、ロボット装置は、搬送中の対象物１１の姿勢位置に拘束されることなく、また、コンベア１０を止めることなく、搬送中の対象物１１を容易に把持することができる。
【００６５】
また、上記ロボット装置は、その設置位置、ＣＣＤカメラ１、グリッパ３及び対象物１１相互間の位置関係を予め求めておく作業が不要なので、例えばＣＣＤカメラ１の設置条件が変化した場合であっても、上述したような単純な教示作業のみで、搬送中の対象物１１を把持することができる。さらに、同じ作業を異なるロボット装置、ＣＣＤカメラ１、グリッパ３で実現しようとした場合や、把持すべき対象物１１が異なる場合であっても、同様に、単純な教示作業のみでよい。
【００６６】
これにより、オペレータは、従来の教示作業、プログラミングにおける負荷を大幅に軽減することができる。さらに、ＣＣＤカメラ１、グリッパ３及び対象物１１に変更が生じても、柔軟に対応することができる。
【００６７】
なお、上述した実施の形態では、ロボット装置は、コンベア１０によって搬送されている対象物１１を把持することを例に挙げて説明したが、静止している対象物１１を把持する場合も同様にすることができる。なお、教示作業は、上述した説明と同様である。
【００６８】
把持作業では、図７に示すステップＳＴ１４において、誤差ベクトルＥ（ｔ）が十分に小さくなったときは、速度指令値ベクトルＶ（ｔ）はゼロになり、同様に、実際のロボット手先位置の速度Ｖactual（ｔ1）もゼロになる。
【００６９】
したがって、ステップＳＴ１７において、ロボットコントローラ５は、現在のロボット手先位置Ｐ（ｔ1）と、教示時における位置の変化分ＰOのみを用いて対象物１１を把持することができる。すなわち、把持すべき位置Ｐaは、以下の式により求められる。
【００７０】
Ｐa＝Ｐ（ｔ1）＋ＰO
そして、ロボットコントローラ５は、算出したＰaに基づいてロボットアーム４を制御し、グリッパ３により静止している対象物１１の把持を行うことができる。
【００７１】
（第２の実施の形態）
つぎに、本発明の第２の実施の形態について説明する。なお、第１の実施の形態と同じ部位には同じ符号を付し、また、重複する説明についてはその記載を省略する。
【００７２】
第１の実施の形態では、搬送中の対象物とロボット手先位置Ｐとの相対的な位置関係が、教示作業時のそれらの相対的な位置関係とほぼ同じになる完全追従状態について説明した。これに対して、第２の実施の形態では、完全追従状態でない場合にロボット装置が対象物を把持することについて説明する。
【００７３】
ここで、追従状態とは、相対位置関係を一定に保った状態と定義する。例えば、対象物が速度Ｖ0で運動している場合では、ロボット手先位置Ｐが速度Ｖ0で運動している状態である。次に、追従状態として、「完全追従状態」と「不完全追従状態」の２つを定義する。
【００７４】
「完全追従状態」とは、ロボット手先位置Ｐと対象物との相対的な位置関係が一定値ＰOのままで一定時間継続している状態、すなわち教示作業時のそれらの相対的な位置関係と同じになっている状態をいう。「不完全追従状態」とは、ロボット手先位置Ｐと対象物との相対的な位置関係が一定値Ｐ’0（≠ＰO）のままで一定時間継続している状態、すなわちロボット手先位置Ｐと対象物との間で相対的な位置の変化はないが、教示作業時の相対的な位置関係と異なる状態をいう。これらの２つの状態について、画像誤差、ロボット手先位置Ｐ、ロボット手先位置Ｐの速度、ロボット手先位置Ｐの加速度の状態を表１に示す。
【００７５】
【表１】
0004265088.tif 000007
【００７６】
「完全追従状態」では、ロボット手先位置Ｐと対象物の相対的な位置関係はＰOであり、しかも、ロボット手先位置Ｐは対象物と同じ速度で移動する。このため接近動作は、追従運動Ｖを維持しつつ、接近移動ＰOの移動を行うことで、把持作業を行うことができる。
【００７７】
一方、「不完全追従状態」でも、ロボット手先位置Ｐは対象物と同じ速度で移動している。ただし、ロボット手先位置Ｐと対象物との相対的な位置関係Ｐ’0は、教示作業時の位置関係ＰOとは異なる。例えば図１２（Ａ）に示すように、追従開始時の対象物１１の初期画像は、目標画像に一致していない。なお、このときは、早く追従状態になるようにするため、ロボット手先位置Ｐsの速度は、図１３に示すように、大きくなっている。図１２（Ｂ）に示すように、追従状態の対象物１１の画像は、目標画像に対して所定のずれを維持した状態になっている。このときのロボット手先位置Ｐsの速度は、図１３に示すように、Ｖactual（ｔ1）になっている。
【００７８】
そこで、相対位置関係Ｐ’0を教示することができれば、図１２（Ｃ）に示すように、「完全追従状態」と全く同一の効果を得ることができる。なお、「不完全追従状態」における位置関係Ｐ’0は、対象物の運動速度の変化やビジュアルサーボのゲインの変更等で変化する可能があり、その度に再教示が必要である。本実施の形態では、「不完全追従状態」にも対応することができるように、再教示作業が行われる。
【００７９】
（再教示作業及び把持作業）
再教示作業では、最初に、ビジュアルサーボを用いて、対象物の追従が行われる。そして、ロボットコントローラ５は、このとき対象物の画像特徴量Ｆ’iを抽出して、各画像特徴量Ｆ’iの座標を記憶する。なお、画像特徴量Ｆ’iの抽出処理は、図３に示すステップＳＴ６の処理と同様である。
【００８０】
つぎに、図３に示すステップＳＴ１からステップＳＴ３までの処理が行われ、グリッパ３は任意の位置（ロボット手先位置Ｐs）に移動させられる。ロボットコントローラ５は、さらに、画像特徴量Ｆi’を用いてビジュアルサーボを行い、追従状態に到達したグリッパ３の位置をロボット手先位置Ｐ’sとする。そして、ロボットコントローラ５は、追従状態における位置姿勢変換行列Ｐ’O（＝Ｐ’S-1Ｐd）を算出し、この位置姿勢変換行列Ｐ’Oを記憶する。
【００８１】
把持作業では、第１の実施の形態と同様に、図７に示すステップＳＴ１１からステップＳＴ１８までの処理を行う。このとき、第１の実施の形態で求めた画像特徴量Ｆi及び位置姿勢変換ＰOに代えて、画像特徴量Ｆ’i及び位置姿勢変換業列Ｐ’Oを用いればよい。
【００８２】
以上のように、第２の実施の形態に係るロボット装置によれば、ロボット手先位置Ｐが対象物１１に完全に追従していなくても、複雑な調整をすることなく上述した簡単な教示作業を行うだけで、対象物１１を把持することができるようになる。
【００８３】
（第３の実施の形態）
第１及び第２の実施の形態では、一例として静止運動、等速直線運動の場合を示したが、本発明を用いれば等加速度直線運動あるいは等速円運動まで拡大することができる。そして、この把持作業でも、第１の実施の形態と同様に、図７に示すステップＳＴ１１からステップＳＴ１７までの処理が行われる。
【００８４】
具体的には、等加速度直線運動の場合、ロボットコントローラ５は、誤差ベクトルＥ（ｔ）が十分に小さくなった時刻ｔ1において、ロボット手先位置Ｐは対象物１１に対して相対的にＰOあるいはＰ’Oの位置に制御されたと判定し、この時刻ｔ1でのＶactual（ｔ1）と、手先加速度ａｃｃactual（ｔ1）から生成される速度成分Ｖestimate（ｔ）と、ＰOあるいはＰ’Oより生成される速度指令値とを重ね合わせた速度指令値を生成し、時刻ｔ1でのロボット手先位置Ｐ（ｔ1）から対象物１１に接近し、グリッパ３により対象物１１の把持を行う。
【００８５】
すなわち、ロボットコントローラ５は、図１４に示すように、追従状態になったときのロボット手先位置ＰsからＰO分だけグリッパ３を移動するように制御する。なお、ロボットコントローラ５は、このときＰO分相対移動するのに時間Ｔ’Oを要するとして、以下の式に従ってＰaを算出する。
【００８６】
【数６】
0004265088.tif 000008
【００８７】
なお、上述した実施の形態では、対象物を把持する場合を例に挙げて説明したが、本発明はこれに限定されるものではない。例えば、対象物１１に部品の取り付け等を行う場合であっても同様にして適用することができる。
【００８８】
【発明の効果】
本発明に係るロボット装置及びその制御方法によれば、オペレータは、教示作業として、次の２つのことを行うだけでよい。第１に、把持機構を第１の手先位置に移動させ、ここで対象物を把持させる。第２に、把持機構から対象物をリリースさせ、撮像範囲内において把持機構を第２の手先位置まで移動させる。このとき記憶される第１及び第２の手先位置や、第２の手先位置における対象物の特徴量の座標は、対象物を追従してこれを把持するために用いられる。すなわち、オペレータは簡単な教示作業を行うだけで、ロボット装置に運動中の対象物を把持させることができる。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に係るロボット装置の構成を示す図である。
【図２】ロボット装置によって把持される対象物の外観斜視図である。
【図３】ロボット装置の教示作業における処理内容を示すフローチャートである。
【図４】教示作業において、対象物を把持した状態のロボット手先位置Ｐdを示す概略的な構成を示す図である。
【図５】教示作業において、対象物をリリースしたときのロボット手先位置Ｐsを示す概略的な構成を示す図である。
【図６】教示作業におけるカメラの撮像画像を示す図である。
【図７】ロボット装置の把持作業における処理内容を示すフローチャートである。
【図８】把持作業において、カメラが対象物を搬送するコンベア上を撮像していることを説明するための図である。
【図９】把持作業におけるカメラの撮像画像を示す図である。
【図１０】時刻ｔ1におけるロボット手先位置Ｐ（ｔ1）及び速度指令値ベクトルＶactual（ｔ1）を説明するための図である。
【図１１】対象物を把持するときの状態を説明する図である。
【図１２】第２の実施の形態に係るロボット装置において初期状態、追従状態、再教示時の対象物の画像を示す図である。
【図１３】ロボット手先位置Ｐの速度指令値ベクトルＶ（ｔ）を説明する図である。
【図１４】本発明の第３の実施の形態に係るロボット装置が搬送中の対象物を把持した状態を説明する図である。
【符号の説明】
１  カメラ
２  センサコントローラ
３  グリッパ
４  ロボットアーム
５  ロボットコントローラ
【図１】
0004265088.tif 000009
【図２】
0004265088.tif 000010
【図３】
0004265088.tif 000011
【図４】
0004265088.tif 000012
【図５】
0004265088.tif 000013
【図６】
0004265088.tif 000014
【図７】
0004265088.tif 000015
【図８】
0004265088.tif 000016
【図９】
0004265088.tif 000017
【図１０】
0004265088.tif 000018
【図１１】
0004265088.tif 000019
【図１２】
0004265088.tif 000020
【図１３】
0004265088.tif 000021
【図１４】
0004265088.tif 000022