(57)【特許請求の範囲】
【請求項１】
６自由度以上のマニピュレータを有し、マニピュレータの手先位置に撮像手段を有するロボット装置を制御するロボット装置の制御方法であって、
前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記マニピュレータの手先の方向を旋回させたときの前記対象物の画像の前記撮像手段における移動量に基づいて、前記対象物の画像が前記撮像手段の視野の中心に位置するように、前記マニピュレータの手先の方向を制御し、
次に、前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記撮像手段によって得られた対象物の画像の大きさに基づいて、前記マニピュレータの手先と前記対象物との間の距離を算出して、前記マニピュレータの手先を前記撮像手段のレンズの光軸方向に移動させ、該手先を前記対象物に接近させ、  
次に、前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記マニピュレータの手先の方向を、前記対象物の計測面に垂直な方向とし、
次に、前記撮像手段の視野内における前記対象物の画像が所定の方向となるように、前記マニピュレータの手先を回転制御する
ことを特徴とするロボット装置の制御方法。
【請求項２】
前記対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっている
ことを特徴とする請求項１記載のロボット装置の制御方法。
【請求項３】
前記対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有している
ことを特徴とする請求項１記載のロボット装置の制御方法。
【請求項４】
前記マニピュレータの手先の方向を制御して、前記対象物の画像を前記撮像手段の視野の中心に位置させることは、
前記撮像手段によって得られた画像上での前記視野の中心Ｃ０及び前記画像上での前記対象物の初期中心位置Ｃ１を取得し、選択した動作軸を回転させて前記画像上での前記対象物の中心位置Ｃ２を取得し、直線Ｃ０−Ｃ１間及び直線Ｃ１−Ｃ２間の距離及び傾きを求め、次に、直線Ｃ１−Ｃ２に垂直で視野中心Ｃ０を通る直線と、直線Ｃ１−Ｃ２との交点Ａを定め、直線Ｃ０−Ａ及び直線Ｃ２−Ａ間の距離を求めることにより、前記マニピュレータの関節の前記選択した動作軸回りの回転量を定め、
次に、前記選択した動作軸に直行する動作軸を回転させて前記画像上での前記対象物の中心位置Ｃ２を取得し、直線Ｃ０−Ｃ１間及び直線Ｃ１−Ｃ２間の距離及び傾きを求め、次に、直線Ｃ１−Ｃ２に垂直で視野中心Ｃ０を通る直線と、直線Ｃ１−Ｃ２との交点Ａを定め、直線Ｃ０−Ａ及び直線Ｃ２−Ａ間の距離を求めることにより、前記マニピュレータの当該動作軸回りの回転量を定め、
定められた前記各回転量に基づいて前記マニピュレータを制御し、前記対象物の画像を前記撮像手段の視野の中心とする
ことを特徴とする請求項１記載のロボット装置の制御方法。
【請求項５】
６自由度以上のマニピュレータを有し、マニピュレータの手先位置に撮像手段を有するロボット装置であって、
前記マニピュレータの動作を制御する制御手段を備え、
前記制御手段は、前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記マニピュレータの手先の方向を旋回させたときの前記対象物の画像の前記撮像手段における移動量に基づいて、前記対象物の画像が前記撮像手段の視野の中心に位置するように、前記マニピュレータの手先の方向を制御し、次に、前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記撮像手段によって得られた対象物の画像の大きさに基づいて、前記マニピュレータの手先と前記対象物との間の距離を算出して、前記マニピュレータの手先を前記撮像手段のレンズの光軸方向に移動させ、該手先を前記対象物に接近させ、次に、前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記マニピュレータの手先の方向を、前記対象物の計測面に垂直な方向とし、次に、前記撮像手段の視野内における前記対象物の画像が所定の方向となるように、前記マニピュレータの手先を回転制御する
ことを特徴とするロボット装置。
【請求項６】
前記対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっている
ことを特徴とする請求項５記載のロボット装置。
【請求項７】
前記対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有している
ことを特徴とする請求項５記載のロボット装置。
【請求項８】
前記制御手段は、前記マニピュレータの手先の方向を制御して、前記対象物の画像を前記撮像手段の視野の中心に位置させることは、
前記撮像手段によって得られた画像上での前記視野の中心Ｃ０及び前記画像上での前記対象物の初期中心位置Ｃ１を取得し、選択した動作軸を回転させて前記画像上での前記対象物の中心位置Ｃ２を取得し、直線Ｃ０−Ｃ１間及び直線Ｃ１−Ｃ２間の距離及び傾きを求め、次に、直線Ｃ１−Ｃ２に垂直で視野中心Ｃ０を通る直線と、直線Ｃ１−Ｃ２との交点Ａを定め、直線Ｃ０−Ａ及び直線Ｃ２−Ａ間の距離を求めることにより、前記マニピュレータの関節の前記選択した動作軸回りの回転量を定め、
次に、前記選択した動作軸に直行する動作軸を回転させて前記画像上での前記対象物の中心位置Ｃ２を取得し、直線Ｃ０−Ｃ１間及び直線Ｃ１−Ｃ２間の距離及び傾きを求め、次に、直線Ｃ１−Ｃ２に垂直で視野中心Ｃ０を通る直線と、直線Ｃ１−Ｃ２との交点Ａを定め、直線Ｃ０−Ａ及び直線Ｃ２−Ａ間の距離を求めることにより、前記マニピュレータの当該動作軸回りの回転量を定め、
定められた前記各回転量に基づいて前記マニピュレータを制御し、前記対象物の画像を前記撮像手段の視野の中心とする
ことを特徴とする請求項５記載のロボット装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、マニピュレータの先端にカメラを備えたロボット装置において、事前に目標状態から算出し記憶した目標特徴量とカメラにより取得した画像から得た特徴量とが一致するようにマニピュレータを動作させるロボット装置の制御方法及びロボット装置に関する。
【背景技術】
【０００２】
従来、マニピュレータを備えたロボット装置が提案されており、また、このようなロボット装置のマニピュレータの動作を制御するためのロボット装置の制御方法が提案されている。そして、ロボット装置の制御方法として、マニピュレータにより把持を行う対象物を目標として、マニピュレータの手先を移動させるための制御方法が種々提案されている。
【０００３】
例えば、特許文献１には、対象物の基準画像を複数用意しておくとともに、マニピュレータの手先に取り付けたカメラによる画像情報を取得し、画像情報における対象物の形状に近い基準画像を検索することによって、対象物とマニピュレータの手先との位置関係を計算するようにしたロボット装置が記載されている。
【０００４】
また、特許文献２には、対象物の基準画像を用意しておき、マニピュレータの手先に取り付けたカメラにより画像を撮影しながら、基準画像に近い画像が得られるまで、マニピュレータを一定量移動させるようにしたロボット装置が記載されている。
【０００５】
【特許文献１】特開２００３−２３１０７８公報
【特許文献２】特開２０００−２６３４８２公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
ところで、特許文献１に記載のロボット装置においては、制御の精度を高めるためには、多くの基準画像を用意しておき、マニピュレータの動作のたびに全ての基準画像の検索を行なう必要があるため、計算負荷が高く、迅速な制御ができない。
【０００７】
また、特許文献２に記載のロボット装置においては、２次元平面内におけるマニピュレータの位置及び姿勢合わせにしか対応できず、マニピュレータの対象物に対する位置及び姿勢を３次元空間内において制御することはできない。さらに、このロボット装置においては、撮像、計測とマニピュレータの動作とを繰り返さなければならないため、迅速な制御ができない。
【０００８】
そこで、本発明は、前記の実情に鑑みてなされたものであり、その目的は、ロボット装置のマニピュレータを制御するにあたり、多くの基準画像を用意する必要がなく、計算負荷が軽く、迅速な制御が可能でありながら、マニピュレータの対象物に対する位置及び姿勢を３次元空間内において制御することができるようになされたロボット装置の制御方法及びロボット装置を提供することにある。
【課題を解決するための手段】
【０００９】
前述の課題を解決し、前記目的を達成するため、本発明に係るロボット装置の制御方法は、以下の構成のいずれか一を有するものである。
【００１０】
〔構成１〕
６自由度以上のマニピュレータを有し、マニピュレータの手先位置に撮像手段を有するロボット装置を制御するロボット装置の制御方法であって、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、マニピュレータの手先の方向を旋回させたときの対象物の画像の撮像手段における移動量に基づいて、対象物の画像が撮像手段の視野の中心に位置するように、マニピュレータの手先の方向を制御し、次に、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、撮像手段によって得られた対象物の画像の大きさに基づいて、マニピュレータの手先と対象物との間の距離を算出して、マニピュレータの手先を撮像手段のレンズの光軸方向に移動させ、該手先を対象物に接近させ、次に、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、マニピュレータの手先の方向を、対象物の計測面に垂直な方向とし、
次に、撮像手段の視野内における対象物の画像が所定の方向となるように、マニピュレータの手先を回転制御することを特徴とするものである。
【００１１】
〔構成２〕
構成１を有するロボット装置の制御方法において、対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっていることを特徴とするものである。
【００１２】
〔構成３〕
構成１を有するロボット装置の制御方法において、対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有していることを特徴とするものである。
【００１３】
〔構成４〕
構成１を有するロボット装置の制御方法において、マニピュレータの手先の方向を制御して、対象物の画像を撮像手段の視野の中心に位置させることは、撮像手段によって得られた画像上での視野の中心Ｃ０及び画像上での対象物の初期中心位置Ｃ１を取得し、選択した動作軸を回転させて画像上での対象物の中心位置Ｃ２を取得し、直線Ｃ０−Ｃ１間及び直線Ｃ１−Ｃ２間の距離及び傾きを求め、次に、直線Ｃ１−Ｃ２に垂直で視野中心Ｃ０を通る直線と、直線Ｃ１−Ｃ２との交点Ａを定め、直線Ｃ０−Ａ及び直線Ｃ２−Ａ間の距離を求めることにより、マニピュレータの関節の選択した動作軸回りの回転量を定め、次に、選択した動作軸に直行する動作軸を回転させて画像上での対象物の中心位置Ｃ２を取得し、直線Ｃ０−Ｃ１間及び直線Ｃ１−Ｃ２間の距離及び傾きを求め、次に、直線Ｃ１−Ｃ２に垂直で視野中心Ｃ０を通る直線と、直線Ｃ１−Ｃ２との交点Ａを定め、直線Ｃ０−Ａ及び直線Ｃ２−Ａ間の距離を求めることにより、マニピュレータの当該動作軸回りの回転量を定め、定められた各回転量に基づいてマニピュレータを制御し、対象物の画像を撮像手段の視野の中心とすることを特徴とするものである。
【００１５】
また、本発明に係るロボット装置は、以下の構成のいずれか一を有するものである。
【００１６】
〔構成５〕
６自由度以上のマニピュレータを有し、マニピュレータの手先位置に撮像手段を有するロボット装置であって、マニピュレータの動作を制御する制御手段を備え、制御手段は、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、マニピュレータの手先の方向を旋回させたときの対象物の画像の撮像手段における移動量に基づいて、対象物の画像が撮像手段の視野の中心に位置するように、マニピュレータの手先の方向を制御し、次に、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、撮像手段によって得られた対象物の画像の大きさに基づいて、マニピュレータの手先と対象物との間の距離を算出して、マニピュレータの手先を撮像手段のレンズの光軸方向に移動させ、該手先を対象物に接近させ、次に、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、マニピュレータの手先の方向を、対象物の計測面に垂直な方向とし、次に、撮像手段の視野内における対象物の画像が所定の方向となるように、マニピュレータの手先を回転制御することを特徴とするものである。
【００１７】
〔構成６〕
構成５を有するロボット装置において、対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっていることを特徴とするものである。
【００１８】
〔構成７〕
構成５を有するロボット装置において、対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有していることを特徴とするものである。
【００１９】
〔構成８〕
構成５を有するロボット装置において、制御手段は、マニピュレータの手先の方向を制御して、対象物の画像を撮像手段の視野の中心に位置させることは、撮像手段によって得られた画像上での視野の中心Ｃ０及び画像上での対象物の初期中心位置Ｃ１を取得し、選択した動作軸を回転させて画像上での対象物の中心位置Ｃ２を取得し、直線Ｃ０−Ｃ１間及び直線Ｃ１−Ｃ２間の距離及び傾きを求め、次に、直線Ｃ１−Ｃ２に垂直で視野中心Ｃ０を通る直線と、直線Ｃ１−Ｃ２との交点Ａを定め、直線Ｃ０−Ａ及び直線Ｃ２−Ａ間の距離を求めることにより、マニピュレータの関節の選択した動作軸回りの回転量を定め、次に、選択した動作軸に直行する動作軸を回転させて画像上での対象物の中心位置Ｃ２を取得し、直線Ｃ０−Ｃ１間及び直線Ｃ１−Ｃ２間の距離及び傾きを求め、次に、直線Ｃ１−Ｃ２に垂直で視野中心Ｃ０を通る直線と、直線Ｃ１−Ｃ２との交点Ａを定め、直線Ｃ０−Ａ及び直線Ｃ２−Ａ間の距離を求めることにより、マニピュレータの当該動作軸回りの回転量を定め、定められた各回転量に基づいてマニピュレータを制御し、対象物の画像を撮像手段の視野の中心とすることを特徴とするものである。
【発明の効果】
【００２１】
本発明に係る複数ロボットの制御方法においては、構成１を有することにより、マニピュレータの手先の方向を旋回させたときの対象物の画像の撮像手段における移動量に基づいてマニピュレータの手先の方向を制御して、対象物の画像を撮像手段の視野の中心に位置させ、次に、撮像手段によって得られた対象物の画像の大きさに基づいてマニピュレータの手先と対象物との間の距離を算出し、マニピュレータの手先を撮像手段のレンズの光軸方向に移動させることにより、マニピュレータの手先を対象物に接近させ、次に、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、マニピュレータの手先の方向を、対象物の計測面に垂直な方向とし、次に、撮像手段の視野内における対象物の画像が所定の方向となるように、マニピュレータの手先を回転制御することができる。
【００２２】
本発明に係る複数ロボットの制御方法においては、構成２を有することにより、対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっているので、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報との照合を迅速に行うことができる。
【００２３】
本発明に係る複数ロボットの制御方法においては、構成３を有することにより、対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有しているので、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報との照合を迅速に行うことができる。
【００２６】
すなわち、本発明に係るロボット装置の制御方法においては、３次元空間においてマニピュレータと対象物との位置合わせ及び姿勢合わせを行うことができ、かつ、任意の関節を指定角度だけ動かし、かつ、撮像手段のレンズの光軸方向に動かすなど、一般的な産業用マニュピレータが有する動作を繰り返して位置合わせを行なうので、制御手段の構成を簡素化でき、演算付加を軽減することができる。
【００２７】
そして、本発明に係る複数ロボットにおいては、構成５を有することにより、制御手段は、マニピュレータの手先の方向を旋回させたときの対象物の画像の撮像手段における移動量に基づいてマニピュレータの手先の方向を制御して、対象物の画像を撮像手段の視野の中心に位置させ、次に、撮像手段によって得られた対象物の画像の大きさに基づいてマニピュレータの手先と対象物との間の距離を算出し、マニピュレータの手先を撮像手段のレンズの光軸方向に移動させるので、任意の関節を指定角度だけ動かし、かつ、撮像手段のレンズの光軸方向に動かすなど、一般的な産業用マニュピレータが有する動作を繰り返すことにより、マニピュレータの手先を対象物に接近させ、次に、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、マニピュレータの手先の方向を、対象物の計測面に垂直な方向とし、次に、撮像手段の視野内における対象物の画像が所定の方向となるように、マニピュレータの手先を回転制御することができる。
【００２８】
本発明に係る複数ロボットにおいては、構成６を有することにより、対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっているので、制御手段は、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報との照合を迅速に行うことができる。
【００２９】
本発明に係る複数ロボットにおいては、構成７を有することにより、対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有しているので、制御手段は、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報との照合を迅速に行うことができる。
【００３２】
すなわち、本発明に係るロボット装置においては、３次元空間においてマニピュレータと対象物との位置合わせ及び姿勢合わせを行うことができ、かつ、任意の関節を指定角度だけ動かし、かつ、撮像手段のレンズの光軸方向に動かすなど、一般的な産業用マニュピレータが有する動作を繰り返して位置合わせを行なうので、制御手段の構成を簡素化でき、演算付加を軽減することができる。
【００３３】
すなわち、本発明は、ロボット装置のマニピュレータを制御するにあたり、多くの基準画像を用意する必要がなく、計算負荷が軽く、迅速な制御が可能でありながら、マニピュレータの対象物に対する位置及び姿勢を３次元空間内において制御することができるようになされたロボット装置の制御方法及びロボット装置を提供することができるものである。
【発明を実施するための最良の形態】
【００３４】
以下、本発明を実施するための最良の形態について図面を参照して説明する。
【００３５】
〔ロボット装置の構成〕
図１は、本発明に係るロボット装置の構成を示す模式的な側面図である。
【００３６】
本発明に係るロボット装置は、図１に示すように、マニピュレータ１を有し、このマニピュレータ１により、対象物１０１の把持、運搬及び他の部材への組立が可能となされているものである。
【００３７】
マニピュレータ１は、複数のアクチュエータ（駆動装置）とリンク（剛体の構造物）とによって構成されており、６自由度を有している。すなわち、各リンク間は、回動（屈曲）、または、旋回可能な関節２ａ，２ｂ，２ｃ，２ｄ，２ｅ，２ｆを介して接続されており、それぞれアクチュエータによって相対駆動されるようになっている。各アクチュエータは、制御手段となるコンピュータ３によって制御される。
【００３８】
このマニピュレータ１において、第１のリンク（基端部のリンク）１ａは、基台部４に対し、基端側を第１の関節２ａを介して接続されて設置されている。第１の関節２ａは、鉛直軸（ｚ軸）回りに旋回可能な関節である。この第１のリンク１ａの先端側には、第２の関節２ｂを介して、第２のリンク１ｂの基端側が接続されている。第２の関節２ｂは、水平な軸回りに第２のリンク１ｂを回動可能とする関節である。第２のリンク１ｂの先端側には、第３の関節２ｃを介して、第３のリンク１ｃの基端側が接続されている。第３の関節２ｃは、水平な軸回りに第３のリンク１ｃを回動可能とする関節である。
【００３９】
そして、第３のリンク１ｃの先端側には、第４の関節２ｄを介して、第４のリンク１ｄの基端側が接続されている。第４の関節２ｄは、第３のリンク１ｃの軸回りに第４のリンク１ｄを旋回可能とする関節である。第４のリンク１ｄの先端側には、第５の関節２ｅを介して、第５のリンク１ｅの基端側が接続されている。第５の関節２ｅは、第４のリンク１ｄの軸に直交する軸回りに第５のリンク１ｅを回動可能とする関節である。第５のリンク１ｅの先端側には、第６の関節２ｆを介して、第６のリンク１ｆの基端側が接続されている。第６の関節２ｆは、第５のリンク１ｅの軸回りに第６のリンク１ｆを旋回可能とする関節である。
【００４０】
このように、マニピュレータ１においては、回動可能な関節と、旋回可能な関節とが、交互に計６個設けられていることにより、６自由度が確保されている。
【００４１】
第６のリンク１ｆの先端側（以下、「手先」という。）には、対象物１０１を把持したり加工したりする先端ツール機構５が設けられている。この先端ツール機構５は、コンピュータ３や図示しないその他の制御装置によって制御される。また、手先の近傍には、撮像レンズ及びＣＣＤ（固体撮像素子）からなる撮像手段となるカメラ６が取付けられている。
【００４２】
コンピュータ３は、画像特徴データテーブル３ａを有している。この画像特徴データテーブル３ａには、複数の対象物１０１の形状を示す画像データ及び数値データが記憶されている。また、コンピュータ３は、画像処理部３ｂを有しており、カメラ６から送られる画像信号から対象部品１０１の形状を抽出し、この形状と、画像特徴データテーブル３ａに記憶された画像データ及び数値データとを照合する。そして、コンピュータ３は、動作指令生成部３ｃを有している。この動作指令生成部３ｃは、画像処理部３ｂにおける画像信号と画像特徴データテーブル３ａ内のデータとの照合結果に応じて、マニピュレータ１の各アクチュエータに対する動作指令信号を生成し、各アクチュエータに送る。
【００４３】
そして、マニピュレータ１においては、第１のリンク１ａに対する第２のリンク１ｂの位置、第２のリンク１ｂに対する第３のリンク１ｃの位置、第３のリンク１ｃに対する第４のリンク１ｄの位置と、順次先端側のリンクの位置が制御されることにより、手先の位置が制御され、この手先において先端ツール機構５によって把持したワークを所定の位置に搬送することができる。
【００４４】
そして、このロボット装置においては、以下に示す本発明に係るロボット装置の制御方法が実行されることにより、カメラ６のレンズの光軸に対して対象物１０１の計測面が垂直となるように、マニピュレータ１が自動制御される。なお、この実施の形態においては、対象物１０１が円形の部品であるものとして説明する。
【００４５】
図２は、本発明に係るロボット装置の制御方法の事前準備（ａ）及び制御動作（ｂ）を示すフローチャートである。
【００４６】
マニピュレータ１の自動制御を行うための事前準備として、図２中の（ａ）に示すように、Ｓ１において事前準備を開始すると、Ｓ２において、制御対象となるロボット装置のマニピュレータ１に固有のパラメータを算出し、コンピュータ３の記憶装置にテーブルとして記憶しておく。
【００４７】
すなわち、カメラ６の撮像素子の縦横の有効素子数（Ｘｐｘ，Ｙｐｘ）（解像度（ピクセル単位））、有効画素の縦横の大きさ（Ｘｌｎ，Ｙｌｎ）（物理長さ）に基づき、撮像素子におけるピクセルと物理量長さとの変換係数を、以下のように算定しておく。
（Ｋｘｌｎ，Ｋｙｌｎ）＝（Ｘｌｎ／Ｘｐｘ，Ｙｌｎ／Ｙｐｘ）
【００４８】
図４は、カメラ６をピンホールカメラに模して表現した構成を示す斜視図である。
【００４９】
また、図４に示すように、カメラ６の撮像レンズ６ａから撮像素子６ｂまでの距離Ｄｐｌ（物理長さ）と、撮像レンズ６ａから対象物１０１までの距離Ｌとに基づき、撮像画像における長さＰＳと実際の物理長さＲＳとの関係を算定しておく。なお、撮像レンズ６ａから撮像素子６ｂまでの距離Ｄｐｌと、撮像レンズ６ａから対象物１０１までの距離Ｌとの関係は、撮像レンズ６ａの焦点距離に基づいて算出することができる。したがって、撮像レンズ６ａから撮像素子６ｂまでの距離Ｄｐｌと撮像レンズ６ａの焦点距離とに基づいて、撮像画像における長さＰＳと実際の物理長さＲＳとの関係を算定することができる。
【００５０】
さらに、ロボット装置に固有のパラメータとしては、手先座標系からカメラ座標系への変換行列Ｔも算定しておく。
【００５１】
次に、Ｓ３において、対象物１０１に固有のパラメータを算出し、コンピュータ３の画像特徴データテーブル３ａに記憶しておく。
【００５２】
すなわち、目標とする対象物１０１の画像特徴量として、カメラ６により対象物１０１を撮像したときの対象物１０１の形状の特徴量を記憶しておく。具体的には、対象物１０１の画像上の目標位置（Ｐｘ，Ｐｙ）（中心位置や重心位置など）をピクセル単位で記憶し、また、方向を示す姿勢θ（degree、あるいは、rad単位）、距離の判定に用いる大きさＰＳ（ピクセル単位）などをテーブルとして記憶しておく。例えば、撮像された対象物１０１の形状が楕円である場合には、画像上の楕円の中心位置（Ｐｘ、Ｐｙ）、ＰＳ（長径、または、短径）、θ（長径の向き角度）などを記憶しておく。
【００５３】
対象物１０１は、画像上において円に近似させ、例えば、円で覆い、その中心位置と半径とを求める。なお、対象物１０１の面積（上面の面積）を算出して記憶しておいてもよい。
【００５４】
そして、Ｓ４において事前準備を終了する。事前準備が終了していれば、図２中の（ｂ）に示すように、制御動作を実行することができる。すなわち、Ｓ５において制御動作を開始すると、Ｓ６においてセンタリングを実行する。
【００５５】
図３は、本発明に係るロボット装置の制御方法を実行している状態における各段階に応じた撮像画像を示す正面図である。
【００５６】
センタリングは、図３中のフェーズ１に示すように、対象物１０１の画像が、カメラ６により撮像された画像の中心にくるように、マニピュレータ１を動作させる制御である。
【００５７】
図５は、対象物１０１のセンタリングを行っている状態における撮像画像を示す正面図である。
【００５８】
このセンタリングについては、図５に示すように、カメラ６により撮像された画像の中心位置をＣ０（Ｘ０，Ｙ０）とし、この画像上における対象物１０１の初期位置をＣ１（Ｐｘ１，Ｐｙ１）として説明する。
【００５９】
図６は、センタリング動作の手順を示すフローチャートである。
【００６０】
このセンタリングは、以下の手順により行う。すなわち、図６に示すように、Ｓ１１でセンタリングを開始すると、Ｓ１２において、カメラ６により撮像された画像の中心位置をＣ０、画像上における対象物１０１の初期位置Ｃ１を取得したうえで、Ｓ１３に進む。Ｓ１３において、第４の関節２ｄを選択し、Ｓ１４において、適当な値ｑｔ４だけ駆動させる。駆動量は、任意の角度及び向きでよいが、例えば、３deg乃至５deg程度とすることが好ましい。
【００６１】
このとき、図５に示すように、カメラ６により再度撮像を行い、このときに取得された画像における対象物１０１の位置をＣ２（Ｐｘ２，Ｐｙ２）とし、図６中のＳ１４に示すように、位置Ｃ２を取得して記憶する。
【００６２】
そして、Ｓ１５に進み、図５に示すように、画像上における対象物１０１の位置Ｃ０と位置Ｃ２の距離ｄ、位置Ｃ１と位置Ｃ２の距離ｄ１２を求める。これらは、それぞれの点の位置（画像上における座標）がわかっているので、以下の式により求めることができる。
ｄ＝√｛（Ｘ０−Ｐｘ２）２＋（Ｙ０−Ｐｙ２）２｝
ｄ１２＝√｛（Ｐｘ１−Ｐｘ２）２＋（Ｐｙ１−Ｐｙ２）２｝
【００６３】
次に、図６のＳ１６に進み、直線Ｃ０〜Ｃ１の傾きｋ１と、直線Ｃ０〜Ｃ２の傾きｋ２と、直線Ｃ１〜Ｃ２の傾きｋ１２とを求める。これらも、それぞれの点の位置（画像上における座標）がわかっているので、以下の式により求めることができる。
ｋ１＝（Ｐｙ１−Ｐｙ０）／（Ｐｘ１−Ｐｘ０）
ｋ２＝（Ｐｙ２−Ｐｙ０）／（Ｐｘ２−Ｐｘ０）
ｋ１２＝（Ｐｙ１−Ｐｙ２）／（Ｐｘ１−Ｐｘ２）
【００６４】
ここで、図５に示すように、直線Ｃ１〜Ｃ２に垂直で位置Ｃ０を通る直線と、直線Ｃ１〜Ｃ２との交点を点Ａとする。図６のＳ１７に進み、点Ａと位置Ｃ０との距離ｄ０Ａを求める。これは、直線Ｃ１〜Ｃ２と点Ｃ０の距離であるので、以下の式により求めることができる。
ｄ０Ａ＝｜ｋ１２・Ｘ０−Ｙ０＋Ｐｙ１−ｋ１２・Ｐｘ１｜／√（ｋ１２２＋１）
【００６５】
次に、Ｓ１８に進み、位置Ｃ１から点Ａまでの距離ｄ２Ａを求める。この距離ｄ２Ａは、直角三角形Ｃ０−Ａ−Ｃ２の辺の長さの関係より、以下のようして求められる。
ｄ２Ａ＝√（ｄ１２２−ｄ０Ａ２）
【００６６】
Ｓ１９に進み、第４の関節２ｄを所定値ｑ４回転させる。この所定値ｑ４の大きさは、以下の式により得る。
ｑ４＝ｄ２Ａ・ｑ４ｔ／ｄ１２
【００６７】
すなわち、第４の関節２ｄをｑ４ｔ回転させて画像上の位置がｄ１２動いたので、ｄ２Ａだけ動かすための回転角度ｑ４はいくらかを求める。
【００６８】
所定値ｑ４の符号は、以下のルールにより決める。すなわち、（−１／ｋ１２−ｋ１）と（ｋ２−ｋ１）とが同符号ならば、ｑ４ｔと同じ方向とし、（−１／ｋ１２−ｋ１）と（ｋ２−ｋ１）とが異符号ならば、ｑ４ｔと逆の方向とする。
【００６９】
次に、Ｓ２０に進み、第５の関節２ｅについて、Ｓ１２からＳ１９までと同様の制御を行う。そして、カメラ６により撮像される画像における中心位置Ｃ０と、対象物１０１の位置（Ｐｘ，Ｐｙ）との差が一定以下の距離になれば、Ｓ２１に進み、センタリング動作を終了する。画像の中心位置Ｃ０と対象物１０１の位置（Ｐｘ，Ｐｙ）との差が一定以下になっていない場合には、Ｓ１２からＳ２０までの動作を再び行う。
【００７０】
前述のようにしてセンタリングが終了した場合には、次に、図２中の（ｂ）においてＳ７に進み、カメラ６を対象物１０１に接近させる動作を行う。
【００７１】
センタリングの終了により、図３中のフェーズ２に示すように、カメラ６のレンズの光軸方向に手先を移動させても、取得される画像上における対象物１０１の位置は、画像の中心位置に停止したままとなる。この状態で、カメラ６のレンズの光軸方向に、手先を移動させて対象物１０１に接近させる。
【００７２】
手先をカメラ６のレンズの光軸方向に移動させるためには、マニピュレータ１とカメラ６のレンズの光軸との位置関係が既知である必要がある。図１に示すように、光軸座標系はＴｃ、手先座標系はＴ６であり、これを変換する行列Ｔを事前に求めておけば、Ｔ・Ｔ６＝Ｔｃにより、光軸座標系Ｔｃを求めることができ、カメラ６のレンズの光軸方向（ｚｃ軸方向）に手先を移動させればよい。
【００７３】
ｚｃ軸方向ヘの移動量Ｌは、図４に示した撮像画像における長さＰＳと実際の物理長さＲＳとの関係により、以下のように求めることができる。
Ｌ＝Ｄｐｌ／（Ｋｘｐｌ・ＰＳ／cos（θ１））・ＲＳ
【００７４】
ただし、これは、ＫｘｐｌとＫｙｐｌとが等しい値である場合である。ＫｘｐｌとＫｙｐｌとが同じ値でない場合には、ＰＳのｘ軸成分とｙ軸成分を取り出し、これらをＰＳｘとＰＳｙとして、以下のようにして求める。
Ｌ＝Ｄｐｌ／〔√｛（Ｋｘｐｌ・ＰＳｘ）２＋（Ｋｙｐｌ・ＰＳｙ）２｝／cos（θ１）〕・ＲＳ
【００７５】
なお、θ１は、鉛直軸に対して手先軸ｚ６がどの程度傾いているかを示す角度であり、以下の式により得られる。
θ１＝acos（（ｚ６×ｚｃ）／（｜ｚ６｜・｜ｚｃ｜））
【００７６】
ここで、（ｚ６×ｚｃ）は、２つのベクトルｚ６とｚｃとの内積であり、｜ｚ６｜、｜ｚｃ｜は、ベクトルｚ６，ｚｃのノルム（長さ）である。acos（ａ）は、cos（θ）＝ａなるθであり、ただし、〔０≦θ≦１８０deg〕である。
【００７７】
これは、斜めから対象物１０１を見ることにより、見かけ上の大きさが小さく見えることを補正する操作である。なお、対象物１０１の形状が円であったり、または、円近似している場合には、最も長い径を求めれば、斜めから見ることによる影響を減らすことができるので、このようなcos（θ１）による補正を行う必要はない。
【００７８】
このようにして求められた距離Ｌに基づいて、カメラ６及び手先を、カメラ６のレンズの光軸ｚｃ方向に、対象物１０１に向けて移動させる。
【００７９】
次に、図２中の（ｂ）においてＳ８に進み、姿勢合わせを行う。この姿勢合わせは、図３中のフェーズ３に示すように、カメラ６のレンズの光軸を対象物１０１の計測面（上面）に垂直となるように手先の姿勢を制御する動作である。
【００８０】
例えば、計測面が床面と平行で、マニピュレータ１も計測面に平行な面上に置かれている場合において、手先軸ｚ６とカメラのレンズの光軸ｚｃとが平行である場合には、手先軸ｚ６が床面に垂直となるように、すなわち、カメラ６が真下を向くように、マニピュレータ１の手先の姿勢を制御する。
【００８１】
そして、図２中の（ｂ）においてＳ９に進み、２次元位置制御及び位相合わせを行う。
【００８２】
図７は、２次元位置制御及び位相合わせの手順を示すフローチャートである。
【００８３】
前述のように、姿勢合わせを行ったことにより、図３中のフェーズ４に示すように、対象物１０１の画像は、カメラ６により取得される画像の中心からずれている。しかし、計測面に対して、カメラ６のレンズの光軸は垂直になっている。したがって、この状態では、２次元画像フィードバックにより、位置及び位相を補正できる。ここで、位相とは、カメラ６の視野内における対象物１０１の画像の方向のことであり、マニピュレータの手先をカメラ６のレンズの光軸回りに回転させることによって補正される。
【００８４】
すなわち、図７のＳ２２において、２次元位置制御及び位相合わせ制御を開始すると、Ｓ２３に進み、画像上の対象物１０１の大きさＰＳと、画像処理により求めた大きさＰＳ１とを比較する。画像処理により求めた大きさＰＳ１のほうが小さければ、手先が対象物１０１に近づきすぎなので、対象物１０１から遠ざかる方向に手先を動作させ、ＰＳ１のほうが大きければ、手先が対象物１０１から遠すぎるので、対象物１０１に近づく方向に手先を動作させる。
【００８５】
Ｓ２４では、大きさＰＳと大きさＰＳ１との偏差が所定の値以下となっているかを判別し、所定の値以下となっていなければ、Ｓ２３に戻り、手先の移動操作を繰り返す。大きさＰＳと大きさＰＳ１との偏差が所定の値以下となっていれば、Ｓ２５に進む。
【００８６】
Ｓ２５では、画像処理により対象物１０１の位置（Ｐｘ，Ｐｙ）を求め、以下の式により、手先の移動量を決定する。
〔カメラのｘ軸方向の手先移動量〕＝Ｋｘｐｌ・（Ｐｘ−Ｘ０）
〔カメラのｙ軸方向の手先移動量〕＝Ｋｙｐｌ・（Ｐｙ−Ｙ０）
【００８７】
なお、実際の移動方向は、画像上における軸の向きと、手先の方向との関係から求める。そして、Ｓ２６に進み、中心位置の偏差（Ｐｘ−Ｘ０，Ｐｙ−Ｙ０）が所定の値以下になっているかを判別し、所定の値以下となっていなければ、Ｓ２５に戻り、手先の移動操作を繰り返す。中心位置の偏差（Ｐｘ−Ｘ０，Ｐｙ−Ｙ０）が所定の値以下となっていれば、Ｓ２７に進む。
【００８８】
Ｓ２７では、対象物１０１の位相θ１が目的の角度θとなるように、θ−θ１だけ第６の関節２ｆを回転操作する。そして、Ｓ２８に進み、２次元位置制御及び位相合わせ制御が完了する。すなわち、図２中の（ｂ）においてＳ１０に進み、制御動作が完了する。
【００８９】
ところで、前述の実施の形態においては、対象物１０１の形状を円形として説明している。しかし、対象物１０１の形状は、円形に限定されるわけではなく、対象物１０１の表面上に何らかの円形の特徴部分があり、画像処理により円形の部分が抽出できればよいのであり、例えば、外形は多角形だが、その中に円形の孔が空いているなどの形状でもよい。
【図面の簡単な説明】
【００９０】
【図１】本発明に係るロボット装置の構成を示す模式的な側面図である。
【図２】本発明に係るロボット装置の制御方法の事前準備（ａ）及び制御動作（ｂ）を示すフローチャートである。
【図３】本発明に係るロボット装置の制御方法を実行している状態における各段階に応じた撮像画像を示す正面図である。
【図４】本発明に係るロボット装置におけるカメラをピンホールカメラに模して表現した構成を示す斜視図である。
【図５】本発明に係るロボット装置の制御方法においてセンタリングを行っている状態における撮像画像を示す正面図である。
【図６】本発明に係るロボット装置の制御方法におけるセンタリング動作の手順を示すフローチャートである。
【図７】本発明に係るロボット装置の制御方法における２次元位置制御及び位相合わせの手順を示すフローチャートである。
【符号の説明】
【００９１】
１    マニピュレータ
１ａ  第１のリンク
１ｂ  第２のリンク
１ｃ  第３のリンク
１ｄ  第４のリンク
１ｅ  第５のリンク
１ｆ  第６のリンク
２ａ  第１の関節
２ｂ  第２の関節
２ｃ  第３の関節
２ｄ  第４の関節
２ｅ  第５の関節
２ｆ  第６の関節
３    コンピュータ
５    先端ツール機構
６    カメラ
１０１    対象物
【図１】
0005292998.tif 000002
【図２】
0005292998.tif 000003
【図３】
0005292998.tif 000004
【図４】
0005292998.tif 000005
【図５】
0005292998.tif 000006
【図６】
0005292998.tif 000007
【図７】
0005292998.tif 000008