(57)【要約】【課題】対象物の移動経路の周辺環境の影響を受けることなく、対象物を目標位置に安定して移動すること。【解決手段】画像照合用マーカ（Ｍ）が設けられたハンド部（２４）を有し、ハンド部が把持した対象物（Ｐ）を視覚サーボ法により目標位置まで移動するロボット（２）を制御するロボット制御システム（１）において、対象物を把持した状態のハンド部を計測して三次元情報を取得しておき、この三次元情報に含まれる画像照合用マーカの三次元座標値に基づいて対象物の照合点を推定してハンド部の移動制御を行う構成とした。【選択図】図１
2017094482.tif 000002
【特許請求の範囲】
【請求項１】
画像照合用マーカが設けられたハンドを有し、前記ハンドが把持した対象物を目標位置まで移動するロボットを制御するロボット制御システムであって、
前記対象物を把持した状態の前記ハンドを計測して三次元情報を取得する三次元情報取得部と、
前記三次元情報に含まれる前記画像照合用マーカの三次元座標値に基づいて前記対象物の照合点を推定する推定部と、
前記推定部で推定した前記対象物の照合点が前記目標位置に一致するように前記ハンドの移動制御を行う制御部と、を具備することを特徴とするロボット制御システム。
【請求項２】
前記対象物を把持した状態の前記ハンドを撮像する撮像部を更に具備し、
前記推定部は、前記三次元情報に含まれる前記画像照合用マーカの三次元座標値から前記画像照合用マーカを基準とした三次元座標を生成する三次元画像処理部と、前記画像照合用マーカを基準とした三次元座標と前記撮像部で撮像された前記画像照合用マーカの二次元座標値とに基づいて前記対象物の照合点の二次元座標値を推定する二次元画像処理部と、を有することを特徴とする請求項１に記載のロボット制御システム。
【請求項３】
前記三次元画像処理部は、前記画像照合用マーカを基準とした三次元座標における前記対象物の三次元座標値群と予め保持された前記対象物の三次元辞書座標値群とに基づいて、前記画像照合用マーカを基準とした三次元座標における前記対象物の位置及び姿勢を特定し、前記画像照合用マーカを基準とした三次元座標における前記対象物の照合点を推定することを特徴とする請求項２に記載のロボット制御システム。
【請求項４】
前記三次元画像処理部は、ＩＣＰ(Iterative  Closest  Point)アルゴリズムにより前記画像照合用マーカを基準とした三次元座標における前記対象物の位置及び姿勢を特定することを特徴とする請求項３に記載のロボット制御システム。
【請求項５】
前記二次元画像処理部は、前記画像照合用マーカの二次元座標値と前記画像照合用マーカを基準とした三次元座標における前記画像照合用マーカの三次元座標値とから、前記画像照合用マーカを基準とした三次元座標から前記撮像部による撮像画像上の二次元座標に射影する射影変換行列を算出し、当該射影変換行列を用いて前記画像照合用マーカを基準とした三次元座標における前記対象物の照合点の三次元座標値から前記対象物の照合点の二次元座標値を推定することを特徴とする請求項２から請求項４のいずれかに記載のロボット制御システム。
【請求項６】
前記撮像部は、前記目標位置近傍にて前記対象物を把持した状態の前記ハンドを撮像し、
前記二次元画像処理部は、前記対象物の全ての照合点が撮像されない場合に前記射影変換行列を算出することを特徴とする請求項５に記載のロボット制御システム。
【請求項７】
前記画像照合用マーカは、前記ハンドに対して少なくとも３つ設けられることを特徴とする請求項１から請求項６のいずれかに記載のロボット制御システム。
【請求項８】
前記画像照合用マーカは、前記三次元座標の原点及び軸方向が特定される位置に配置されることを特徴とする請求項７に記載のロボット制御システム。
【請求項９】
前記画像照合用マーカは、白色領域と黒色領域とを有し、当該画像照合用マーカの中心位置が特定される構成を有することを特徴とする請求項７又は請求項８に記載のロボット制御システム。
【請求項１０】
画像照合用マーカが設けられたハンドを有し、前記ハンドが把持した対象物を目標位置まで移動するロボットを制御するロボット制御方法であって、
前記対象物を把持した状態の前記ハンドを計測して三次元情報を取得するステップと、
前記三次元情報に含まれる前記画像照合用マーカの三次元座標値に基づいて前記対象物の照合点を推定するステップと、
推定した前記対象物の照合点が前記目標位置に一致するように前記ハンドの移動制御を行うステップと、を具備することを特徴とするロボット制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御システム及びロボット制御方法に関し、特に、撮像装置等で撮像した画像情報に基づいて対象物を目標位置まで移動させるロボット制御システム及びロボット制御方法に関する。
【背景技術】
【０００２】
撮像装置等で撮像した画像情報に基づいて、ロボットが把持した対象物を目標位置まで移動させるロボット制御技術として視覚サーボ法が知られている。この視覚サーボ法は、撮像された画像情報に基づいて、対象物に予め設定された照合点が画像上に与えられた目標点と一致するように、視覚フィードバックによりロボットを制御する。
【０００３】
このような視覚サーボ法では、対象物上の照合点の三次元位置や、撮像装置又はロボットの運動制御パラメータに誤差があった場合でも、目標位置近傍で収束させることができ、動作安定性の面で優れている。一方、対象物が目標位置近傍に到達するまでは、安定性を欠く場合があり、目標位置まで適切に対象物を移動させることができない場合も想定される。
【０００４】
従来、このような視覚サーボ法による動作不安定性を回避するためのロボット制御方法（マニピュレータ制御方法）が提案されている（例えば、特許文献１参照）。特許文献１に記載のロボット制御方法においては、視覚サーボ法による安定動作領域に到達するまではステレオカメラを用いて対象物と目標位置との三次元の位置関係を計算して対象物の移動制御を行う。そして、対象物が目標位置近傍に到達した段階で視覚サーボ法による対象物の移動制御を行っている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平９−２９０３８３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上述した視覚サーボ法においては、予め設定された対象物の照合点を抽出し、抽出した照合点を用いた視覚フィードバックによりロボットを制御する必要がある。このような対象物の照合点は、対象物の移動経路の周辺環境に応じて適切に抽出することができない場合がある。対象物の照合点を適切に抽出できない場合には、対象物を目標位置に安定して移動することができない事態が発生し得る。
【０００７】
本発明は、このような実情に鑑みてなされたものであり、対象物の移動経路の周辺環境の影響を受けることなく、対象物を目標位置に安定して移動することができるロボット制御システム及びロボット制御方法を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明におけるロボット制御システムは、画像照合用マーカが設けられたハンドを有し、前記ハンドが把持した対象物を目標位置まで移動するロボットを制御するロボット制御システムであって、前記対象物を把持した状態の前記ハンドを計測して三次元情報を取得する三次元情報取得部と、前記三次元情報に含まれる前記画像照合用マーカの三次元座標値に基づいて前記対象物の照合点を推定する推定部と、前記推定部で推定した前記対象物の照合点が前記目標位置に一致するように前記ハンドの移動制御を行う制御部と、を具備することを特徴とする。
【０００９】
例えば、上記ロボット制御システムにおいて、前記対象物を把持した状態の前記ハンドを撮像する撮像部を更に具備し、前記推定部は、前記三次元情報に含まれる前記画像照合用マーカの三次元座標値から前記画像照合用マーカを基準とした三次元座標を生成する三次元画像処理部と、前記画像照合用マーカを基準とした三次元座標と前記撮像部で撮像された前記画像照合用マーカの二次元座標値とに基づいて前記対象物の照合点の二次元座標値を推定する二次元画像処理部と、を有する。
【００１０】
また、上記ロボット制御システムにおいて、前記三次元画像処理部は、前記画像照合用マーカを基準とした三次元座標における前記対象物の三次元座標値群と予め保持された前記対象物の三次元辞書座標値群とに基づいて、前記画像照合用マーカを基準とした三次元座標における前記対象物の位置及び姿勢を特定し、前記画像照合用マーカを基準とした三次元座標における前記対象物の照合点を推定する。
【００１１】
特に、上記ロボット制御システムにおいて、前記三次元画像処理部は、ＩＣＰ(Iterative  Closest  Point)アルゴリズムにより前記画像照合用マーカを基準とした三次元座標における前記対象物の位置及び姿勢を特定することが好ましい。
【００１２】
さらに、上記ロボット制御システムにおいて、前記二次元画像処理部は、前記画像照合用マーカの二次元座標値と前記画像照合用マーカを基準とした三次元座標における前記画像照合用マーカの三次元座標値とから、前記画像照合用マーカを基準とした三次元座標から前記撮像部による撮像画像上の二次元座標に射影する射影変換行列を算出し、当該射影変換行列を用いて前記画像照合用マーカを基準とした三次元座標における前記対象物の照合点の三次元座標値から前記対象物の照合点の二次元座標値を推定することが好ましい。
【００１３】
例えば、上記ロボット制御システムにおいて、前記撮像部は、前記目標位置近傍にて前記対象物を把持した状態の前記ハンドを撮像し、前記二次元画像処理部は、前記対象物の全ての照合点が撮像されない場合に前記射影変換行列を算出することが好ましい。
【００１４】
例えば、上記ロボット制御システムにおいて、前記画像照合用マーカは、前記ハンドに対して少なくとも３つ設けられる。
【００１５】
特に、上記ロボット制御システムにおいて、前記画像照合用マーカは、前記三次元座標の原点及び軸方向が特定される位置に配置されることが好ましい。
【００１６】
また、上記ロボット制御システムにおいて、前記画像照合用マーカは、白色領域と黒色領域とを有し、当該画像照合用マーカの中心位置が特定される構成を有することが好ましい。
【００１７】
本発明におけるロボット制御方法は、画像照合用マーカが設けられたハンドを有し、前記ハンドが把持した対象物を目標位置まで移動するロボットを制御するロボット制御方法であって、前記対象物を把持した状態の前記ハンドを計測して三次元情報を取得するステップと、前記三次元情報に含まれる前記画像照合用マーカの三次元座標値に基づいて前記対象物の照合点を推定するステップと、推定した前記対象物の照合点が前記目標位置に一致するように前記ハンドの移動制御を行うステップと、を具備することを特徴とする。
【発明の効果】
【００１８】
本発明によれば、対象物の移動経路の周辺環境の影響を受けることなく、対象物を目標位置に安定して移動することができる。
【図面の簡単な説明】
【００１９】
【図１】第１の実施の形態に係るロボット制御システムの概略構成を示す模式図である。
【図２】第１の実施の形態に係るロボット制御システムにおける画像照合用マーカの一例を示す図である。
【図３】第１の実施の形態に係るロボット制御システムにおける全体動作を説明するためのフロー図である。
【図４】第１の実施の形態に係るロボット制御システムにおける三次元情報解析処理を説明するためのフロー図である。
【図５】第１の実施の形態に係るロボット制御システムにおける三次元情報解析処理で得られる三次元座標値の説明図である。
【図６】第１の実施の形態に係るロボット制御システムにおける三次元情報解析処理で得られる三次元座標値の説明図である。
【図７】第１の実施の形態に係るロボット制御システムにおけるロボット制御処理を説明するためのフロー図である。
【図８】第１の実施の形態に係るロボット制御システムにおけるロボット制御処理により移動するハンド部の説明図である。
【図９】第１の実施の形態に係るロボット制御システムにおけるロボット制御処理により移動するハンド部の説明図である。
【図１０】第２の実施の形態に係るロボット制御システムにおける三次元情報解析処理を説明するためのフロー図である。
【図１１】第２の実施の形態に係るロボット制御システムにおけるロボット制御処理を説明するためのフロー図である。
【発明を実施するための形態】
【００２０】
以下、本発明の複数の実施の形態について図面を参照しながら説明する。なお、以下においては、本発明に係るロボット制御システムが制御する対象のロボットの一例として、多関節型ロボットを例に説明するが、制御対象となるロボットについては、これに限定されるものではなく適宜変更が可能である。制御対象となるロボットについては、対象物を把持するロボットハンドを備えることを条件として、人型ロボットなどの任意のロボットに適用することができる。
【００２１】
（第１の実施の形態）
図１は、第１の実施の形態に係るロボット制御システムの概略構成を示す模式図である。図１に示すように、本実施の形態に係るロボット制御システム１は、制御対象となる多関節型ロボット（以下、単に「ロボット」という）２を制御する制御装置３と、制御装置３に接続された三次元情報取得部としての３次元スキャナ（以下、「３Ｄスキャナ」という）４と、制御装置３に接続された撮像部としてのカメラ５とを含んで構成されている。なお、図１に示すロボット制御システム１の構成は一例を示すものであり、これに限定されるものではない。例えば、ここでは、ロボット制御システム１が３Ｄスキャナ４を備える場合について説明するが、後述する対象物Ｐを把持した状態のハンド部２４を三次元計測できることを条件として、３Ｄスキャナ４の代わりにステレオカメラ等を備えてもよい。
【００２２】
ロボット２は、例えば、製造工場に構築された製造ラインの近傍に設置される。ロボット２は、所定位置に配置されたパレット等から対象物Ｐを取り出し、視覚サーボ法により予め定められた目標位置まで移動する。なお、図１においては、対象物Ｐをピックアップする位置に配置された状態のロボット２を実線で示し、対象物Ｐが目標位置に移動させた状態のロボット２を破線で示している。また、図１においては、ロボット２により対象物Ｐが把持された状態について示している。
【００２３】
ロボット２により移動される対象物Ｐとしては、例えば、製造過程の製品に組み付けられる部品等が考えられる。対象物Ｐには、視覚サーボ法による駆動制御に利用される複数（本実施の形態では４つ）の照合点Ｃが予め設定されている。このような対象物Ｐの照合点Ｃは、後述する制御装置３の記憶部３２において、対象物Ｐの種別毎に関連づけて記憶されている。なお、これらの照合点Ｃの詳細については後述する。
【００２４】
ロボット２は、製造工場の床面に設置されるベース部２１と、このベース部２１に対して鉛直方向を回転軸として回転可能に設けられる本体部２２と、この本体部２２の上端に揺動可能に接続されたアーム部２３と、このアーム部２３の先端に揺動可能に接続されたハンドとしてのハンド部２４とを有する。ベース部２１に対して回転する本体部２２の上端でアーム部２３が揺動すると共に、アーム部２３の先端でハンド部２４が揺動する。これにより、アーム部２３及びハンド部２４の駆動領域内において、ハンド部２４が任意の位置に移動可能に構成されている。
【００２５】
ハンド部２４の先端には、対向して配置された一対の把持部２５が設けられている。これらの把持部２５は、ハンド部２４の先端部から延出して設けられている。これらの把持部２５は、互いに接近及び離間可能に構成され、その先端部分で対象物Ｐを把持できるように構成されている。また、ハンド部２４（把持部２５を含む）の所定位置には、複数の画像照合用マーカＭが設けられている。なお、これらの画像照合用マーカＭの詳細については後述する。
【００２６】
制御装置３は、３Ｄスキャナ４及びカメラ５を介してロボット２の駆動制御に必要な情報を取得し、取得した情報に基づいてロボット２の駆動を制御する。制御装置３は、画像処理部３１、記憶部３２及び駆動制御部３３を含んで構成される。画像処理部３１は、３Ｄスキャナ４で取得される三次元情報及びカメラ５で撮像される画像情報に対して画像処理を行う。記憶部３２は、ロボット２の駆動制御に必要な情報（例えば、対象物Ｐの照合点Ｃに関する情報等）や画像処理部３１による画像処理に必要となる情報を記憶する。駆動制御部３３は、記憶部３２に記憶された情報及び画像処理部３１による画像処理結果に応じてロボット２の駆動を制御する。
【００２７】
画像処理部３１は、推定部の一例を構成するものであり、三次元画像処理部（以下、「３Ｄ処理部」という）３１１と、二次元画像処理部（以下、「２Ｄ処理部」という）３１２とを有する。３Ｄ処理部３１１は、３Ｄスキャナ４による計測を制御すると共に、３Ｄスキャナ４が取得した三次元情報に対して画像処理を行う。２Ｄ処理部３１２は、カメラ５による撮像を制御すると共に、カメラ５が撮像した画像情報に対して画像処理を行う。なお、３Ｄ処理部３１１及び２Ｄ処理部３１２における画像処理については後述する。
【００２８】
３Ｄスキャナ４は、例えば、３Ｄレーザスキャナで構成される。３Ｄスキャナ４は、計測対象に対して計測光（レーザ光）を照射する一方、計測対象から反射する反射光を受光して計測対象の三次元情報（３Ｄモデル）を取得する。３Ｄスキャナ４は、ロボット２のハンド部２４が対象物Ｐをピックアップする位置（ピックアップ位置）に対応して配置される。３Ｄスキャナ４は、把持部２５で対象物Ｐを把持した状態のハンド部２４に計測光を照射し、ハンド部２４（把持部２５を含む）及び対象物Ｐの三次元情報（３Ｄモデル）を取得する。なお、３Ｄスキャナ４の構成については、レーザ光タイプに限定されるものではなくパターン光タイプなど他の方式を適用してもよい。
【００２９】
カメラ５は、例えば、ＣＣＤ（Charge Coupled Device）カメラで構成される。カメラ５は、ロボット２が対象物Ｐを移動する目標位置近傍を撮像可能な位置に配置される。カメラ５は、目標位置近傍において、把持部２５で対象物Ｐを把持した状態のハンド部２４を撮像する。なお、カメラ５は、ハンド部２４に設けられた画像照合用マーカＭや対象物Ｐの照合点Ｃを撮像できることを条件として、任意の撮像素子を適用することができる。例えば、目標位置近傍の一定領域を広角に撮像可能な固定焦点レンズを備えてもよい。
【００３０】
このような構成を有し、ロボット制御システム１は、対象物Ｐを移動する目標位置近傍において、対象物Ｐを把持した状態のハンド部２４をカメラ５で撮像し、対象物Ｐの照合点Ｃが予め定められる目標位置と一致するように、視覚フィードバックによりロボット２を制御する（視覚サーボ法）。
【００３１】
ところで、このような視覚サーボ法においては、対象物Ｐの照合点Ｃを抽出し、抽出した照合点Ｃを用いた視覚フィードバックによりロボット２を制御する。このような対象物Ｐの照合点Ｃは、対象物Ｐの移動経路の周辺環境に応じて適切に抽出することができない場合がある。対象物Ｐの照合点Ｃを適切に抽出できない場合には、対象物Ｐを目標位置に安定して移動することが困難となる。この結果、対象物Ｐを製品に組み付けるための作業効率が低下する事態が発生し得る。例えば、対象物Ｐの照合点Ｃを適切に抽出できない場合としては、対象物Ｐの照合点Ｃが遮蔽などにより特定できない場合や、照明環境の不具合などにより対象物Ｐの照合点Ｃの特定に失敗する場合などが想定される。
【００３２】
本発明者は、このような視覚サーボ法の実情に着目し、対象物Ｐの照合点Ｃのみに依存するのではなく、対象物Ｐの照合点Ｃとは異なる基準により照合点Ｃを推定して対象物Ｐの移動を制御することが視覚サーボ法の動作安定性を向上できることを見出し、本発明に想到した。
【００３３】
すなわち、本発明の骨子は、画像照合用マーカＭが設けられたハンド部２４及びハンド部２４が把持した対象物Ｐの三次元情報を予め取得しておき、上記三次元情報に含まれる画像照合用マーカＭの三次元座標値に基づいて対象物Ｐの照合点Ｃを推定して対象物Ｐの移動制御を行うことである。
【００３４】
これにより、対象物Ｐの照合点Ｃが遮蔽などにより特定されない場合や、照明環境の不具合などにより対象物Ｐの照合点Ｃの特定に失敗する場合であっても、予め取得されたハンド部２４及び対象物Ｐの三次元情報に基づいて対象物Ｐの照合点Ｃを推定することができる。このため、対象物Ｐの移動経路の周辺環境の影響を受けることなく、対象物Ｐを目標位置に安定して移動することができる。
【００３５】
適切に照合点Ｃが抽出されない場合に対象物Ｐの照合点Ｃとは異なる基準の生成に利用するため、本実施の形態に係るロボット制御システム１では、ロボット２のハンド部２４に複数（本実施の形態では４つ）の画像照合用マーカＭを設けている。以下、本実施の形態に係るロボット制御システム１における画像照合用マーカＭについて説明する。
【００３６】
図２は、第１の実施の形態に係るロボット制御システム１における画像照合用マーカＭの一例を示す図である。なお、図２に示す画像照合用マーカＭの数及び配置は一例を示すものであり、これに限定されるものではなく適宜変更が可能である。画像照合用マーカＭは、ハンド部２４の同一面に少なくとも３個以上存在すればよい。
【００３７】
図２に示すように、ロボット制御システム１においては、ロボット２のハンド部２４に４つの画像照合用マーカＭ１〜Ｍ４が設けられている。画像照合用マーカＭ１、Ｍ２は、ハンド部２４の下端部近傍において、ハンド部２４の幅方向に離間して配置されている。画像照合用マーカＭ３、Ｍ４は、ハンド部２４の把持部２５の延出方向に離間して配置されている。特に、画像照合用マーカＭ３は、把持部２５の基端部近傍に設けられ、画像照合用マーカＭ４は、把持部２５の先端部近傍に設けられている。
【００３８】
画像照合用マーカＭ１〜Ｍ４は、同一の構成を有する。このため、画像照合用マーカＭ１を代表して説明し、その他の画像照合用マーカＭ２〜Ｍ４の説明を省略する。図２に示すように、画像照合用マーカＭ１は、正面視にて真円形状を有している。画像照合用マーカＭ１は、同一面積を有する４つの扇型形状の領域に分割されている。画像照合用マーカＭ１を構成する４つの領域は、一対の白色領域Ｍ１１と、一対の黒色領域Ｍ１２とで構成される。画像照合用マーカＭ１においては、白色領域Ｍ１１と黒色領域Ｍ１２とがそれぞれ隣り合うように配置されている。
【００３９】
このように白色領域Ｍ１１及び黒色領域Ｍ１２を配置することにより、３Ｄスキャナ４で適切に画像照合用マーカＭ１の中心位置を特定することができる。３Ｄスキャナ４では、計測対象が黒色の場合に反射光を適切に得ることができない特性を有する。これに対し、画像照合用マーカＭ１を白色領域Ｍ１１及び黒色領域Ｍ１２で構成することにより、白色領域Ｍ１１と黒色領域Ｍ１２との境界位置に基づいて画像照合用マーカＭ１の中心位置を正確に特定することができる。
【００４０】
上述したように、対象物Ｐには、予め視覚サーボ法に利用される照合点Ｃが設定されている。一般に、照合点Ｃは、対象物Ｐの形状のうち、特徴を有する部分に設定される。本実施の形態においては、図２に示すように、正面視にて対象物Ｐが長方形状を有しており、その一方側の面の各頂点に照合点Ｃ１〜Ｃ４が設定されているものとする。
【００４１】
以下、このような構成を有するロボット制御システム１において、対象物Ｐを目標位置に移動する際の動作について説明する。図３は、第１の実施の形態に係るロボット制御システム１における全体動作を説明するためのフロー図である。なお、本実施の形態に係るロボット制御システム１において、対象物Ｐを目標位置に移動する際には、制御装置３の構成要素が主体となって制御を行う。
【００４２】
対象物Ｐを目標位置に移動する際、駆動制御部３３がロボット２の駆動を制御する。駆動制御部３３は、まず、ハンド部２４を対象物Ｐのピックアップ位置に移動する。なお、ピックアップ位置に対するハンド部２４の移動及びピックアップ位置における対象物Ｐの把持については、予め定められた動作プログラムにより制御されるものとする。ハンド部２４をピックアップ位置まで移動させた後、駆動制御部３３は、ハンド部２４の把持部２５により対象物Ｐを把持する。これにより、ピックアップ位置において、把持部２５により対象物Ｐが把持される（ステップＳＴ１０）。
【００４３】
把持部２５により対象物Ｐが把持された後、駆動制御部３３は、ハンド部２４を３Ｄスキャナ４で計測し易い位置（以下、「３Ｄ計測位置」という）まで移動する。３Ｄ計測位置に対するハンド部２４の移動が完了すると、その旨が画像処理部３１（より具体的には、３Ｄ処理部３１１）に通知される。この通知を受けると、３Ｄ処理部３１１は、対象物Ｐを把持した状態のハンド部２４の三次元情報を解析する処理（三次元情報解析処理）を実行する（ステップＳＴ２０）。
【００４４】
ここで、三次元情報解析処理について説明する。図４は、第１の実施の形態に係るロボット制御システム１における三次元情報解析処理を説明するためのフロー図である。三次元情報解析処理において、３Ｄ処理部３１１は、まず３Ｄスキャナ４に対して対象物Ｐを把持した状態のハンド部２４の三次元情報（三次元座標値）の取得を指示する。この指示に応じて３Ｄスキャナ４は、対象物Ｐを把持した状態のハンド部２４の三次元情報を取得する（ステップＳＴ２１）。取得された三次元情報は、３Ｄ処理部３１１に出力される。なお、ステップＳＴ２１で取得される三次元情報は、３Ｄモデルと呼ぶこともできる。
【００４５】
３Ｄスキャナ４から三次元情報を受け取ると、３Ｄ処理部３１１は、ハンド部２４に設けられた画像照合用マーカＭ１〜Ｍ４及び対象物Ｐの照合点Ｃ１〜Ｃ４の三次元座標値（以下、適宜「３Ｄ座標値」という）を取得（抽出）する（ステップＳＴ２２）。ここでは、３Ｄスキャナ４を基準とした三次元座標（以下、適宜「スキャナ基準三次元座標」という）に基づく画像照合用マーカＭ１〜Ｍ４及び照合点Ｃ１〜Ｃ４の３Ｄ座標値が取得される。なお、取得された画像照合用マーカＭ１〜Ｍ４及び照合点Ｃ１〜Ｃ４の３Ｄ座標値は、一時的に記憶部３２に格納される。
【００４６】
図５は、第１の実施の形態に係るロボット制御システム１における三次元情報解析処理で得られる三次元座標値の説明図である。図５においては、スキャナ基準三次元座標系の画像照合用マーカＭ１〜Ｍ４及び照合点Ｃ１〜Ｃ４の３Ｄ座標値を示している。なお、図５においては、説明の便宜上、対象物Ｐを把持した状態のハンド部２４のみを示している。
【００４７】
図５に示すように、ステップＳＴ２２の処理により、画像照合用マーカＭ１の３Ｄ座標値として「Ｘｓｍ１，Ｙｓｍ１，Ｚｓｍ１」が抽出される。同様に、画像照合用マーカＭ２、Ｍ３及びＭ４の３Ｄ座標値として、それぞれ「Ｘｓｍ２，Ｙｓｍ２，Ｚｓｍ２」、「Ｘｓｍ３，Ｙｓｍ３，Ｚｓｍ３」及び「Ｘｓｍ４，Ｙｓｍ４，Ｚｓｍ４」が抽出される。また、対象物Ｐの照合点Ｃ１の３Ｄ座標値として「Ｘｓｃ１，Ｙｓｃ１，Ｚｓｃ１」が抽出される。同様に、照合点Ｃ２、Ｃ３及びＣ４の３Ｄ座標値として、それぞれ「Ｘｓｃ２，Ｙｓｃ２，Ｚｓｃ２」、「Ｘｓｃ３，Ｙｓｃ３，Ｚｓｃ３」及び「Ｘｓｃ４，Ｙｓｃ４，Ｚｓｃ４」が抽出される。
【００４８】
画像照合用マーカＭ１〜Ｍ４及び対象物Ｐの照合点Ｃ１〜Ｃ４の３Ｄ座標値を抽出した後、３Ｄ処理部３１１は、画像照合用マーカＭ１〜Ｍ４を基準とした三次元座標（以下、適宜「マーカ基準三次元座標」という）を生成する（ステップＳＴ２３）。このマーカ基準三次元座標は、ハンド部２４に設けられた画像照合用マーカＭ１〜Ｍ４の配列を基準とした三次元座標である。３Ｄ処理部３１１は、ハンド部２４に設けられた画像照合用マーカＭ１〜Ｍ４の配列から、このマーカ基準三次元座標を生成する。
【００４９】
図６は、第１の実施の形態に係るロボット制御システム１における三次元情報解析処理で得られる三次元座標値の説明図である。図６においては、マーカ基準三次元座標系の画像照合用マーカＭ１〜Ｍ４の３Ｄ座標値を示している。図６に示すように、マーカ基準三次元座標上においては、例えば、画像照合用マーカＭ１の３Ｄ座標値として「Ｘｍｍ１，Ｙｍｍ１，Ｚｍｍ１」が割り当てられる。同様に、画像照合用マーカＭ２、Ｍ３及びＭ４の３Ｄ座標値として、それぞれ「Ｘｍｍ２，Ｙｍｍ２，Ｚｍｍ２」、「Ｘｍｍ３，Ｙｍｍ３，Ｚｍｍ３」及び「Ｘｍｍ４，Ｙｍｍ４，Ｚｍｍ４」が割り当てられる。
【００５０】
このようにマーカ基準三次元座標を生成することにより、スキャナ基準三次元座標系の画像照合用マーカＭ１〜Ｍ４の３Ｄ座標値と、マーカ基準三次元座標系における画像照合用マーカＭ１〜Ｍ４等の３Ｄ座標値との相対位置関係が特定される。例えば、スキャナ基準三次元座標系の画像照合用マーカＭ１の３Ｄ座標値「Ｘｓｍ１，Ｙｓｍ１，Ｚｓｍ１」は、マーカ基準三次元座標系の３Ｄ座標値「Ｘｍｍ１，Ｙｍｍ１，Ｚｍｍ１」に関連付けられることが分かる。
【００５１】
本実施の形態において、画像照合用マーカＭ１〜Ｍ４は、マーカ基準三次元座標の原点及び軸（Ｘ軸、Ｙ軸、Ｚ軸）方向が特定される位置に配置されている。すなわち、本実施の形態においては、画像照合用マーカＭ１、Ｍ２の中心位置を通過する直線によりマーカ基準三次元座標のＸ軸を特定することができる。また、画像照合用マーカＭ３、Ｍ４の中心位置を通過する直線によりマーカ基準三次元座標のＹ軸を特定することができる。さらに、これらのＸ軸とＹ軸とが交差する点で原点を特定することができる。さらに、この原点を通過し、Ｘ軸及びＹ軸を含む平面と直交する直線によりマーカ基準三次元座標のＺ軸を特定することができる。このように画像照合用マーカＭ１〜Ｍ４が、マーカ基準三次元座標の原点及び軸方向を特定可能な位置に配置されることから、マーカ基準三次元座標を容易に生成することができるものとなっている。
【００５２】
図６に示すようなマーカ基準三次元座標を生成した後、３Ｄ処理部３１１は、照合点Ｃ１〜Ｃ４のマーカ基準三次元座標系の３Ｄ座標値を算出する（ステップＳＴ２４）。このとき、３Ｄ処理部３１１は、スキャナ基準三次元座標とマーカ基準三次元座標との相対位置関係に基づいて、ステップＳＴ２２で抽出した照合点Ｃ１〜Ｃ４のスキャナ基準三次元座標系の３Ｄ座標値から照合点Ｃ１〜Ｃ４のマーカ基準三次元座標系の３Ｄ座標値を算出する。ここでは、図示していないが、照合点Ｃ１の３Ｄ座標値として「Ｘｍｃ１，Ｙｍｃ１，Ｚｍｃ１」が算出されるものとする。同様に、照合点Ｃ２、Ｃ３及びＣ４の３Ｄ座標値として、それぞれ「Ｘｍｃ２，Ｙｍｃ２，Ｚｍｃ２」、「Ｘｍｃ３，Ｙｍｃ３，Ｚｍｃ３」及び「Ｘｍｃ４，Ｙｍｃ４，Ｚｍｃ４」が算出されるものとする。
【００５３】
このように対象物Ｐの照合点Ｃ１〜Ｃ４のマーカ基準三次元座標系の３Ｄ座標値の算出まで完了すると、三次元情報解析処理が終了する。三次元情報解析処理が終了すると、３Ｄ処理部３１１からその旨が駆動制御部３３に通知される。この通知を受けると、駆動制御部３３は、対象物Ｐを把持した状態のハンド部２４が目標位置の近傍まで移動するようにロボット２を制御する。なお、対象物Ｐを把持した状態のハンド部２４の目標位置近傍までの移動については、予め定められた動作プログラムにより制御されるものとする。
【００５４】
対象物Ｐを把持した状態のハンド部２４が目標位置近傍に到達すると、駆動制御部３３は、２Ｄ処理部３１２にその旨を通知する。この通知を受けると、２Ｄ処理部３１２は、駆動制御部３３と共同して視覚サーボ法によるロボット制御処理（以下、単に「ロボット制御処理」という）を行う（図３に示すステップＳＴ３０）。
【００５５】
ここで、ロボット制御処理について説明する。図７は、第１の実施の形態に係るロボット制御システム１におけるロボット制御処理を説明するためのフロー図である。ロボット制御処理において、２Ｄ処理部３１２は、まずカメラ５に対して対象物Ｐを把持した状態のハンド部２４の撮像を指示する。この指示に応じてカメラ５は、対象物Ｐを把持した状態のハンド部２４を撮像する（ステップＳＴ３１）。撮像された画像情報は、２Ｄ処理部３１２に出力される。撮像された画像情報は、カメラ５を基準とした二次元座標（以下、適宜「カメラ基準二次元座標」という）上の情報として捉えることができる。
【００５６】
カメラ５から画像情報を受け取ると、２Ｄ処理部３１２は、画像情報から対象物Ｐの照合点Ｃ１〜Ｃ４のカメラ基準二次元座標系の二次元座標値（以下、適宜「２Ｄ座標値」という）を抽出する（ステップＳＴ３２）。ここで、照合点Ｃ１〜Ｃ４の２Ｄ座標値は、カメラ５から受け取った画像情報に対して二値化処理やエッジ処理等の画像処理を施すことにより抽出される。
【００５７】
そして、２Ｄ処理部３１２は、対象物Ｐの全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値を抽出できたか否かを判定する（ステップＳＴ３３）。このステップＳＴ３３の処理により、カメラ５により全ての照合点Ｃ１〜Ｃ４を撮像できたか否かを判定することができる。例えば、目標位置近傍で照合点Ｃ１〜Ｃ４のいずれか又は全部が他の部品等で遮蔽されているか否かを判定することができる。なお、全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値の抽出可否については、記憶部３３に記憶された対象物Ｐの照合点に関する情報との比較により判定される。
【００５８】
ここで、全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値が抽出された場合（ステップＳＴ３３：ＹＥＳ）、すなわち、全ての照合点Ｃ１〜Ｃ４が撮像された場合、２Ｄ処理部３１２は、これらの照合点Ｃ１〜Ｃ４が予め定められた対象物Ｐの目標点（目標位置）に一致しているかを判定する（ステップＳＴ３４）。ここで、全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値が目標点と一致している場合、２Ｄ処理部３１２は、対象物Ｐが目標位置に到達したと認識してロボット制御処理を終了する。一方、全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値が目標点と一致していない場合、２Ｄ処理部３１２は、その旨を駆動制御部３３に通知する。
【００５９】
この通知を受けると、駆動制御部３３は、現在の照合点Ｃ１〜Ｃ４と、画像上に与えられる目標点（図８、図９参照）との相対位置関係に基づいて視覚サーボ法による駆動制御を行う（ステップＳＴ３５）。なお、画像上に与えられる目標点は、記憶部３２に記憶された目標点に関する情報に基づいて特定される。ハンド部２４に対する一定量の駆動制御を行うと、駆動制御部３３は、その旨を２Ｄ処理部３１２に通知する。この通知を受けると、２Ｄ処理部３１２は、ステップＳＴ３１以降の処理を再び実行する。
【００６０】
一方、ステップＳＴ３３において、全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値が抽出されない場合（ステップＳＴ３３：ＮＯ）、すなわち、全ての照合点Ｃ１〜Ｃ４が撮像されなかった場合、２Ｄ処理部３１２は、ステップＳＴ３１で撮像された画像情報から画像照合用マーカＭ１〜Ｍ４のカメラ基準二次元座標系の２Ｄ座標値を抽出する（ステップＳＴ３６）。ここで、画像照合用マーカＭ１〜Ｍ４の２Ｄ座標値は、照合点Ｃ１〜Ｃ４と同様に、受け取った画像情報に対して二値化処理やエッジ処理等の画像処理を施すことにより抽出される。
【００６１】
そして、２Ｄ処理部３１２は、抽出した画像照合用マーカＭ１〜Ｍ４のカメラ基準二次元座標系の２Ｄ座標値と、上述した三次元情報解析処理で抽出した画像照合用マーカＭ１〜Ｍ４のマーカ基準三次元座標系の３Ｄ座標値とに基づいて射影変換行列を算出する（ステップＳＴ３７）。より具体的にいうと、２Ｄ処理部３１２は、これらの情報に基づいて、マーカ基準三次元座標からカメラ基準二次元座標に射影する射影変換行列を算出する。
【００６２】
この射影変換行列を算出することにより、マーカ基準三次元座標系の任意の３Ｄ座標値に対応するカメラ基準二次元座標系の２Ｄ座標値を求めることが可能となる。言い換えると、マーカ基準三次元座標系の任意の３Ｄ座標値から、カメラ基準二次元座標系で対応する２Ｄ座標値を推定することが可能となる。
【００６３】
ここで、画像照合用マーカＭ１〜Ｍ４のカメラ基準二次元座標系の２Ｄ座標値は、ステップＳＴ３６にて抽出されている。まず、マーカ基準三次元座標系における画像照合用マーカＭ１〜Ｍ４の３Ｄ座標値と、カメラ基準二次元座標系における画像照合用マーカＭ１〜Ｍ４の２Ｄ座標値とから、マーカ基準三次元座標系からカメラ基準二次元座標系に変換する射影変換行列を求める。一方、照合点Ｃ１〜Ｃ４のマーカ基準三次元座標系の３Ｄ座標値は、上述した三次元情報解析処理により既知となっている。このため、照合点Ｃ１〜Ｃ４のマーカ基準三次元座標系の３Ｄ座標値から先に求めた射影変換行列を用いて演算することで、カメラ基準二次元座標系の照合点Ｃ１〜Ｃ４の２Ｄ座標値を推定することができる。
【００６４】
ステップＳＴ３７において、射影変換行列を算出した後、２Ｄ処理部３１２は、この射影変換行列を用いて対象物Ｐの照合点Ｃ１〜Ｃ４のカメラ基準二次元座標系の２Ｄ座標値を推定する（ステップＳＴ３８）。より具体的にいうと、２Ｄ処理部３１２は、上述した手順により推定したマーカ基準三次元座標系の照合点Ｃ１〜Ｃ４の３Ｄ座標値に対して射影変換行列を用いて演算することにより、カメラ基準二次元座標系の照合点Ｃ１〜Ｃ４の２Ｄ座標値を推定する。
【００６５】
カメラ基準二次元座標系の照合点Ｃ１〜Ｃ４の２Ｄ座標値を推定した後、２Ｄ処理部３１２は、これらの照合点Ｃ１〜Ｃ４が予め定められた対象物Ｐの目標点（目標位置）に一致しているかを判定する（ステップＳＴ３４）。ここで、全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値が目標点と一致している場合、２Ｄ処理部３１２は、対象物Ｐが目標位置に到達したと認定してロボット制御処理が終了する。一方、全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値が目標点と一致していない場合、２Ｄ処理部３１２は、その旨を駆動制御部３３に通知する。
【００６６】
この通知を受けると、駆動制御部３３は、現在の照合点Ｃ１〜Ｃ４と、画像上に与えられる目標点（図８、図９参照）との相対位置関係に基づいて視覚サーボ法による駆動制御を行う（ステップＳＴ３５）。そして、ハンド部２４に対する一定量の駆動制御を行うと、その旨を２Ｄ処理部３１２に通知する。この通知を受けると、２Ｄ処理部３１２は、ステップＳＴ３１以降の処理を再び実行する。このような処理を繰り返す過程において、ステップＳＴ３４において、全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値が目標点と一致すると判定されると、２Ｄ処理部３１２は、一連のロボット制御処理を終了する。
【００６７】
ここで、対象物Ｐを目標位置まで移動する際の具体例について、図８及び図９を参照して説明する。図８及び図９は、第１の実施の形態に係るロボット制御システム１におけるロボット制御処理により移動するハンド部２４の説明図である。なお、図８及び図９においては、カメラ５により撮像された画像情報を示している。また、図８及び図９においては、説明の便宜上、対象物Ｐを把持した状態のハンド部２４のみを示している。
【００６８】
また、図８及び図９においては、対象物Ｐが目標位置に到達する直前の状態（目標到達前）と、対象物Ｐが目標位置に到達した状態（目標到達後）について示している。図８においては、目標到達前の状態において、対象物Ｐの照合点Ｃ１〜Ｃ４が遮蔽物により遮蔽されない場合（カメラ５により撮像可能な場合）について示している。図９においては、目標到達前の状態において、照合点Ｃ１〜Ｃ４の一部が遮蔽物ＭＡによって遮蔽される場合（カメラ５により一部の照合点Ｃ１〜Ｃ４が撮像不能な場合）について示している。なお、図８及び図９においては、説明の便宜上、対象物Ｐの照合点Ｃ１〜Ｃ４が一致される目標点ＯＢ１〜ＯＢ４を示している。
【００６９】
図８に示す目標到達前の状態において、図７に示すロボット制御処理が実行されると、ステップＳＴ３３において、対象物Ｐの全ての照合点Ｃ１〜Ｃ４が抽出される（ステップＳＴ３３：ＹＥＳ）。そして、ステップ３４において、各照合点Ｃ１〜Ｃ４と目標点ＯＢ１〜ＯＢ４との一致が判定される。この状態では、各照合点Ｃ１〜Ｃ４と目標点ＯＢ１〜ＯＢ４とが一致していない（ステップＳＴ３４：ＮＯ）。このため、視覚サーボ法による駆動制御が行われ（ステップＳＴ３５）、ステップＳＴ３１以降の処理が繰り返される。そして、視覚サーボ法による駆動制御によって図８に示す目標到達後の状態となると、ステップＳＴ３４において、各照合点Ｃ１〜Ｃ４と目標点ＯＢ１〜ＯＢ４とが一致したと判定され、ロボット制御処理が完了する。このように目標位置近傍において、全ての照合点Ｃ１〜Ｃ４が抽出（撮像）される場合には、通常の視覚サーボ法によりハンド部２４の移動制御が行われる。
【００７０】
一方、図９に示す目標到達前の状態において、図７に示すロボット制御処理が実行されると、ステップＳＴ３３において、対象物Ｐの全ての照合点Ｃ１〜Ｃ４を抽出することができない（ステップＳＴ３３：ＮＯ）。より具体的には、照合点Ｃ３及びＣ４の２Ｄ座標値を抽出することができない。この場合、ステップＳＴ３６において、ステップＳＴ３１で撮像された画像情報から画像照合用マーカＭ１〜Ｍ４のカメラ基準二次元座標系の２Ｄ座標値が抽出される。
【００７１】
そして、ステップＳＴ３７において、抽出された画像照合用マーカＭ１〜Ｍ４のカメラ基準二次元座標系の２Ｄ座標値と、上述した三次元情報解析処理（図４参照）で抽出された画像照合用マーカＭ１〜Ｍ４のマーカ基準三次元座標系の３Ｄ座標値とに基づいて射影変換行列が算出される。さらに、ステップＳＴ３８において、マーカ基準三次元座標系の照合点Ｃ１〜Ｃ４の３Ｄ座標値に射影変換行列を用いて演算され、カメラ基準二次元座標系の照合点Ｃ１〜Ｃ４の２Ｄ座標値が推定される。これにより、ステップＳＴ３３にて抽出されなかった照合点Ｃ３、Ｃ４の２Ｄ座標値が推定される。
【００７２】
照合点Ｃ３、Ｃ４の２Ｄ座標値が推定されると、ステップ３４において、各照合点Ｃ１〜Ｃ４と目標点ＯＢ１〜ＯＢ４との一致が判定される。この状態では、各照合点Ｃ１〜Ｃ４と目標点ＯＢ１〜ＯＢ４とが一致していない（ステップＳＴ３４：ＮＯ）。このため、視覚サーボ法による駆動制御が行われ（ステップＳＴ３５）、ステップＳＴ３１以降の処理が繰り返される。そして、視覚サーボ法による駆動制御によって図９に示す目標到達後の状態となると、ステップＳＴ３４において、各照合点Ｃ１〜Ｃ４と目標点ＯＢ１〜ＯＢ４とが一致したと判定され、ロボット制御処理が完了する。このように目標位置近傍において、全ての照合点Ｃ１〜Ｃ４が抽出（撮像）されない場合には、抽出されなかった照合点Ｃ１〜Ｃ４が推定され、推定後の照合点Ｃ１〜Ｃ４に基づいて視覚サーボ法によりハンド部２４の移動制御が行われる。
【００７３】
以上説明したように、第１の実施の形態に係るロボット制御システム１においては、対象物Ｐを把持した状態のハンド部２４を計測して三次元情報を取得しておき、目標位置近傍にて対象物Ｐの照合点Ｃ１〜Ｃ４が撮像されない場合に、上記三次元情報に含まれる画像照合用マーカＭ１〜Ｍ４の３Ｄ座標値に基づいて対象物Ｐの照合点Ｃ１〜Ｃ４の２Ｄ座標値を推定する。そして、推定した照合点Ｃ１〜Ｃ４が目標位置に一致するようにハンド部２４の移動制御を行う。これにより、対象物Ｐの照合点Ｃ１〜Ｃ４が遮蔽などにより特定されない場合であっても、予め取得されたハンド部２４及び対象物Ｐの三次元情報から対象物Ｐの照合点Ｃ１〜Ｃ４を特定することができ、全ての照合点Ｃ１〜Ｃ４が特定される場合と同様に、対象物Ｐを目標位置に適切に移動することができる。
【００７４】
特に、制御装置３の推定部３１は、三次元情報に含まれる画像照合用マーカＭ１〜Ｍ４の３Ｄ座標値から画像照合用マーカＭ１〜Ｍ４を基準とした三次元座標（マーカ基準三次元座標）を生成する３Ｄ処理部３１１と、マーカ基準三次元座標とカメラ５で撮像された画像照合用マーカＭ１〜Ｍ４の２Ｄ座標値とに基づいて対象物Ｐの照合点Ｃ１〜Ｃ４の２Ｄ座標値を推定する２Ｄ処理部３１２とを有している。
【００７５】
この構成によれば、マーカ基準三次元座標と、カメラ５で撮像された画像照合用マーカＭ１〜Ｍ４の２Ｄ座標値（カメラ基準二次元座標系の２Ｄ座標値）とに基づいて対象物Ｐの照合点Ｃ１〜Ｃ４の２Ｄ座標値が推定される。このため、マーカ基準三次元座標に基づいて対象物Ｐの照合点Ｃ１〜Ｃ４を推定することができるので、対象物Ｐの照合点Ｃ１〜Ｃ４の特定の有無に関わらず、対象物Ｐを目標位置に適切に移動することができる。
【００７６】
また、２Ｄ処理部３１２は、画像照合用マーカＭ１〜Ｍ４の２Ｄ座標値とマーカ基準三次元座標系の画像照合用マーカＭ１〜Ｍ４の３Ｄ座標値とから、マーカ基準三次元座標からカメラ基準二次元座標に射影する射影変換行列を算出し、当該射影変換行列を用いてマーカ基準三次元座標系の対象物Ｐの照合点Ｃ１〜Ｃ４の３Ｄ座標値から対象物Ｐの照合点Ｃ１〜Ｃ４の２Ｄ座標値を推定する。
【００７７】
この構成によれば、マーカ基準三次元座標からカメラ基準二次元座標に射影する射影変換行列が算出され、この射影変換行列に基づいて対象物Ｐの照合点Ｃ１〜Ｃ４の２Ｄ座標値を推定することができる。これにより、カメラ５による対象物Ｐの照合点Ｃ１〜Ｃ４の撮像の有無に関わらず、ハンド部２４の移動制御に必要となる照合点Ｃ１〜Ｃ４の２Ｄ座標値を適切に推定することができる。
【００７８】
さらに、２Ｄ処理部３１２は、対象物Ｐの全ての照合点Ｃ１〜Ｃ４が撮像されない場合に射影変換行列を算出する。この構成によれば、対象物Ｐの照合点Ｃ１〜Ｃ４が１つでも撮像されない場合に射影変換行列が算出され、当該照合点Ｃ１〜Ｃ４の２Ｄ座標値が推定される。このため、対象物Ｐの全ての照合点Ｃ１〜Ｃ４を確実に推定することができるので、対象物Ｐを目標位置に適切に移動することができる。
【００７９】
さらに、本実施の形態に係るロボット制御システム１においては、ロボット２のハンド部２４に対して４個の画像照合用マーカＭ１〜Ｍ４が設けられている。このため、３Ｄスキャナ４で取得された三次元情報の中において、これらの画像照合用マーカＭ１〜Ｍ４を含む平面を特定することができる。これにより、画像照合用マーカＭ１〜Ｍ４を基準とした三次元座標（マーカ基準三次元座標）を適切に生成することができる。
【００８０】
（第２の実施の形態）
第１の実施の形態では、対象物Ｐの照合点Ｃが遮蔽され、照合点Ｃが特定できない場合であっても、対象物Ｐを目標位置に安定して移動する場合について説明している。第２の実施の形態では、対象物Ｐの移動経路の照明環境の不具合等により照合点Ｃを抽出できない場合に対象物Ｐを目標位置に安定して移動するものである。
【００８１】
また、第２の実施の形態では、視覚サーボ法にて要請されるロバスト性の向上及び高速処理を実現しながら、対象物Ｐを目標位置に安定して移動するものである。一般に、視覚サーボ法においては、対象物Ｐに予め設定された照合点Ｃを高速且つロバストにカメラ５で撮像された撮像画像から抽出することが要請される。しかしながら、撮像画像から照合点Ｃを抽出するアルゴリズムにおいては、ロバスト性を高くするほど演算量が多く高速化することが困難となるという課題が存在する。第２の実施の形態では、このような課題も解決する。
【００８２】
第２の実施の形態に係るロボット制御システムでは、予め保持された対象物Ｐの三次元辞書座標値群に基づいて、マーカ基準三次元座標系の対象物Ｐの照合点Ｃ１〜Ｃ４の３Ｄ座標値を推定する。そして、推定したマーカ基準三次元座標系の対象物Ｐの照合点Ｃ１〜Ｃ４の３Ｄ座標値と、画像照合用マーカＭ１〜Ｍ４の２Ｄ座標値とに基づいて、対象物Ｐの照合点Ｃ１〜Ｃ４の２Ｄ座標値を推定する点で第１の実施の形態と相違する。
【００８３】
より具体的には、図３のステップＳＴ２０及びＳＴ３０にそれぞれ示す三次元情報解析処理及びロボット制御処理において、第１の実施の形態と相違する。なお、第２の実施の形態に係るロボット制御システムにおいて、システムの構成要素は、第１の実施の形態と同様である。以下、第２の実施の形態に係るロボット制御システムについて、第１の実施の形態との相違点を中心に説明する。
【００８４】
図１０は、第２の実施の形態に係るロボット制御システムにおける三次元情報解析処理を説明するためのフロー図である。図１１は、第２の実施の形態に係るロボット制御システムにおけるロボット制御処理を説明するためのフロー図である。なお、図１０及び図１１において、それぞれ図４及び図７と共通する処理については同一の符号を付与し、その詳細な説明を省略する。
【００８５】
第２の実施の形態に係るロボット制御システムでは、対象物Ｐを目標位置に移動する動作に先立って、対象物Ｐの形状モデルである三次元辞書座標値群を保持している。この三次元辞書座標値群は、例えば、三次元ＣＡＤ（Computer-Aided  Design）システムにおける対象物Ｐの理想的な座標値群データとして得られる。このような三次元辞書座標値群は、移動制御の対象物Ｐが特定された場合に記憶部３２に格納される。
【００８６】
第２の実施の形態に係る三次元情報解析処理においては、図１０に示すように、第１の実施の形態と同様に、ステップＳＴ２１〜ＳＴ２３の処理が行われる。すなわち、ステップＳＴ２１において、対象物Ｐを把持した状態のハンド部２４の三次元情報（３Ｄ座標値）が取得される。そして、ステップＳＴ２２において、ハンド部２４に設けられた画像照合用マーカＭ１〜Ｍ４及び対象物Ｐの照合点Ｃ１〜Ｃ４の３Ｄ座標値が取得（抽出）される。そして、ステップＳＴ２３において、画像照合用マーカＭ１〜Ｍ４を基準とした三次元座標（マーカ基準三次元座標）が生成される。
【００８７】
マーカ基準三次元座標を生成した後、３Ｄ処理部３１１は、ステップＳＴ２１で取得した三次元情報（スキャナ基準三次元座標の三次元座標値群）の全てを、マーカ基準三次元座標の三次元座標値群に変換する（ステップＳＴ１０１）。これにより、対象物Ｐを把持した状態のハンド部２４におけるスキャナ基準三次元座標系の全ての３Ｄ座標値が、マーカ基準三次元座標の３Ｄ座標値として表現される。
【００８８】
次に、３Ｄ処理部３１１は、ＩＣＰ(Iterative  Closest  Point)アルゴリズムにより、マーカ基準三次元座標における対象物Ｐの位置及び姿勢を特定する（ステップＳＴ１０２）。ここで、ＩＣＰアルゴリズムとは、予め保持された対象物の形状モデルである三次元辞書座標値群と、三次元測定器（本実施の形態では３Ｄスキャナ４）で計測した三次元座標値群との間において、対応する座標値の誤差が最小となるように座標計算を繰り返し行うことで位置合わせを行い、対象物の位置と姿勢を特定する手法である。
【００８９】
ステップＳＴ１０２において、３Ｄ処理部３１１は、マーカ基準三次元座標の三次元座標値群と、予め保持された対象物Ｐの形状モデルである三次元辞書座標値群とをＩＣＰアルゴリズムにより座標計算を行う。そして、３Ｄ処理部３１１は、マーカ基準三次元座標における対象物Ｐの位置と姿勢を特定し、同次変換行列Ｔを求める。
【００９０】
この同次変換行列Ｔは、三次元辞書座標値（Ｐｄｍｉ）をマーカ基準三次元座標の三次元座標値（Ｐｍｍｉ）に座標変換する行列である。例えば、三次元辞書座標値群のｉ番目の座標値（ｘｄｍｉ，ｙｄｍｉ，ｚｄｍｉ）は、同次変換行列Ｔを用いた以下の式（１）によってマーカ基準三次元座標の三次元座標値（ｘｍｍｉ，ｙｍｍｉ，ｚｍｍｉ）に座標変換される。
（式１）
2017094482.tif 000003
【００９１】
そして、同次変換行列Ｔを求めた後、３Ｄ処理部３１１は、マーカ基準三次元座標系における対象物Ｐの照合点Ｃ１〜Ｃ４の３Ｄ座標値を算出する（ステップＳＴ１０３）。より具体的にいうと、３Ｄ処理部３１１は、予め保持された三次元辞書座標における対象物Ｐの照合点Ｃ１〜Ｃ４の座標値を、同次変換行列Ｔを用いてマーカ基準三次元座標系における対象物Ｐの照合点Ｃ１〜Ｃ４の３Ｄ座標値に変換する。
【００９２】
例えば、三次元辞書座標値群のｉ番目の照合点Ｃの座標値（ｘｄｃｉ，ｙｄｃｉ，ｚｄｃｉ）は、同次変換行列Ｔを用いた以下の式（２）によってマーカ基準三次元座標の照合点Ｃの三次元座標値（ｘｍｃｉ，ｙｍｃｉ，ｚｍｃｉ）に座標変換される。これにより、図示していないが、照合点Ｃ１の３Ｄ座標値として「Ｘｍｃ１，Ｙｍｃ１，Ｚｍｃ１」が算出される。同様に、照合点Ｃ２、Ｃ３及びＣ４の３Ｄ座標値として、それぞれ「Ｘｍｃ２，Ｙｍｃ２，Ｚｍｃ２」、「Ｘｍｃ３，Ｙｍｃ３，Ｚｍｃ３」及び「Ｘｍｃ４，Ｙｍｃ４，Ｚｍｃ４」が算出される。
（式２）
2017094482.tif 000004
【００９３】
このように対象物Ｐの照合点Ｃ１〜Ｃ４のマーカ基準三次元座標系の３Ｄ座標値の算出まで完了すると、三次元情報解析処理が終了する。三次元情報解析処理が終了すると、３Ｄ処理部３１１からその旨が駆動制御部３３に通知される。この通知を受けると、駆動制御部３３は、対象物Ｐを把持した状態のハンド部２４が目標位置の近傍まで移動するようにロボット２を制御する。なお、対象物Ｐを把持した状態のハンド部２４の目標位置近傍までの移動については、予め定められた動作プログラムにより制御されるものとする。
【００９４】
そして、対象物Ｐを把持した状態のハンド部２４が目標位置近傍に到達すると、２Ｄ処理部３１２は、駆動制御部３３からその旨の通知を受け、図１１に示すロボット制御処理（視覚サーボ法によるロボット制御処理）を行う。第２の実施の形態に係るロボット制御処理において、２Ｄ処理部３１２は、図７に示すステップＳＴ３３の処理を除き、第１の実施の形態と同様の処理を行う。
【００９５】
すなわち、ステップＳＴ３１において、対象物Ｐを把持した状態のハンド部２４が撮像される。そして、ステップＳＴ３２において、対象物Ｐの照合点Ｃ１〜Ｃ４のカメラ基準二次元座標系の二次元座標値（２Ｄ座標値）が抽出される。その後、第２の実施の形態では、ＳＴ３１で撮像された画像情報から画像照合用マーカＭ１〜Ｍ４のカメラ基準二次元座標系の２Ｄ座標値が抽出される（ステップＳＴ３６）。
【００９６】
そして、ステップＳＴ３７において、抽出した画像照合用マーカＭ１〜Ｍ４のカメラ基準二次元座標系の２Ｄ座標値と、上述した三次元情報解析処理で抽出した画像照合用マーカＭ１〜Ｍ４のマーカ基準三次元座標系の３Ｄ座標値とに基づいて射影変換行列が算出される。その後、ステップＳＴ３８において、この射影変換行列を用いて対象物Ｐの照合点Ｃ１〜Ｃ４のカメラ基準二次元座標系の２Ｄ座標値が推定される。
【００９７】
カメラ基準二次元座標系の照合点Ｃ１〜Ｃ４の２Ｄ座標値が推定された後、ステップＳＴ３４において、これらの照合点Ｃ１〜Ｃ４が予め定められた対象物Ｐの目標点（目標位置）に一致しているかが判定される。全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値が目標点と一致している場合には、ロボット制御処理が終了する。
【００９８】
一方、全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値が目標点と一致していない場合には、その旨が駆動制御部３３に通知される。そして、ステップ３５において、現在の照合点Ｃ１〜Ｃ４と、画像上に与えられる目標点（図８参照）との相対位置関係に基づいて視覚サーボ法による駆動制御が行われる。その後、上述したステップＳＴ３１以降の処理が再び実行される。そして、ステップＳＴ３４において、全ての照合点Ｃ１〜Ｃ４の２Ｄ座標値が目標点と一致すると判定されると、一連のロボット制御処理を終了する。
【００９９】
以上説明したように、第２の実施の形態に係るロボット制御システムにおいては、対象物Ｐを把持した状態のハンド部２４を計測して三次元情報（三次元座標値群）を取得しておき、予め保持された対象物Ｐの三次元辞書座標値群に基づいてマーカ基準三次元座標における対象物Ｐの位置及び姿勢を特定し、マーカ基準三次元座標における対象物Ｐの照合点Ｃ１〜Ｃ４の３Ｄ座標値を推定する。そして、この対象物Ｐの照合点Ｃ１〜Ｃ４の３Ｄ座標値からカメラ基準二次元座標系の照合点Ｃ１〜Ｃ４の２Ｄ座標値を推定する。その後、推定した照合点Ｃ１〜Ｃ４が目標位置に一致するようにハンド部２４の移動制御を行う。これにより、対象物Ｐの照合点Ｃ１〜Ｃ４が照明環境などにより特定されない場合であっても、対象物Ｐの照合点Ｃ１〜Ｃ４の２Ｄ座標値を特定することができ、全ての照合点Ｃ１〜Ｃ４が特定される場合と同様に、対象物Ｐを目標位置に適切に移動することができる。
【０１００】
特に、第２の実施の形態に係るロボット制御システムにおいては、予め保持された対象物Ｐの三次元辞書座標値群に基づいて対象物Ｐの照合点Ｃ１〜Ｃ４が抽出されるため、正確に対象物Ｐの照合点Ｃ１〜Ｃ４を抽出することができる。この結果、予め取得されたハンド部２４及び対象物Ｐの三次元情報から対象物Ｐの照合点Ｃ１〜Ｃ４を正確に特定でき、対象物Ｐを目標位置に適切に移動することができる。
【０１０１】
また、第２の実施の形態に係るロボット制御システムにおいては、マーカ基準三次元座標における対象物Ｐの三次元座標値群と、予め保持された対象物Ｐの三次元辞書座標値群とに基づいて、マーカ基準三次元座標における対象物Ｐの位置及び姿勢を特定し、マーカ基準三次元座標における対象物Ｐの照合点Ｃ１〜Ｃ４を推定する。このため、予め保持された対象物Ｐの三次元辞書座標値群に基づいてマーカ基準三次元座標における対象物Ｐの照合点Ｃ１〜Ｃ４が推定されるので、照合点Ｃ１〜Ｃ４を推定する際に必要となる演算量を低減することができる。
【０１０２】
特に、第２の実施の形態に係るロボット制御システムにおいて、マーカ基準三次元座標における対象物Ｐの三次元座標値群と、予め保持された対象物Ｐの三次元辞書座標値群とに基づいて、ＩＣＰアルゴリズムによってマーカ基準三次元座標における対象物Ｐの位置及び姿勢が特定される。このため、マーカ基準三次元座標における対象物Ｐの三次元座標値群と、対象物Ｐの三次元辞書座標値群との誤差が最小となるように位置合わせが行われる。これにより、マーカ基準三次元座標における対象物Ｐの三次元座標値群と、対象物Ｐの三次元辞書座標値群との完全一致を要求しないで位置合わせを行うことができ、ロバスト性を向上することができる。
【０１０３】
なお、第２の実施の形態に係るロボット制御システムでは、マーカ基準三次元座標における対象物Ｐの位置及び姿勢を特定する際にＩＣＰアルゴリズムを利用する場合について説明している。しかしながら、マーカ基準三次元座標における対象物Ｐの位置及び姿勢を特定する際のアルゴリズムについては、ＩＣＰアルゴリズムに限定されるものではなく適宜変更が可能である。すなわち、マーカ基準三次元座標における対象物Ｐの三次元座標値群と、予め保持された対象物Ｐの三次元辞書座標値群との位置合わせを可能とする任意のアルゴリズムを適用することができる。
【０１０４】
なお、本発明は上記実施の形態に限定されず、さまざまに変更して実施可能である。上記実施の形態において、添付図面に図示されている部材や孔などの大きさや形状などについては、これに限定されず、本発明の効果を発揮する範囲内で適宜変更が可能である。その他、本発明の目的の範囲を逸脱しない限りにおいて適宜変更して実施可能である。
【０１０５】
例えば、上記実施の形態においては、ロボット２のハンド部２４の一方の面に画像照合用マーカＭ１〜Ｍ４を設ける場合について説明している。しかしながら、画像照合用マーカＭの配置については、これに限定されるものではなく適宜変更が可能である。例えば、ハンド部２４の両面や、ハンド部２４の側面に画像照合用マーカＭを設けるようにしてもよい。このように画像照合用マーカＭを設ける場合には、対象物Ｐを目標位置に移動する際にハンド部２４が回転するような態様においても、画像照合用マーカＭを識別することにより、適切に対象物Ｐを移動することができる。なお、この場合には、ハンド部２４の表面、裏面及び側面等、画像照合用マーカＭが配置される面毎に画像照合用マーカＭの態様（形状、模様等）を変更する必要がある。
【０１０６】
また、上記実施の形態においては、ロボット２のハンド部２４に同一の態様（形状、模様等）の画像照合用マーカＭ１〜Ｍ４を設ける場合について説明している。しかしながら、画像照合用マーカＭの態様については、これに限定されるものではなく適宜変更が可能である。例えば、画像照合用マーカＭ１〜Ｍ４の一部（例えば、マーカ基準三次元座標の原点となる画像照合用マーカＭ）の態様を変更することや、全部の態様をそれぞれ変更するようにしてもよい。この場合には、特定の画像照合用マーカＭを検出することにより、マーカ基準三次元座標を容易に生成することができる。
【０１０７】
さらに、上記実施の形態においては、画像照合用マーカＭを４つの領域に分割し、一対の白色領域Ｍ１１と一対の黒色領域Ｍ１２と隣接させて構成する場合について示している。しかしながら、画像照合用マーカＭの構成については、これに限定されるものではなく適宜変更が可能である。例えば、３Ｄスキャナ４によって画像照合用マーカＭの中心位置を特定し易くなることを条件として、任意の領域に分割し、白色領域Ｍ１１と黒色領域Ｍ１２とを混在させるようにしてもよい。
【０１０８】
さらに、上記実施の形態においては、制御装置３における画像処理部３１が対象物Ｐの照合点Ｃを推定する推定部としての機能を有する場合について示している。しかしながら、制御装置３の構成については、これに限定されるものではなく適宜変更が可能である。対象物Ｐの照合点Ｃを推定する機能を有する構成要素を画像処理部３１とは別に備えるようにしてもよい。
【０１０９】
さらに、上記実施の形態においては、対象物Ｐの照合点Ｃが遮蔽によって特定できない場合について例示して説明している。しかしながら、照合点Ｃが特定できない状況については、遮蔽に起因するものに限られず、様々な態様に適用することができる。例えば、遮蔽以外の任意の理由でカメラ５によって照合点Ｃを撮像できない場合にも、適切に対象物Ｐを目標位置まで移動することができる。
【０１１０】
さらに、上記実施の形態においては、制御装置３における画像処理部３１が対象物Ｐの照合点Ｃを推定する推定部としての機能を有する場合について示している。しかしながら、制御装置３の構成については、これに限定されるものではなく適宜変更が可能である。対象物Ｐの照合点Ｃを推定する機能を有する構成要素を画像処理部３１とは別に備えるようにしてもよい。
【産業上の利用可能性】
【０１１１】
本発明のロボット制御システムによれば、対象物の移動経路の周辺環境の影響を受けることなく、対象物を目標位置に安定して移動することができるという効果を奏し、特に、対象物Ｐを目標位置まで視覚サーボ法により移動させるロボット制御システムに好適に用いることができる。
【符号の説明】
【０１１２】
１  ロボット制御システム
２  多関節ロボット（ロボット）
２４  ハンド部
２５  把持部
３  制御装置
３１  推定部
３１１  三次元画像処理部（３Ｄ処理部）
３１２  二次元画像処理部（２Ｄ処理部）
３２  記憶部
３３  駆動制御部
４  ３次元スキャナ（３Ｄスキャナ）
５  カメラ
Ｐ  対象物
Ｍ、Ｍ１〜Ｍ４  画像照合用マーカ
Ｃ、Ｃ１〜Ｃ４  照合点
ＯＢ１〜ＯＢ４  目標点
【図１】
2017094482.tif 000005
【図２】
2017094482.tif 000006
【図３】
2017094482.tif 000007
【図４】
2017094482.tif 000008
【図５】
2017094482.tif 000009
【図６】
2017094482.tif 000010
【図７】
2017094482.tif 000011
【図８】
2017094482.tif 000012
【図９】
2017094482.tif 000013
【図１０】
2017094482.tif 000014
【図１１】
2017094482.tif 000015