(57)【要約】【課題】対象物の姿勢の推定精度を保ちつつ対象物の姿勢推定処理を簡便にする。【解決手段】第１の撮像装置が対象物を撮像して得た第１の撮像装置画像と対象物のモデル画像との相違度を示す第１の相違度をモデル画像における対象物の姿勢毎に算出する第１の相違度算出部１５１と、変換規則に基づいて、第１の仮想空間における対象物の複数の姿勢に対応する第２の仮想空間における対象物の複数の姿勢を取得する姿勢取得部１５２と、第２の撮像装置が対象物を撮像して得た第２の撮像装置画像と対象物のモデル画像との相違度を示す第２の相違度をモデル画像における対象物の姿勢毎に算出する第２の相違度算出部１５３と、複数の第１の相違度と複数の第２の相違度とに基づいて第１の撮像装置座標系における対象物の姿勢を推定する姿勢推定部１５４と、を備える。【選択図】図５
2014161937.tif 000002
【特許請求の範囲】
【請求項１】
第１の撮像装置が対象物を撮像して得た第１の撮像画像と、前記第１の撮像装置が撮像した空間に対応する第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における対象物の複数の異なる姿勢毎に算出する第１の相違度算出部と、
前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記第２の仮想空間における対象物の複数の姿勢毎に算出する第２の相違度算出部と、
前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて前記第１の撮像座標系における前記対象物の姿勢を推定する姿勢推定部と、
を備える姿勢検出装置。
【請求項２】
前記姿勢推定部は、前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度のそれぞれに重み係数を乗じ、乗じることで得た値を基に前記第１の撮像座標系における前記対象物の姿勢を推定する請求項１に記載の姿勢検出装置。
【請求項３】
前記第１の撮像装置が前記第２の撮像装置よりも前記対象物の視認性が良い場合、
前記姿勢推定部は、前記対象物が目標の位置から予め決められた位置範囲にあるとき、前記第１の相違度に乗じる重み係数を前記第２の相違度に乗じる重み係数より大きくする請求項２に記載の姿勢検出装置。
【請求項４】
前記第１の撮像装置が前記第２の撮像装置よりも前記対象物の視認性が良い場合、
前記姿勢推定部は、前記対象物が目標の位置から予め決められた位置範囲にあるとき、前記第２の相違度に乗じる重み係数が予め決められた範囲に収まるよう、前記対象物の移動開始時刻から前記対象物が目標の位置から予め決められた位置範囲に到達する時刻まで、前記第２の相違度に乗じる重み係数を変化させる請求項２または３に記載の姿勢検出装置。
【請求項５】
前記第１の撮像装置が前記第２の撮像装置よりも前記対象物の視認性が良い場合、
前記姿勢推定部は、前記対象物の移動開始時刻から前記対象物が目標の位置から予め決められた位置範囲に到達する時刻までの期間で、前記第２の相違度に乗じる重み係数を時間の経過とともに小さくする請求項１または２に記載の姿勢検出装置。
【請求項６】
前記第１の撮像装置が前記第２の撮像装置よりも前記対象物の視認性が良く、前記対象物の位置が移動する場合、
前記姿勢推定部は、前記対象物の単位時間あたりの移動量が小さくなるに従って前記第２の相違度に乗じる重み係数を小さくする請求項１から３のいずれか一項に記載の姿勢検出装置。
【請求項７】
前記第１の撮像装置が前記第２の撮像装置よりも前記対象物の視認性が良い場合、
前記姿勢推定部は、前記第２の相違度に乗じる重み係数を、前記対象物が目標の位置に近づくに従って小さくする請求項１から４のいずれか一項に記載の姿勢検出装置。
【請求項８】
前記姿勢推定部は、前記第１の相違度に乗じる重み係数と、前記第２の相違度に乗じる重み係数を、前記第１の撮像画像と前記第２の撮像画像のそれぞれに混入している雑音成分に応じて変化させる請求項３に記載の姿勢検出装置。
【請求項９】
前記姿勢推定部は、前記第１の相違度に乗じる重み係数と、前記第２の相違度とに乗じる重み係数を、前記第１の撮像画像と前記第２の撮像画像のそれぞれに混入している正規化ランダムノイズの分散の逆数とする請求項８に記載の姿勢検出装置。
【請求項１０】
前記姿勢推定部は、前記第１の撮像装置で撮像した異なるフレーム間の画像の相違度を前記第１の撮像画像に混入している正規化ランダムノイズの分散として算出し、前記第２の撮像装置で撮像した異なるフレーム間の画像の相違度を前記第２の撮像画像に混入する正規化ランダムノイズの分散として算出する請求項９に記載の姿勢検出装置。
【請求項１１】
前記第１の撮像装置を基準とする第１の撮像座標系で前記対象物の姿勢を表したときの座標を第２の撮像装置を基準とする第２の撮像座標系で前記対象物の姿勢を表したときの座標へ変換する変換規則に基づいて、前記第１の仮想空間における前記対象物の複数の異なる姿勢に対応する第２の仮想空間における前記対象物の複数の姿勢をそれぞれ取得する姿勢取得部を更に備え、
前記第２の相違度算出部は、前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記姿勢取得部が取得した前記第２の仮想空間における対象物の複数の姿勢毎に算出する請求項１から１０のいずれか一項に記載の姿勢検出装置。
【請求項１２】
前記第１の相違度算出部は、前記第１の撮像装置が対象物を撮像することで得られた第１の撮像画像と、前記第１の仮想空間における前記対象物のモデル画像との相違度を示す第３の相違度を、前記第１の仮想空間における前記対象物の複数の異なる位置毎に算出し、
前記第２の相違度算出部は、前記第２の撮像装置が対象物を撮像することで得られた第２の撮像画像と、前記第２の仮想空間における前記対象物のモデル画像との相違度を示す第４の相違度を、前記第２の仮想空間における前記対象物の複数の異なる位置毎に算出し、
複数の前記第３の相違度と複数の前記第４の相違度に基づいて、前記第１の撮像座標系における前記対象物の位置を推定する位置推定部と、
を備える請求項１から１１のいずれか一項に記載の姿勢検出装置。
【請求項１３】
前記位置推定部は、
前記第１の相違度算出部が算出した複数の第３の相違度を参照して、前記第１の撮像座標系における撮像装置の光軸と直行する第１の平面の位置を推定し、前記第２の相違度算出部が算出した複数の第４の相違度を参照して、前記第２の撮像座標系における撮像装置の光軸と直行する第２の平面の位置を推定する平面位置推定部と、
前記第１の平面の位置と前記第２の平面の位置とを参照して、前記第１の撮像座標系における前記対象物の奥行きを算出する奥行き算出部と、
を備える請求項１２に記載の姿勢検出装置。
【請求項１４】
前記位置推定部は、複数の前記第３の相違度と複数の前記第４の相違度に基づいて、前記第１の撮像座標系における前記対象物の奥行きを推定する請求項１から１１のいずれか一項に記載の姿勢検出装置。
【請求項１５】
前記姿勢推定部が推定した対象物の姿勢とそれ以前に推定した前記対象物の姿勢との差を参照して、推定が収束したか否か判定する判定部と、
を備え、
前記判定部が収束していないと判定した場合、前記第１の相違度算出部は、前記対象物のモデル画像の姿勢を予め決められたように変更した後のモデル画像と前記第１の撮像画像とに基づいて前記複数の第１の相違度を算出し、前記第２の相違度算出部は、前記対象物のモデル画像の姿勢を予め決められたように変更した後のモデル画像と前記第２の撮像画像とに基づいて前記複数の第２の相違度を算出し、
前記姿勢推定部は、前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて、前記対象物の姿勢を推定し、
前記判定部は、前記姿勢推定部が推定した前記対象物の姿勢を用いて判定を再び行う請求項１から１４のいずれか一項に記載の姿勢検出装置。
【請求項１６】
前記対象物が目標の位置から予め決められた範囲にある際に、該対象物の位置または姿勢が変化する前後で前記第１の撮像装置が撮像して得た二つの画像間の変化量は、該対象物の位置または姿勢が変化する前後で前記第２の撮像装置が撮像して得た二つの画像間の変化量より大きい請求項１から１４のいずれか一項に記載の姿勢検出装置。
【請求項１７】
前記対象物が目標の位置及び姿勢にあるときの該対象物を前記第１の撮像装置で撮影した第１のゴール画像と前記第２の撮像装置で撮影した第２のゴール画像とを取得するゴール画像取得部と、
前記第１のゴール画像を撮影した空間を再現する第１の仮想空間において存在する対象物のモデルの位置及び姿勢から、予め決められた軸周りまたは軸方向に予め決められたように前記モデルを移動させて撮影した画像と、前記第１のゴール画像との相違度を示す第５の相違度を算出し、
前記第２のゴール画像を撮影した空間を再現する第２の仮想空間において存在する対象物のモデルの位置及び姿勢から、予め決められた軸周りまたは軸方向に予め決められたように前記モデルを移動させて撮影した画像と、第２のゴール画像との相違度を示す第６の相違度を算出し、
前記第５の相違度を算出した際の移動方向への前記第５の相違度の勾配と、前記第６の相違度を算出した際の移動方向への前記第６の相違度の勾配とを比較することで、姿勢推定の基準となる撮像装置を決定し、該決定した撮像装置を新たに前記第１の撮像装置とする位置決め基準撮像装置決定部と、
を備える請求項１から１５のいずれか一項に記載の姿勢検出装置。
【請求項１８】
前記対象物が複数ある場合、前記第１の相違度算出部は、前記第１の仮想空間において位置姿勢推定の対象となる対象物以外の対象物全ての画像領域を特定し、該特定した画像領域を演算対象から除いて第１の相違度を算出し、
前記第２の相違度算出部は、前記第２の仮想空間において位置姿勢推定の対象となる対象物以外の対象物全ての画像領域を特定し、該特定した画像領域を演算対象から除いて第２の相違度を算出する請求項１から１６のいずれか一項に記載の姿勢検出装置。
【請求項１９】
前記推定された対象物の位置及び姿勢と前記対象物が目標の位置にある場合の位置及び姿勢とに基づいて、現在の時刻から予め決められた時間経過した後のロボット座標系における位置及び姿勢の変化量を決定する目標位置姿勢決定部と、
ロボットを制御する時間間隔毎に、前記目標位置姿勢決定部が決定した変化量のうち最新の変化量に基づいて、前記時間間隔後のロボットの位置及び姿勢を決定し、該決定した位置及び姿勢になるようロボットを制御するロボット制御部と、
を更に備え、
前記ロボット制御部は、前記予め決められた時間のうち、自装置が第１の撮像画像及び第２の撮像画像から前記変化量を決定するまでの最短時間に基づいて決められた時間まではロボットが加速及び減速で移動するように前記ロボットの位置及び姿勢を決定し、その後の時間はロボットが等速で移動するように前記ロボットの位置及び姿勢を決定する請求項１２から１４に記載の姿勢検出装置。
【請求項２０】
第１の撮像装置が撮像した空間に対応する第１の仮想空間における対象物の複数の異なる姿勢毎に第１の相違度を算出する第１の相違度算出部と、
前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が撮像した空間に対応する第２の仮想空間における対象物の複数の姿勢毎に第２の相違度を算出する第２の相違度算出部と、
前記複数の第１の相違度と前記複数の第２の相違度とに基づいて前記第１の撮像装置を基準とする第１の撮像座標系における前記対象物の姿勢を推定する姿勢推定部と、
を備える姿勢検出装置。
【請求項２１】
第１の撮像装置が対象物を撮像することで得られた第１の撮像画像と、第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における前記対象物の複数の異なる位置毎に算出する第３の相違度算出部と、
第２の撮像装置が対象物を撮像することで得られた第２の撮像画像と、第２の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第２の仮想空間における前記対象物の複数の異なる位置毎に算出する第２の相違度算出部と、
前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて第１の撮像座標系における前記対象物の位置を推定する位置推定部と、
を備える位置検出装置。
【請求項２２】
第１の撮像装置が対象物を撮像して得た第１の撮像画像と、前記第１の撮像装置が撮像した空間に対応する第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における対象物の複数の異なる姿勢毎に算出する第１の相違度算出部と、
前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記第２の仮想空間における対象物の複数の姿勢毎に算出する第２の相違度算出部と、
前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて前記第１の撮像装置座標系における前記対象物の姿勢を推定する姿勢推定部と、
を備えるロボット。
【請求項２３】
可動部を有するロボットと、
第１の撮像装置が対象物を撮像して得た第１の撮像画像と、前記第１の撮像装置が撮像した空間に対応する第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における対象物の複数の異なる姿勢毎に算出する第１の相違度算出部と、
前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記第２の仮想空間における対象物の複数の姿勢毎に算出する第２の相違度算出部と、
前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて前記第１の撮像装置座標系における前記対象物の姿勢を推定する姿勢推定部と、
を備えるロボットシステム。
【請求項２４】
第１の相違度算出部が、第１の撮像装置が対象物を撮像して得た第１の撮像画像と、前記第１の撮像装置が撮像した空間に対応する第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における対象物の複数の異なる姿勢毎に算出する手順と、
第２の相違度算出部が、前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記第２の仮想空間における対象物の複数の姿勢毎に算出する手順と、
姿勢推定部が、前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて前記第１の撮像座標系における前記対象物の姿勢を推定する手順と、
を有する姿勢検出方法。
【請求項２５】
コンピューターに、
第１の撮像装置が対象物を撮像して得た第１の撮像画像と、前記第１の撮像装置が撮像した空間に対応する第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における対象物の複数の異なる姿勢毎に算出する第１の相違度算出ステップと、
前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記第２の仮想空間における対象物の複数の姿勢毎に算出する第２の相違度算出ステップと、
前記第１の相違度算出ステップで算出された複数の第１の相違度と、前記第２の相違度算出ステップで算出された複数の第２の相違度とに基づいて前記第１の撮像座標系における前記対象物の姿勢を推定する姿勢推定ステップと、
を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、姿勢検出装置、位置検出装置、ロボット、ロボットシステム、姿勢検出方法及びプログラムに関する。
【背景技術】
【０００２】
従来よりカメラで撮影した画像から対象物の位置及び姿勢を検出して、予め決められた位置及び姿勢に制御するロボットの技術が開示されている。従来技術において、カメラ座標におけるカメラの光軸方向の位置検出については、画像中の対象物の大きさからその位置を検出するが、位置の変化に対して対象物の大きさの変化の度合いが小さく、検出の精度が低いという問題があった。更に、対象物の姿勢変化によっては画像中の対象物が小さくなり、姿勢の検出精度が低くなるという問題があった。
【０００３】
この問題に対して、従来からステレオカメラを用いて、それぞれのカメラ座標におけるカメラの光軸と直交する平面上の位置を検出して、三角測量の原理を用いてカメラの光軸方向の位置を高精度に検出する技術が開示されている（例えば、特許文献１参照）。その技術では、更にそれぞれのカメラ座標において検出した姿勢を利用することで、姿勢の検出精度が低下することを防ぐことが開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特表２００４−５０８９５４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上記位置及び姿勢の検出精度を上げるためにはステレオカメラを構成するカメラ同士の相対的な位置及び姿勢を高精度に推定する必要があり、それぞれの座標の対応する点の数を増やす必要があり、煩雑で時間のかかる作業となってしまう。一方、上記対応点の数を減らして時間の短縮を図ると高精度な位置及び姿勢の検出が出来ないという問題がある。
【０００６】
そこで本発明の一態様は、上記問題に鑑みてなされたものであり、対象物の姿勢の推定精度を保ちつつ対象物の姿勢推定処理を簡便にすることを可能とする姿勢検出装置、位置検出装置、ロボット、ロボットシステム、姿勢検出方法及びプログラムを提供することを課題とする。
【課題を解決するための手段】
【０００７】
（１）本発明の一態様は、第１の撮像装置が対象物を撮像して得た第１の撮像画像と、前記第１の撮像装置が撮像した空間に対応する第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における対象物の複数の異なる姿勢毎に算出する第１の相違度算出部と、前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記第２の仮想空間における対象物の複数の姿勢毎に算出する第２の相違度算出部と、前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて前記第１の撮像座標系における前記対象物の姿勢を推定する姿勢推定部と、を備える姿勢検出装置である。
これにより、姿勢検出装置は、複数の異なる視点から得られた相違度それぞれに基づいて対象物の姿勢を推定できるので、姿勢の推定精度を向上させることができる。
【０００８】
（２）本発明の一態様は、上述の姿勢検出装置であって、前記姿勢推定部は、前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度のそれぞれに重み係数を乗じ、乗じることで得た値を基に前記第１の撮像座標系における前記対象物の姿前記対象物の姿勢を推定する。これにより、姿勢検出装置は、第１の撮像装置と第２の撮像装置のうち対象物の視認性がより悪い方の撮像装置の撮像画像から得られた相違度を軽視するように重み係数を乗じることで、対象物の姿勢を推定する際に視認性がより悪い方の撮像装置の撮像画像から得られた相違度が含む誤差が対象物の姿勢推定に与える影響を小さくすることができるので、姿勢の推定精度を向上させることができる。
【０００９】
（３）本発明の一態様は、上述の姿勢検出装置であって、前記第１の撮像装置が前記第２の撮像装置よりも前記対象物の視認性が良い場合、前記姿勢推定部は、前記対象物が目標の位置から予め決められた位置範囲にあるとき、前記第１の相違度に乗じる重み係数を前記第２の相違度に乗じる重み係数より大きくする。
これにより、姿勢推定部は、対象物が目標の位置から予め決められた位置範囲にあるとき、視認性がより悪い第２の撮像装置の撮像画像から得られた第２の相違度に係る重みが相対的に小さくなるので、第２の相違度が含む誤差が対象物の姿勢推定に与える影響を小さくすることができる。その結果、姿勢推定部は、対象物の姿勢推定の精度を向上させることができる。また、撮像装置同士の相対的な位置関係を算出する必要がないので、簡便に対象物の姿勢を推定することができる。
【００１０】
（４）また、本発明の一態様は、上述の姿勢検出装置であって、前記第１の撮像装置が前記第２の撮像装置よりも前記対象物の視認性が良い場合、前記姿勢推定部は、前記対象物が目標の位置から予め決められた位置範囲にあるとき、前記第２の相違度に乗じる重み係数が予め決められた範囲に収まるよう、前記対象物の移動開始時刻から前記対象物が目標の位置から予め決められた位置範囲に到達する時刻まで、前記第２の相違度に乗じる重み係数を変化させる。
これにより、対象物が目標の位置から予め決められた位置範囲にあるとき、視認性がより悪い第２の撮像装置の撮像画像から得られた第２の相違度に係る重みを予め決められた範囲に収まるようすることで、第２の相違度が含む誤差が対象物の姿勢推定に与える影響をより小さくすることができる。その結果、姿勢推定部は、対象物の姿勢推定の精度を向上させることができる。
【００１１】
（５）また、本発明の一態様は、上述の姿勢検出装置であって、前記第１の撮像装置が前記第２の撮像装置よりも前記対象物の視認性が良い場合、前記姿勢推定部は、前記対象物の移動開始時刻から前記対象物が目標の位置から予め決められた位置範囲に到達する時刻までの期間で、前記第２の相違度に乗じる重み係数を時間の経過とともに小さくする。
これにより、時間が経過するに連れて、視認性がより悪い第２の撮像装置の撮像画像から得られた第２の相違度に係る重みが小さくなるので、第２の相違度が含む誤差が対象物の姿勢推定に与える影響を小さくすることができる。その結果、姿勢推定部は、対象物の姿勢推定の精度を向上させることができる。
【００１２】
（６）また、本発明の一態様は、上述の姿勢検出装置であって、前記第１の撮像装置が前記第２の撮像装置よりも前記対象物の視認性が良く、前記対象物の位置が移動する場合、前記姿勢推定部は、前記対象物の単位時間あたりの移動量が小さくなるに従って前記第２の相違度に乗じる重み係数を小さくする。
ここで、対象物の単位時間あたりの移動量は、対象物を動かし始めたときが最も大きく、その後、時間の経過とともに徐々に小さくなるものである。このため、時間が経過するに連れて、第２の相違度に係る重みが小さくなるので、第２の相違度が含む誤差が対象物の姿勢推定に与える影響を小さくすることができる。その結果、姿勢推定部は、対象物の姿勢推定の精度を向上させることができる。
【００１３】
（７）また、本発明の一態様は、上述の姿勢検出装置であって、前記第１の撮像装置が前記第２の撮像装置よりも前記対象物の視認性が良い場合、前記姿勢推定部は、前記第２の相違度に乗じる重み係数を、前記対象物が目標の位置に近づくに従って小さくする。
これにより、対象物が目標の位置に近づくに従って、第２の相違度に係る重みが小さくなるので、第２の相違度が含む誤差が対象物の姿勢推定に与える影響を小さくすることができる。その結果、姿勢推定部は、対象物の姿勢推定の精度を向上させることができる。
【００１４】
（８）また、本発明の一態様は、上述の姿勢検出装置であって、前記姿勢推定部は、前記第１の相違度に乗じる重み係数と、前記第２の相違度に乗じる重み係数を、前記第１の撮像画像と前記第２の撮像画像のそれぞれに混入している雑音成分に応じて変化させる。これにより、姿勢推定部は、雑音成分が多い画像に対する重み係数を相対的に小さくすることができ、雑音成分が多い画像に対応する相違度の姿勢推定への寄与を相対的に小さくすることができるので、対象物の姿勢推定の精度を向上させることができる。
【００１５】
（９）また、本発明の一態様は、上述の姿勢検出装置であって、前記姿勢推定部は、前記第１の相違度に乗じる重み係数と、前記第２の相違度とに乗じる重み係数を、前記第１の撮像画像と前記第２の撮像画像のそれぞれに混入している正規化ランダムノイズの分散の逆数とする。これにより、正規化ランダムノイズの分散が大きいほどノイズが多いが、正規化ランダムノイズの分散が大きいほど重み係数が小さくなるので、ノイズが多いほど重み係数が小さくなる。その結果、ノイズが多い画像に対する重み係数を相対的に小さくすることができ、ノイズが多い画像に対応する相違度の姿勢推定への寄与を相対的に小さくすることができるので、対象物の姿勢推定の精度を向上させることができる。
【００１６】
（１０）また、本発明の一態様は、上述の姿勢検出装置であって、前記姿勢推定部は、前記第１の撮像装置で撮像した異なるフレーム間の画像の相違度を前記第１の撮像画像に混入している正規化ランダムノイズの分散として算出し、前記第２の撮像装置で撮像した異なるフレーム間の画像の相違度を前記第２の撮像画像に混入する正規化ランダムノイズの分散として算出する。これにより、フレーム間の画像の相違度を算出することで、異なるフレーム間に含まれるノイズ量の差を算出することができ、この異なるフレーム間に含まれるノイズ量の差で、正規化ランダムノイズの分散を近似することができる。これにより、フレーム間の画像の相違度が大きいほど正規化ランダムノイズの分散が大きくなり、重み係数が小さくなる。その結果、ノイズが多い画像に対する重み係数を相対的に小さくすることができ、ノイズが多い画像に対応する相違度の姿勢推定への寄与を相対的に小さくすることができるので、対象物の姿勢推定の精度を向上させることができる。
【００１７】
（１１）また、本発明の一態様は、上述の姿勢検出装置であって、前記第１の撮像装置を基準とする第１の撮像座標系で前記対象物の姿勢を表したときの座標を第２の撮像装置を基準とする第２の撮像座標系で前記対象物の姿勢を表したときの座標へ変換する変換規則に基づいて、前記第１の仮想空間における前記対象物の複数の異なる姿勢に対応する第２の仮想空間における前記対象物の複数の姿勢をそれぞれ取得する姿勢取得部を更に備え、前記第２の相違度算出部は、前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記姿勢取得部が算出した前記第２の仮想空間における対象物の複数の姿勢毎に算出する。これにより、対象物を第１の仮想空間と第２の仮想空間とで異なる視点から見たモデル画像について、それぞれ相違度を算出することができる。これにより、姿勢検出に向いた撮像装置が撮像した撮像画像から検出された姿勢に重きを置いて姿勢を推定できるので、対象物の姿勢推定の精度を向上させることができる。
【００１８】
（１２）また、本発明の一態様は、上述の姿勢検出装置であって、前記第１の相違度算出部は、前記第１の撮像装置が対象物を撮像することで得られた第１の撮像画像と、前記第１の仮想空間における前記対象物のモデル画像との相違度を示す第３の相違度を、前記第１の仮想空間における前記対象物の複数の異なる位置毎に算出し、前記第２の相違度算出部は、前記第２の撮像装置が対象物を撮像することで得られた第２の撮像画像と、前記第２の仮想空間における前記対象物のモデル画像との相違度を示す第４の相違度を、前記第２の仮想空間における前記対象物の複数の異なる位置毎に算出し、複数の前記第３の相違度と複数の前記第４の相違度に基づいて、前記第１の撮像座標系における前記対象物の位置を推定する位置推定部と、を備える。
これにより、第１の撮像画像を用いて算出された第３の相違度と第２の撮像画像を用いて算出された第４の相違度から位置を推定するので、一つの撮像装置で撮像した画像から位置を推定するよりも位置の推定精度を向上させることができる。
【００１９】
（１３）また、本発明の一態様は、上述の姿勢検出装置であって、前記位置推定部は、前記第１の相違度算出部が算出した複数の第３の相違度を参照して、前記第１の撮像座標系における撮像装置の光軸と直行する第１の平面の位置を推定し、前記第２の相違度算出部が算出した複数の第４の相違度を参照して、前記第２の撮像座標系における撮像装置の光軸と直行する第２の平面の位置を推定する平面位置推定部と、前記第１の平面の位置と前記第２の平面の位置とを参照して、前記第１の撮像座標系における前記対象物の奥行きを算出する奥行き算出部と、を備える。
これにより、第１の撮像画像を用いて算出された第３の相違度と第２の撮像画像を用いて算出された第３の相違度から、奥行きを算出するので、奥行きの推定精度を向上させることができる。また、平面位置推定部で推定された第１の平面の位置と、第２の平面の位置とを参照して第１の撮像装置の光軸方向の位置を算出するため、第１の撮像装置の光軸方向の位置の推定精度を向上させることができる。
【００２０】
（１４）また、本発明の一態様は、上述の姿勢検出装置であって、前記位置推定部は、複数の前記第３の相違度と複数の前記第４の相違度に基づいて、前記第１の撮像座標系における前記対象物の奥行きを推定する。これにより、第１の撮像画像を用いて算出された第３の相違度と第２の撮像画像を用いて算出された第４の相違度から奥行きを推定するので、一つの撮像装置で撮像した画像から奥行きを推定するよりも奥行きの推定精度を向上させることができる。
【００２１】
（１５）また、本発明の一態様は、上述の姿勢検出装置であって、前記姿勢推定部が推定した対象物の姿勢とそれ以前に推定した前記対象物の姿勢との差を参照して、推定が収束したか否か判定する判定部と、を備え、前記判定部が収束していないと判定した場合、前記第１の相違度算出部は、前記対象物のモデル画像の姿勢を予め決められたように変更した後のモデル画像と前記第１の撮像画像とに基づいて前記複数の第１の相違度を算出し、前記第２の相違度算出部は、前記対象物のモデル画像の姿勢を予め決められたように変更した後のモデル画像と前記第２の撮像画像とに基づいて前記複数の第２の相違度を算出し、前記姿勢推定部は、前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて、前記対象物の姿勢を推定し、前記判定部は、前記姿勢推定部が推定した前記対象物の姿勢を用いて判定を再び行う。
これにより、姿勢推定部は、対象物の姿勢とそれ以前に推定した対象物の姿勢との差が小さくなるまで、対象物の姿勢を更新するので、対象物の姿勢推定の精度を向上させることができる。
【００２２】
（１６）また、本発明の一態様は、上述の姿勢検出装置であって、前記対象物が目標の位置から予め決められた範囲にある際に、該対象物の位置または姿勢が変化する前後で前記第１の撮像装置が撮像して得た二つの画像間の変化量は、該対象物の位置または姿勢が変化する前後で前記第２の撮像装置が撮像して得た二つの画像間の変化量より大きい。
これにより、第１の撮像装置は、対象物が目標の位置から予め決められた範囲にある際に対象物の視認性が良く、かつ対象物が目標の位置から予め決められた範囲にある際に第１の相違度が姿勢の検出に貢献する割合が相対的に大きくなるので、対象物が目標の位置から予め決められた範囲にある際の対象物の姿勢精度を向上させることができる。
【００２３】
（１７）また、本発明の一態様は、上述の姿勢検出装置であって、前記対象物が目標の位置及び姿勢にあるときの該対象物を前記第１の撮像装置で撮影した第１のゴール画像と前記第２の撮像装置で撮影した第２のゴール画像とを取得するゴール画像取得部と、前記第１のゴール画像を撮影した空間を再現する第１の仮想空間において存在する対象物のモデルの位置及び姿勢から、予め決められた軸周りまたは軸方向に予め決められたように前記モデルを移動させて撮影した画像と、前記第１のゴール画像との相違度を示す第５の相違度を算出し、前記第２のゴール画像を撮影した空間を再現する第２の仮想空間において存在する対象物のモデルの位置及び姿勢から、予め決められた軸周りまたは軸方向に予め決められたように前記モデルを移動させて撮影した画像と、第２のゴール画像との相違度を示す第６の相違度を算出し、前記第５の相違度を算出した際の移動方向への前記第５の相違度の勾配と、前記第６の相違度を算出した際の移動方向への前記第６の相違度の勾配とを比較することで、姿勢推定の基準となる撮像装置を決定し、該決定した撮像装置を新たに前記第１の撮像装置とする位置決め基準撮像装置決定部と、を備える。
これにより、目標の位置及び姿勢付近において、決定された姿勢推定の基準となる撮像装置が撮影した画像から導出される相違度が姿勢推定に貢献する割合が大きくなる。その際、目標の位置及び姿勢付近において、別の撮像装置を基準としたときよりも相違度の変化が大きくなることで、対象物を目標の位置及び姿勢により早く到達させることができる。
【００２４】
（１８）また、本発明の一態様は、上述の姿勢検出装置であって、前記対象物が複数ある場合、前記第１の相違度算出部は、前記第１の仮想空間において位置姿勢推定の対象となる対象物以外の対象物全ての画像領域を特定し、該特定した画像領域を演算対象から除いて第１の相違度を算出し、前記第２の相違度算出部は、前記第２の仮想空間において位置姿勢推定の対象となる対象物以外の対象物全ての画像領域を特定し、該特定した画像領域を演算対象から除いて第２の相違度を算出する。これにより、対象物が複数あったとしても、姿勢推定の対象となる対象物以外の対象物の画像領域が相違度に貢献しないようにすることで、姿勢推定の対象となる対象物の画像領域だけから相違度を算出することができる。その結果、複数の対象物それぞれについて姿勢を推定することができる。
【００２５】
（１９）また、本発明の一態様は、上述の姿勢検出装置であって、前記推定された対象物の位置及び姿勢と前記対象物が目標の位置にある場合の位置及び姿勢とに基づいて、現在の時刻から予め決められた時間経過した後のロボット座標系における位置及び姿勢の変化量を決定する目標位置姿勢決定部と、ロボットを制御する時間間隔毎に、前記目標位置姿勢決定部が決定した変化量のうち最新の変化量に基づいて、前記時間間隔後のロボットの位置及び姿勢を決定し、該決定した位置及び姿勢になるようロボットを制御するロボット制御部と、を更に備え、前記ロボット制御部は、前記予め決められた時間のうち、自装置が第１の撮像画像及び第２の撮像画像から前記変化量を決定するまでの最短時間に基づいて決められた時間まではロボットが加速及び減速で移動するように前記ロボットの位置及び姿勢を決定し、その後の時間はロボットが等速で移動するように前記ロボットの位置及び姿勢を決定する。
これにより、第１の撮像画像及び第２の撮像画像から前記変化量を決定するまでの時間に揺らぎがあっても、第１の撮像画像及び第２の撮像画像の撮像開始から前記変化量を決定するまでの一連の画像処理が終了した時点では、常にロボットが等速で移動することになる。それにより、ロボット制御部はその等速で移動しているときの速度を初速としてロボットの加速制御または減速制御を始めることができる。そのため、ロボット制御部は、画像処理が終わると同時に、その画像処理で得られた変化量を用いてロボットを制御できるので、対象物を目標の位置及び目標の姿勢にするまでの時間を短縮することができる。
【００２６】
（２０）また、本発明の一態様は、第１の撮像装置が撮像した空間に対応する第１の仮想空間における対象物の複数の異なる姿勢毎に第１の相違度を算出する第１の相違度算出部と、前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が撮像した空間に対応する第２の仮想空間における対象物の複数の姿勢毎に第２の相違度を算出する第２の相違度算出部と、前記複数の第１の相違度と前記複数の第２の相違度とを参照して前記第１の撮像装置を基準とする第１の撮像座標系における前記対象物の姿勢を推定する姿勢推定部と、を備える姿勢検出装置である。
これにより、姿勢検出装置は、複数の異なる視点から得られた相違度それぞれに基づいて対象物の姿勢を推定できるので、姿勢の推定精度を向上させることができる。
【００２７】
（２１）また、本発明の一態様は、第１の撮像装置が対象物を撮像することで得られた第１の撮像画像と、第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における前記対象物の複数の異なる位置毎に算出する第３の相違度算出部と、第２の撮像装置が対象物を撮像することで得られた第２の撮像画像と、第２の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第２の仮想空間における前記対象物の複数の異なる位置毎に算出する第２の相違度算出部と、前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて第１の撮像座標系における前記対象物の位置を推定する位置推定部と、を備える位置検出装置である。
これにより、位置検出装置は、複数の異なる視点から得られた相違度それぞれに基づいて対象物の位置を推定できるので、位置の推定精度を向上させることができる。
【００２８】
（２２）また、本発明の一態様は、第１の撮像装置が対象物を撮像して得た第１の撮像画像と、前記第１の撮像装置が撮像した空間に対応する第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における対象物の複数の異なる姿勢毎に算出する第１の相違度算出部と、前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記第２の仮想空間における対象物の複数の姿勢毎に算出する第２の相違度算出部と、前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて前記第１の撮像座標系における前記対象物の姿勢を推定する姿勢推定部と、を備えるロボットである。
これにより、複数の異なる視点から得られた相違度それぞれに基づいて対象物の姿勢を推定できるので、姿勢の推定精度を向上させることができる。
【００２９】
（２３）また、本発明の一態様は、可動部を有するロボットと、第１の撮像装置が対象物を撮像して得た第１の撮像画像と、前記第１の撮像装置が撮像した空間に対応する第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における対象物の複数の異なる姿勢毎に算出する第１の相違度算出部と、前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記第２の仮想空間における対象物の複数の姿勢毎に算出する第２の相違度算出部と、前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて前記第１の撮像座標系における前記対象物の姿勢を推定する姿勢推定部と、を備えるロボットシステムである。
これにより、複数の異なる視点から得られた相違度それぞれに基づいて対象物の姿勢を推定できるので、姿勢の推定精度を向上させることができる。
【００３０】
（２４）また、本発明の一態様は、第１の相違度算出部が、第１の撮像装置が対象物を撮像して得た第１の撮像画像と、前記第１の撮像装置が撮像した空間に対応する第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における対象物の複数の異なる姿勢毎に算出する手順と、第２の相違度算出部が、前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記第２の仮想空間における対象物の複数の姿勢毎に算出する手順と、姿勢推定部が、前記第１の相違度算出部が算出した複数の第１の相違度と、前記第２の相違度算出部が算出した複数の第２の相違度とに基づいて前記第１の撮像座標系における前記対象物の姿勢を推定する手順と、を有する姿勢検出方法である。
これにより、複数の異なる視点から得られた相違度それぞれに基づいて対象物の姿勢を推定できるので、姿勢の推定精度を向上させることができる。
【００３１】
（２５）また、本発明の一態様は、コンピューターに、第１の撮像装置が対象物を撮像して得た第１の撮像画像と、前記第１の撮像装置が撮像した空間に対応する第１の仮想空間における前記対象物のモデル画像との相違度を示す第１の相違度を、前記第１の仮想空間における対象物の複数の異なる姿勢毎に算出する第１の相違度算出ステップと、前記第１の撮像装置とは異なる位置に配置された第２の撮像装置が対象物を撮像して得た第２の撮像画像と、前記第２の撮像装置が撮像した空間に対応する第２の仮想空間における前記対象物のモデル画像との相違度を示す第２の相違度を、前記姿勢取得ステップで算出された前記第２の仮想空間における対象物の複数の姿勢毎に算出する第２の相違度算出ステップと、前記第１の相違度算出ステップで算出された複数の第１の相違度と、前記第２の相違度算出ステップで算出された複数の第２の相違度とに基づいて前記第１の撮像座標系における前記対象物の姿勢を推定する姿勢推定ステップと、を実行させるためのプログラムである。
このプログラムを実行することで、複数の異なる視点から得られた相違度それぞれに基づいて対象物の姿勢を推定できるので、姿勢の推定精度を向上させることができる。
【図面の簡単な説明】
【００３２】
【図１】第１の実施形態におけるロボットの構成を示す概略ブロック図である。
【図２】対象物の位置及び姿勢を検出する際に用いる座標を説明するための図である。
【図３】第１のカメラ及び第２のカメラで撮影した画像の一例を示す図である。
【図４】第１の実施形態における制御装置の構成を示す概略ブロック図である。
【図５】第１の実施形態における位置姿勢検出部の構成を示す概略ブロック図である。
【図６】第１の実施形態における制御装置の処理全体の流れの一例を示すフローチャートである。
【図７】図４のステップＳ１０５及びＳ１０６で用いる対象物１０４の位置及び姿勢の検出処理の一例を示すフローチャートである。
【図８】図７の続きのフローチャートである。
【図９】図８の続きのフローチャートである。
【図１０】第１のカメラの重みｗＡと第２のカメラの重みｗＢの時間的変化の一例である。
【図１１】第２の実施形態における制御装置の構成を示す概略ブロック図である。
【図１２】第２の実施形態における制御装置の処理全体の流れの一例を示すフローチャートである。
【図１３】第３の実施形態における制御装置の構成を示す概略ブロック図である。
【図１４】ロボット座標系におけるロボット本体の手先位置のｘ座標の時間変化の一例である。
【図１５】第３の実施形態における制御装置の処理全体の流れの一例を示すフローチャートである。
【図１６】第３の実施形態におけるロボット制御部の処理の一例を示すフローチャートである。
【図１７】マスク画像について説明するための図である。
【発明を実施するための形態】
【００３３】
＜第１の実施形態＞
以下、本発明の実施形態について、図面を参照して詳細に説明する。
第１の実施形態におけるロボット１は、一例として、ステレオカメラを用いて、位置及び姿勢推定の対象となる対象物の位置及び姿勢の推定を行う。そして、ロボット１は、一例として、各カメラで撮影した画像と、対象物のＣＧ（Ｃｏｍｐｕｔｅｒ  Ｇｒａｐｈｉｃｓ）モデルを撮影した画像との相違度の加重平均を用いて対象物の位置及び姿勢を検出する。そして、ロボット１は、一例として、ビジュアルサーボを用いて対象物を所定の位置及び姿勢に制御する。
【００３４】
図１は、第１の実施形態におけるロボットの構成を示す概略ブロック図である。ロボット１は、制御装置（姿勢検出装置）１０１、ロボット本体１０２、グリッパ１０３、第１のカメラ（第１の撮像装置）１０５、及び第２のカメラ（第２の撮像装置）１０６を備える。ロボット本体１０２は、一例として垂直多関節型ロボットである。第１のカメラ１０５、第２のカメラ１０６は、ステレオカメラを構成する。同図は、グリッパ１０３で対象物１０４をつかんでいる際のロボット１である。同図において、ロボットの手先の位置（以下、ロボット手先位置ともいう）Ｐ１の一例が示されている。
【００３５】
同図において、ΣＲはロボットの座標であり、ΣＲにおける後述する第１のカメラ１０５の座標の姿勢を示す回転行列をＲ（ハット）ＲＡとする。ここで、Ｒ（ハット）は、Ｒの上に記号＾が付されたものである。
制御装置１０１は、ロボット本体１０２を制御する。
【００３６】
ロボット本体１０２は、制御装置１０１の制御に従って、グリッパ１０３で対象物１０４をつかむ。そして、ロボット本体１０２は、対象物１０４が目標となる位置及び姿勢をとるように、グリッパ１０３の位置（ロボット手先位置ともいう）及び姿勢をグリッパ１０３の目標位置及び目標姿勢に変更する。
ロボット本体１０２は一例として６個の関節を含む。なお、ロボット本体１０２の関節は５個以下でも７個以上であってもよい。本実施形態では、一例として、制御装置１０１は、対象物１０４を目標の位置及び姿勢をとるようにロボット本体１０２を制御することで、対象物１０４を搬送する例について説明する。なお、対象物１０４の組み立て作業なども同様の処理で行わせることが可能である。
【００３７】
本実施形態では、ステレオカメラの配置として並行ステレオのようにそれぞれのカメラの座標軸の向きを一致させるように配置するよりも、互いが補完し合う関係になるように、互いのカメラ座標の軸の向きが一致しないように配置する（互いに違う方向から対象物１０４を撮影するように配置する）とよい。
【００３８】
図２は、対象物１０４の位置及び姿勢を検出する際に用いる座標を説明するための図である。同図において、ΣＡは第１のカメラ１０５を基準とする座標（以下、第１のカメラ座標ともいう）、ΣＢは第２のカメラ１０６を基準とする座標（以下、第２のカメラ座標ともいう）であり、Ｔ（アロー）ＢＡ及びＲ（ハット）ＢＡはそれぞれ第２のカメラ座標ΣＢにおける第１のカメラ座標ΣＡの位置を示す位置ベクトル、第２のカメラ座標ΣＢにおける第１のカメラ座標ΣＡの姿勢を示す回転行列である。ここで、Ｔ（アロー）は、Ｔの上に記号→が付されたものである。Ｔ（アロー）Ａ及びＲ（ハット）Ａは、それぞれ第１のカメラ座標ΣＡにおける対象物１０４の位置を示す位置ベクトル、第１のカメラ座標ΣＡにおける対象物１０４の姿勢を示す回転行列である。Ｔ（アロー）Ｂ及びＲ（ハット）Ｂは、それぞれΣＢにおける対象物１０４の位置を示す位置ベクトル、ΣＢにおける対象物１０４の姿勢を示す回転行列である。
【００３９】
図３は、第１のカメラ１０５及び第２のカメラ１０６で撮影した画像の一例を示す図である。同図において、第１のカメラ１０５で撮影した画像Ｇ３０１と、第２のカメラ１０６で撮影した画像Ｇ３０２が示されている。第１のカメラ１０５で撮影した画像Ｇ３０１の座標は該画像Ｇ３０１の左上隅を原点として横軸をｕＡ、縦軸をｖＡとする。第２のカメラ１０６で撮影した画像Ｇ３０２の座標は該画像Ｇ３０２の左上隅を原点として横軸をｕＢ、縦軸をｕＢとする。尚、第１のカメラ１０５で撮影した画像３０１及び第２のカメラ１０６で撮影した画像３０２中の実線で描かれた直方体は現在の対象物１０４の画像領域を示している。
【００４０】
図４は、第１の実施形態における制御装置１０１の構成を示す概略ブロック図である。制御装置１０１は、入力部１１０、処理部１２０、及び記憶部１９０を備える。
入力部１１０は、ユーザからの入力（例えば、設定項目に関する事項）を受け付け、受け付けた入力を示す入力情報を処理部１２０に出力する。
記憶部１９０には、処理部１２０が読み出して実行するための各種のプログラムが記憶されている。
【００４１】
処理部１２０は、入力部１１０から入力された入力情報を記憶部１９０に記憶させる。処理部１２０は、記憶部１９０に記憶されているプログラムを読み出して実行する。処理部１２０は、変換規則決定部１３０と、ゴール画像取得部１４０と、現在画像取得部１４５と、位置姿勢検出部１５０と、ロボット制御部１６０と、第１判定部１７０とを備える。
【００４２】
変換規則決定部１３０は、第１のカメラ１０５を基準とする第１の撮像座標系で対象物の姿勢を表したときの座標を第２のカメラ１０６を基準とする第２の撮像座標系で対象物１０４の姿勢を表したときの座標へ変換する（例えば、回転行列）を決定する。変換規則決定部１３０は、決定した変換規則を示す変換規則情報を位置姿勢検出部１５０に出力する。
【００４３】
本実施形態では、一例として、対象物１０４が目標の位置から予め決められた範囲にある際の対象物１０４の視認性は、第２のカメラ１０６より第１のカメラ１０５の方が良いものとする。ここで、視認性とは、対象物１０４の位置または姿勢が変化した際の画像の変化量である。視認性が良いカメラは、対象物１０４の位置または姿勢が微小に変化した場合であっても、その変化の前後における画像間の変化量が大きい。これにより、視認性が良いカメラは、対象物１０４の位置または姿勢の微小変化を、より画像の違いとして捉えることができる。上述したことから、本実施形態では、一例として、対象物１０４が目標の位置から予め決められた範囲にある際に、該対象物１０４の位置または姿勢が変化する前後で第１のカメラ１０５が撮像して得た二つの画像間の変化量は、該対象物１０４の位置または姿勢が変化する前後で第２のカメラ１０６が撮像して得た二つの画像間の変化量より大きい。
【００４４】
ゴール画像取得部１４０は、対象物１０４が目標の位置及び姿勢にあるときに、第１のカメラ１０５が撮影した画像（以下、ゴール画像ともいう）を生成する。ゴール画像取得部１４０は、生成したゴール画像を示すゴール画像情報を位置姿勢検出部１５０へ出力する。
現在画像取得部１４５は、第１のカメラ１０５に現在の対象物１０４を撮像させ、撮像して得られた第１の画像を取得する。また、現在画像取得部１４５は、第２のカメラ１０６に現在の対象物１０４を撮像させ、撮像して得られた第２の画像を取得する。現在画像取得部１４５は、取得した第１の画像を示す第１画像情報と第２の画像を示す第２画像情報を位置姿勢検出部１５０へ出力する。
【００４５】
位置姿勢検出部１５０は、変換規則決定部１３０が決定した変換規則とゴール画像取得部１４０が生成したゴール画像を参照して、対象物１０４が目標の位置及び姿勢にあるとき（ゴール状態）の対象物１０４の位置を示す位置ベクトルＴ（アロー）ｇＡ及び姿勢Ｒ（ハット）ｇＡを検出する。
また、位置姿勢検出部１５０は、変換規則決定部１３０が決定した変換規則と現在画像取得部１４５から入力された第１画像情報と第２画像情報を参照して、現在の対象物１０４の位置を示す位置ベクトルＴ（アロー）ｃＡ及び姿勢Ｒ（ハット）ｃＡを検出する。位置姿勢検出部１５０は、検出したゴール状態の対象物１０４の位置を示す位置ベクトルＴ（アロー）ｇＡ及び姿勢Ｒ（ハット）ｇＡを示す情報と、現在の対象物１０４の位置を示す位置ベクトルＴ（アロー）ｃＡ及び姿勢Ｒ（ハット）ｃＡを示す情報をロボット制御部１６０に出力する。
【００４６】
ロボット制御部１６０は、位置姿勢検出部１５０から入力されたゴール状態の対象物１０４の位置を示す位置ベクトルＴ（アロー）ｇＡと現在の対象物１０４の位置を示す位置ベクトルＴ（アロー）ｃＡとを参照して、グリッパ１０３を並進方向に移動させる。また、ロボット制御部１６０は、ゴール状態の対象物１０４の姿勢Ｒ（ハット）ｇＡと現在の対象物１０４の姿勢Ｒ（ハット）ｃＡとを参照して、グリッパ１０３を回転方向に移動させる。
【００４７】
第１判定部１７０は、現在の対象物１０４の位置が、ゴール状態の対象物１０４の位置に到達したか否か、及び現在の対象物１０４の姿勢が、ゴール状態の対象物１０４の姿勢に到達したか否か判定する。
第１判定部１７０は、現在の対象物１０４の位置が、ゴール状態の対象物１０４の位置に到達し、かつ現在の対象物１０４の姿勢が、ゴール状態の対象物１０４の姿勢に到達した場合、対象物１０４がゴール状態に到達したと判定する。一方、それ以外の場合、第１判定部１７０は、ゴール状態に到達していないと判定し、第１のカメラ座標における現在の対象物１０４の位置を示す位置ベクトルＴ（アロー）ｃＡ及び姿勢を示す回転行列Ｒ（ハット）ｃＡを位置姿勢検出部１５０に検出させ、処理を続行させる。
【００４８】
図５は、第１の実施形態における位置姿勢検出部１５０の構成を示す概略ブロック図である。位置姿勢検出部１５０は、第１の相違度算出部１５１、姿勢取得部１５２、第２の相違度算出部１５３、姿勢推定部１５４、位置推定部１５６及び第２判定部１５９を備える。位置推定部１５６は、平面位置推定部１５７及び奥行き更新部１５８を備える。まず、位置姿勢検出部１５０が備える各部の処理概要について説明した後に、後述するフローチャートを用いて各部の詳細な処理について説明する。
【００４９】
第１の相違度算出部１５１は、第１のカメラ１０５が姿勢推定の対象となる対象物１０４を撮像して得た第１の撮像画像と、第１のカメラ１０５が撮像した空間に対応する第１の仮想空間における対象物１０４のモデル画像との相違度を示す第１の相違度を、第１の仮想空間における対象物の複数の異なる姿勢毎に算出する。本実施系形態では一例として、モデル画像はコンピューターグラフィック画像（ＣＧ画像）である。第１の相違度算出部１５１は、算出した複数の第１の相違度を示す情報を姿勢取得部１５２と姿勢推定部１５４へ出力する。ここで、ある軸方向またはある軸周りの相違度の変化量が大きいほど、視認性が高い。
【００５０】
姿勢取得部１５２は、変換規則決定部１３０が決定した変換規則に基づいて、第１の仮想空間における対象物１０４の複数の異なる姿勢に対応する第２の仮想空間における対象物１０４の複数の姿勢をそれぞれ取得する。姿勢取得部１５２は、算出した複数の姿勢を示す情報を第２の相違度算出部１５３へ出力する。
【００５１】
第２の相違度算出部１５３は、第２のカメラ１０６が対象物１０４を撮像して得た第２の撮像画像と、第２のカメラ１０６が撮像した空間に対応する第２の仮想空間における対象物１０４のモデル画像との相違度を示す第２の相違度を、姿勢取得部１５２が算出した第２の仮想空間における対象物１０４の複数の姿勢毎に算出する。第２の相違度算出部１５３は、算出した複数の第２の相違度を示す情報を姿勢推定部１５４へ出力する。
【００５２】
姿勢推定部１５４は、第１の相違度算出部１５１が算出した複数の第１の相違度と、第２の相違度算出部１５３が算出した複数の第２の相違度とを参照して、対象物１０４の姿勢を推定する。また、姿勢推定部１５４は、第１のカメラ１０５が第２のカメラ１０６よりも対象物１０４の視認性が良い場合に、対象物１０４が目標の位置から予め決められた位置範囲にあるとき（例えば、対象物１０４が目標の位置にあるとき）に、対象物１０４の姿勢の推定における第１の相違度に係る重みを第２の相違度に係る重みより大きくする。ここで視認性が良いとは、対象物１０４の位置または姿勢が変化した際の画像の変化量が大きいことを示し、第１のカメラ１０５が第２のカメラ１０６よりも対象物１０４の視認性が良い場合とは、対象物１０４の位置または姿勢が変化した際の第１画像の変化量が第２画像の変化量よりも大きいことを意味する。ここで、対象物１０４が目標の位置から予め決められた位置範囲にあるときには、対象物１０４が完全に停止しない状態も含まれる。姿勢推定部１５４は、推定した対象物１０４の姿勢を示す情報を第２判定部１５９へ出力する。
第１の相違度算出部１５１は、第１のカメラ１０５が対象物を撮像することで得られた第１の撮像画像と、第１の仮想空間における対象物１０４のモデル画像との相違度を示す第３の相違度を、第１の仮想空間における対象物１０４の複数の異なる位置毎に算出する。そして、第１の相違度算出部１５１は、算出した複数の第４の相違度を示す情報を平面位置推定部１５７と奥行き更新部１５８へ出力する。
【００５３】
第２の相違度算出部１５３は、第２のカメラ１０６が対象物を撮像することで得られた第２の撮像画像と、第２の仮想空間における対象物１０４のモデル画像との相違度を示す第４の相違度を、第２の仮想空間における対象物１０４の複数の異なる位置毎に算出する。そして、第２の相違度算出部１５３は、算出した複数の第４の相違度を示す情報を奥行き更新部１５８へ出力する。
【００５４】
平面位置推定部１５７は、第１の相違度算出部１５１が算出した複数の第３の相違度を参照して、第１の撮像座標系における対象物１０４の水平位置及び垂直位置を更新する。平面位置推定部１５７は、例えば、更新後の水平位置及び垂直位置を示す情報を奥行き更新部１５８へ出力する。
【００５５】
奥行き更新部１５８は、第１の相違度算出部１５１が算出した複数の第３の相違度と、第２の相違度算出部１５３が算出した複数の第４の相違度と、平面位置推定部１５７から入力された更新後の水平位置及び垂直位置とを参照して、第１の撮像座標系における対象物１０４の奥行きを更新する。奥行き更新部１５８は、更新後の奥行きを示す情報と平面位置推定部１５７から入力された更新後の水平位置及び垂直位置を示す情報を第２判定部１５９へ出力する。
【００５６】
第２判定部１５９は、姿勢推定部１５４が推定した対象物の姿勢とそれ以前に推定した対象物１０４の姿勢との差を参照して、推定が収束したか否か判定する。更に、第２判定部１５９は、奥行き更新部１５８から入力された更新後の水平位置及び垂直位置とそれ以前に推定した対象物１０４の水平位置及び垂直位置とのそれぞれの差も参照して、推定が収束したか否か判定する。更に、第２判定部１５９は、奥行き更新部１５８から入力された更新後の奥行きとそれ以前に推定した奥行きとのそれぞれの差も参照して、推定が収束したか否か判定する。
【００５７】
第２判定部１５９が収束していないと判定した場合、各部は以下の処理を実行する。第１の相違度算出部１５１は、対象物１０４のモデル画像の姿勢を予め決められたように変更した後のモデル画像と第１の撮像画像とに基づいて、複数の第１の相違度を算出する。更に、第２の相違度算出部１５３は、対象物１０４のモデル画像の姿勢を予め決められたように変更した後のモデル画像と第２の撮像画像とに基づいて、複数の第２の相違度を算出する。そして、姿勢推定部１５４は、第１の相違度算出部１５１が算出した複数の第１の相違度と、第２の相違度算出部１５３が算出した複数の第２の相違度とに基づいて、対象物１０４の姿勢を更新する。第２判定部１５９は、姿勢推定部１５４が更新した対象物１０４の姿勢を用いて判定を再び行う。これらの処理の詳細は後述するフローチャートで説明する。
【００５８】
一方、第２判定部１５９が収束したと判定した場合で、推定対象がゴール状態の対象物１０４の位置を示す位置ベクトルＴ（アロー）ｇＡ及び姿勢を示す回転行列Ｒ（ハット）ｇＡの場合、第２判定部１５９は、姿勢推定部１５４が推定した対象物１０４の姿勢をゴール状態の対象物１０４の姿勢を示す回転行列Ｒ（ハット）ｇＡとして、ロボット制御部１６０へ出力する。また、平面位置推定部１５７が出力した更新後の水平位置及び垂直位置と奥行き更新部１５８が出力した更新後の奥行きとをあわせて、ゴール状態の対象物１０４の位置Ｔ（アロー）ｇＡとして、ロボット制御部１６０へ出力する。
【００５９】
第２判定部１５９が収束したと判定した場合で、推定対象が現在の対象物１０４の位置Ｔ（アロー）ｃＡ及び姿勢を示す回転行列Ｒ（ハット）ｃＡの場合、第２判定部１５９は、姿勢推定部１５４が推定した対象物１０４の姿勢を現在の対象物１０４の姿勢を示す回転行列Ｒ（ハット）ｃＡとして、ロボット制御部１６０へ出力する。また、平面位置推定部１５７が出力した更新後の水平位置及び垂直位置と奥行き更新部１５８が出力した更新後の奥行きとをあわせて、現在の対象物１０４の位置Ｔ（アロー）ｃＡとして、ロボット制御部１６０へ出力する。
【００６０】
続いて、図６は、第１の実施形態における制御装置１０１の処理全体の流れの一例を示すフローチャートである。
（ステップＳ１０１）まず、処理部１２０は、ロボット座標ΣＲにおける第１のカメラ座標ΣＡの姿勢を示す回転行列Ｒ（ハット）ＲＡを推定する。具体的には、例えば、検出が容易なマーカーをグリッパ１０３で把持させて、ロボット座標ΣＲの各軸方向に移動させ、処理部１２０は、後述する対象物１０４の位置検出処理と同じ処理を用いて、第１のカメラ座標ΣＡにおけるマーカーの位置を検出する。処理部１２０は、検出したマーカーの位置を用いてロボット座標ΣＲにおける第１のカメラ座標ΣＡの姿勢を示す回転行列Ｒ（ハット）ＲＡを推定する。
【００６１】
（ステップＳ１０２）次に、変換規則決定部１３０は、第２のカメラ座標ΣＢにおける第１のカメラ座標ΣＡの原点の位置を示す位置ベクトルＴ（アロー）ＢＡを推定する。また、変換規則決定部１３０は、第２のカメラ座標ΣＢにおける第１のカメラ座標ΣＡの姿勢を示す回転行列Ｒ（ハット）ＢＡを推定する。具体的には、例えば、変換規則決定部１３０は、既知の模様が描かれた対象物１０４を第１のカメラ１０５及び第２のカメラ１０６で撮影させ、撮像で得た二つの画像から、後述する図７〜９で示される位置及び姿勢の検出処理で位置ベクトルＴ（アロー）ＢＡと回転行列Ｒ（ハット）ＢＡを推定してもよい。
【００６２】
（ステップＳ１０３）次に、ゴール画像取得部１４０は、対象物１０４が目標の位置及び姿勢にあるときの第１のカメラ１０５が撮像する画像を生成する。具体的には、本実施形態では、一例として対象物１０４の搬送作業を想定しているので、ゴール画像取得部１４０は、一例として、搬送先における位置及び姿勢に対象物１０４があるときの第１のカメラ１０５が撮像する画像を生成してもよい。この際、ゴール画像取得部１４０は、実際に実空間上で対象物１０４を移動させた後に当該画像を取得してもよいし、仮想空間上で対象物１０４のＣＧモデルを移動させた後に当該画像を取得してもよい。
なお、組み立て作業であれば組み立て後の位置及び姿勢に対象物１０４があるときの第１のカメラ１０５における画像を生成してもよい。
【００６３】
同図において、ステップＳ１０１及びＳ１０２はキャリブレーション作業であり少なくとも１回実施されれば良い。また、ステップＳ１０３は教示処理であり、処理内容が変わるのに応じて少なくとも１回実施されれば良い。次に説明するステップＳ１０４〜Ｓ１０８は毎回実行される処理である。
【００６４】
（ステップＳ１０４）次に、ロボット制御部１６０は、対象物１０４を第１のカメラ１０５及び第２のカメラ１０６の視野内に移動させる。
（ステップＳ１０５）次に、位置姿勢検出部１５０は、後述する検出方式を用いて、ステップＳ１０３で作成した画像から第１のカメラ座標における対象物１０４の目標の位置（ゴール位置ともいう）を示す位置ベクトルＴ（アロー）ｇＡ及び目標の姿勢（ゴール姿勢ともいう）を示す回転行列Ｒ（ハット）ｇＡを検出する。
なお、ステップＳ１０３において、ゴール画像取得部１４０が仮想空間上で対象物１０４のＣＧモデルを移動させてゴール画像を取得した場合、仮想空間における位置及び姿勢は既知であるため、位置姿勢検出部１５０はそれらを目標の位置及び姿勢として設定してもよい。
【００６５】
（ステップＳ１０６）次に、第１のカメラ１０５及び第２のカメラ１０６を用いて、現在の第１のカメラ座標における対象物１０４の位置を示す位置ベクトルＴ（アロー）ｃＡ及び姿勢を示す回転行列Ｒ（ハット）ｃＡを検出する。この処理方式は後に詳述する。
（ステップＳ１０７）次に、ロボット制御部１６０は、ステップＳ１０１で取得したロボット座標ΣＲにおける第１のカメラ座標ΣＡの姿勢を示す回転行列Ｒ（ハット）ＲＡを用いて、ロボットの手先についての並進方向の移動量及び回転方向の移動量を算出する。そして、ロボット制御部１６０は、ロボットの手先を算出した並進方向の移動量だけ並進方向へ移動させ、算出した回転方向の移動量Δθ（アロー）Ｒだけ回転方向へ移動させる。例えば、ロボット制御部１６０は、並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒを次の式（１）に従って算出する。但し、ロボット制御部１６０は、例えば回転方向の移動量Δθ（アロー）Ｒの各要素Δθｘ、Δθｙ、Δθｚを、後述する式（５）を用いて算出する。
【００６６】
【数１】
2014161937.tif 000003
【００６７】
ここで、θ（アロー）ｃＲ及びθ（アロー）ｇＲは、それぞれ推定によって得られた角度θ（アロー）ｇＡ及びθ（アロー）ｃＡの回転行列Ｒ（ハット）Ａ（θ（アロー）ｃＡ）及びＲ（ハット）Ａ（θ（アロー）ｇＡ）を、それぞれＲ（ハット）ｃＲ＝Ｒ（ハット）ＲＡＲ（ハット）Ａ（θ（アロー）ｃＡ）及びＲ（ハット）ｇＲ＝Ｒ（ハット）ＲＡＲ（ハット）Ａ（θ（アロー）ｇＡ）に従って、ロボット座標に変換し、後述する式（５）を用いて算出された角度である。
（ステップＳ１０８）次に、第１判定部１７０は、ステップＳ１０７でロボット制御部１６０が算出した並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒが予め決められた値より小さいか否かを判定する。一方、予め決められた値より小さい場合（ＹＥＳ）、第１判定部１７０は対象物１０４がステップＳ１０３で作成した画像における対象物１０４の位置及び姿勢に到達したと判断してその処理を終了する。予め決められた値以上の場合（ＮＯ）、第１判定部１７０は目標の位置及び姿勢に到達していないと判断し、ステップＳ１０６に処理を戻す。以上で、本フローチャートの説明を終了する。
【００６８】
図７は、図４のステップＳ１０２、Ｓ１０５及びＳ１０６で用いる対象物１０４の位置及び姿勢の検出処理の一例を示すフローチャートである。図８は、図７の続きのフローチャートである。図９は、図８の続きのフローチャートである。
（ステップＳ２０１）まず、位置姿勢検出部１５０は、第１のカメラ座標及び第２のカメラ座標における対象物１０４の初期検出位置を示す位置ベクトルＴ（アロー）Ａ［ｘＡ  ｙＡ  ｚＡ］、Ｔ（アロー）Ｂ［ｘＢ  ｙＢ  ｚＢ］を設定する。搬送作業の処理フローが予め決められている場合、例えば、入力部１１０がユーザからの初期検出位置の入力を受け付ける。そして、例えば、位置姿勢検出部１５０が、入力部１１０が受け付けた初期検出位置が示す位置に上述の位置ベクトルを設定する。
【００６９】
（ステップＳ２０２）次に、姿勢取得部１５２は、第１のカメラ座標における対象物１０４の初期検出姿勢を示す回転行列Ｒ（ハット）Ａを各軸方向の回転角度から算出する。具体的には、例えば、姿勢取得部１５２は、対象物１０４の姿勢の表現としてｘｙｚオイラー角表現を用いて、回転行列Ｒ（ハット）Ａを次の式（２）に従って算出する。
【００７０】
【数２】
2014161937.tif 000004
【００７１】
（ステップＳ２０３）次に、姿勢取得部１５２は、例えば、第１のカメラ座標における対象物１０４の検出姿勢Ｒ（ハット）Ａから第２のカメラ座標における姿勢を示す回転行列Ｒ（ハット）Ｂ（＝Ｒ（ハット）ＢＡＲ（ハット）Ａ）を次の式（３）に従って算出する。
【００７２】
【数３】
2014161937.tif 000005
【００７３】
（ステップＳ２０４）次に、第１の相違度算出部１５１は、例えば、第１のカメラ１０５で撮影した空間を再現する第１の仮想空間において、対象物１０４のＣＧモデルを位置Ｔ（アロー）Ａ及び姿勢Ｒ（ハット）Ａから各軸正方向にΔ微小移動させて撮影した画像と、第１のカメラ１０５で撮影した画像との相違度Ｅ＋ｘＡＡ、Ｅ＋ｙＡＡ、Ｅ＋ｚＡＡ、Ｅ＋θｘＡＡ、Ｅ＋θｙＡＡ、Ｅ＋θｚＡＡを算出する。ここで、相違度Ｅ＋θｘＡＡ、Ｅ＋θｙＡＡ、Ｅ＋θｚＡＡは第１の相違度で、相違度Ｅ＋ｘＡＡ、Ｅ＋ｙＡＡ、Ｅ＋ｚＡＡは第３の相違度である。相違度の一例として、Ｓｕｍ  ｏｆ  Ｓｑｕａｒｅｄ  Ｄｉｆｆｅｒｅｎｃｅ（ＳＳＤ）を採用すると、第１の相違度算出部１５１は、各相違度を次の式（４）に従って算出する。
【００７４】
【数４】
2014161937.tif 000006
【００７５】
ここで、Ｗは画像の横方向の画素数、Ｈは画像の縦方向の画素数、Ｉ（ｕ，ｖ）は片方の画像の座標（ｕ，ｖ）における階調、Ｊ（ｕ，ｖ）はもう一方の画像の座標（ｕ，ｖ）における階調である。
（ステップＳ２０５）次に、第１の相違度算出部１５１は、第１のカメラ１０５で撮影した空間を再現する第１の仮想空間において、対象物１０４のＣＧモデルを位置Ｔ（アロー）Ａ及び姿勢Ｒ（ハット）Ａから各軸負方向にΔ微小移動させて撮影した画像と、第１のカメラ１０５で撮影した画像との相違度Ｅ−ｘＡＡ、Ｅ−ｙＡＡ、Ｅ−ｚＡＡ、Ｅ−θｘＡＡ、Ｅ−θｙＡＡ、Ｅ−θｚＡＡを算出する。ここで、相違度Ｅ−θｘＡＡ、Ｅ−θｙＡＡ、Ｅ−θｚＡＡは第１の相違度で、相違度Ｅ−ｘＡＡ、Ｅ−ｙＡＡ、Ｅ−ｚＡＡは第３の相違度である。
【００７６】
（ステップＳ２０６）次に、第２の相違度算出部１５３は、例えば、第２のカメラ１０６で撮影した空間を再現する第２の仮想空間において、対象物１０４のＣＧモデルを位置Ｔ（アロー）Ｂから各軸正方向にΔ微小移動させる。また、第２の相違度算出部１５３は、例えば、ステップＳ２０４における姿勢Ｒ（ハット）Ｂの微小移動量を第２のカメラ座標における微小移動量へ式（３）に従って変換する。そして、第２の相違度算出部１５３は、例えば、変換後の微小移動量に従って上記ＣＧモデルを各軸方向に微小移動させて撮影した画像と、第２のカメラ１０６で撮影した画像との相違度Ｅ＋ｘＢＢ、Ｅ＋ｙＢＢ、Ｅ＋ｚＢＡ、Ｅ＋θｘＢＢ、Ｅ＋θｙＢＢ、Ｅ＋θｚＢＢを算出する。ここで、相違度Ｅ＋θｘＢＢ、Ｅ＋θｙＢＢ、Ｅ＋θｚＢＢは第２の相違度で、相違度Ｅ＋ｘＢＢ、Ｅ＋ｙＢＢ、Ｅ＋ｚＢＡは第４の相違度である。また、第２の相違度算出部１５３は、例えば各軸方向の回転角度を次の式（５）に従って算出する。
【００７７】
【数５】
2014161937.tif 000007
【００７８】
ここで、［Ｒ（ハット）］ｉｊは回転行列Ｒ（ハット）のｉ行ｊ列成分を表す。
（ステップＳ２０７）次に、第２の相違度算出部１５３は、例えば、第２のカメラ１０６で撮影した空間を再現する第２の仮想空間において、対象物１０４のＣＧモデルを位置Ｔ（アロー）Ｂから各軸負方向にΔ微小移動させる。また、第２の相違度算出部１５３は、例えば、Ｓ２０５における姿勢Ｒ（ハット）Ａの微小移動を第２のカメラ座標における微小移動量へ変換する。そして、第２の相違度算出部１５３は、例えば、変換後の微小移動量に従って上記ＣＧモデルを各軸方向に微小移動させて撮影した画像と、第２のカメラ１０６で撮影した画像との相違度Ｅ−ｘＢＢ、Ｅ−ｙＢＢ、Ｅ−ｚＢＡ、Ｅ−θｘＢＢ、Ｅ−θｙＢＢ、Ｅ−θｚＢＢを算出する。ここで、相違度Ｅ−θｘＢＢ、Ｅ−θｙＢＢ、Ｅ−θｚＢＢは第２の相違度で、相違度Ｅ−ｘＢＢ、Ｅ−ｙＢＢ、Ｅ−ｚＢＡは第４の相違度である。
【００７９】
（ステップＳ２０８）次に、平面位置推定部１５７は、例えば、第１のカメラ１０５において、対象物１０４がＴ（アロー）ＡにあるときのｘＡ及びｙＡ方向それぞれの相違度の勾配を、次の式（６）に従って算出する。
【００８０】
【数６】
2014161937.tif 000008
【００８１】
（ステップＳ２０９）次に、平面位置推定部１５７は、例えば、第１のカメラ１０５における対象物１０４のｘＡ、及びｙＡ軸方向の検出位置を次の式（７）に従って更新する。ここで式（７）は、最急降下法による更新処理を表す。
【００８２】
【数７】
2014161937.tif 000009
【００８３】
ここで、Ｃは定数であって適宜設定される。
（ステップＳ２１０）次に、平面位置推定部１５７は、例えば、第２のカメラ１０６において、対象物１０４がＴ（アロー）ＢにあるときのｘＢ及びｙＢ軸方向それぞれの相違度の勾配を次の式（８）に従って算出する。
【００８４】
【数８】
2014161937.tif 000010
【００８５】
（ステップＳ２１１）次に、平面位置推定部１５７は、例えば、第２のカメラ１０６における対象物１０４のｘＢ及びｙＢ軸方向の検出位置を次の式（９）に従って更新する。ここで式（９）は、最急降下法による更新処理を表している。
【００８６】
【数９】
2014161937.tif 000011
【００８７】
ここで、Ｃは定数であって適宜設定される。なお、ｘＡ軸方向の検出位置を更新する際には、定数Ｃは例えば、次の式（１０）で表されてもよい。
【００８８】
【数１０】
2014161937.tif 000012
【００８９】
ここで、Δは対象物をｘＡ軸方向に微小移動させた場合の距離である。この場合、定数Ｃにｘ軸方向の相違度Ｅの勾配を乗じた値は、次の式（１１）から−Δ〜Δの範囲に収まる。
【００９０】
【数１１】
2014161937.tif 000013
【００９１】
これにより、更新量が−Δ〜Δの範囲であるので、平面位置推定部１５７は、水平位置を更新する際に、更新とともに水平位置が振動するのを低減することができる。
【００９２】
（ステップＳ２１２）次に、奥行き更新部１５８は、第１のカメラ１０５における対象物１０４のｚＡ軸方向の検出位置ｚＡを、三角測量を利用して例えば次の式（１２）に従って算出する。
【００９３】
【数１２】
2014161937.tif 000014
【００９４】
上述した処理をまとめると、以下の通りである。平面位置推定部１５７は、第１の相違度算出部が算出した複数の第３の相違度を参照して、前記第１の撮像座標系におけるカメラの光軸と直行する第１の平面の位置（ｘＡ，ｙＡ）を推定する。平面位置推定部１５７は、第２の相違度算出部が算出した複数の第４の相違度を参照して、前記第２の撮像座標系におけるカメラの光軸と直行する第２の平面の位置（ｘＢ，ｙＢ）を推定する。奥行き更新部１５８は、平面位置推定部１５７が推定した第１の平面の位置（ｘＡ，ｙＡ）と第２１の平面の位置（ｘＢ，ｙＢ）とを参照して、第１の撮像座標系における対象物の奥行きｚＡを算出する。
なお、奥行き更新部１５８は、第２のカメラ１０６における対象物１０４のｚＢ軸方向の検出位置ｚＢを次の式（１３）に従って、算出してもよい。
【００９５】
【数１３】
2014161937.tif 000015
【００９６】
そして、奥行き更新部１５８は、更新結果とステップＳ２０９の算出結果であるｘＡ、ｙＡとを合わせて第１のカメラ１０５における検出位置をＴ（アロー）Ａ＝［ｘＡ  ｙＡ  ｚＡ］Ｔと設定する。ここで、本ステップＳ２１２では３次元復元によって得られた３次元位置のうちｚＡ軸方向のみを検出位置に反映させている。これは、３次元復元を行う際に第２のカメラ座標ΣＢにおける第２のカメラ座標ΣＡの姿勢を示す回転行列Ｒ（ハット）ＢＡを用いているが、回転行列Ｒ（ハット）ＢＡは誤差を含んでいる可能性が高い。そのため、その誤差を加味しても精度の向上が見込まれるｚＡ軸方向のみ第１のカメラ１０５における検出位置Ｔ（アロー）Ａに反映させることで、検出精度の低下を防ぐことができる。
【００９７】
（ステップＳ２１３）次に、奥行き更新部１５８は、ステップＳ２１２で設定した第１のカメラ１０５における対象物１０４のｚＡ軸方向の検出位置ｚＡを第２のカメラ座標に変換する。奥行き更新部１５８は、ｚＢ軸方向の検出位置ｚＢを変換後の値で更新する。そして、奥行き更新部１５８は、更新結果とステップＳ２１１の算出結果であるｘＢとｙＢと合わせて第２のカメラ１０６における検出位置をＴ（アロー）Ｂ＝［ｘＢ  ｙＢ  ｚＢ］Ｔと設定する。
【００９８】
（ステップＳ２１４）次に、姿勢推定部１５４は、例えば、ステップＳ２０４〜Ｓ２０７で算出された対象物１０４のＣＧモデルの姿勢を微小移動させた時の各軸方向の相違度の加重平均をそれぞれ評価関数Ｅ＋θｘＡＡ´、Ｅ―θｘＡＡ´、Ｅ＋θｙＡＡ´、Ｅ―θｙＡＡ´、Ｅ＋θｚＡＡ´、Ｅ―θｚＡＡ´、として、次の式（１４）〜式（１６）に従って算出する。
【００９９】
【数１４】
2014161937.tif 000016
【０１００】
【数１５】
2014161937.tif 000017
【０１０１】
【数１６】
2014161937.tif 000018
【０１０２】
ここで、加重平均における第１のカメラ１０５の重みｗＡ、第２のカメラ１０６の重みｗＢを例えば次の式（１７）及び式（１８）のように定める。
【０１０３】
【数１７】
2014161937.tif 000019
【０１０４】
【数１８】
2014161937.tif 000020
【０１０５】
ここで、Ｄは定数であって適宜設定される。式（１７）及び式（１８）のように重みを設定することにより、例えば第１のカメラ１０５と第２のカメラ１０６とを比較して、第１のカメラ１０５の方が対象物１０４の視認性が良く、相違度が小さい場合、式（１７）より加重平均における第１のカメラ１０５の重みｗＡの方が第２のカメラ１０６の重みｗＢより大きくなる。
【０１０６】
一方、第２のカメラ１０６の方が対象物１０４の視認性が良く、相違度が小さい場合、式（１８）より加重平均における第２のカメラ１０６の重みｗＢの方が第１のカメラ１０５の重みｗＡより大きくなる。これにより、視認性の良い方のカメラで撮影された画像に基づく相違度の評価関数に占める割合が高くなるため、対象物１０４の姿勢が変化してもその姿勢の変化が評価関数の変化として反映されるためロバストに姿勢を検出できる。
【０１０７】
また、ステップＳ２１４において、対象物１０４が目標の位置及び姿勢にあるときの第１のカメラ１０５における画像を生成する際に、第１のカメラ１０５を対象物１０４の視認性が良い位置及び姿勢に設定してあることを想定する。その場合、式（１７）より目標の位置及び姿勢付近での対象物１０４の姿勢検出では、第１のカメラ１０５により撮像された画像に基づく相違度の評価関数に占める割合が高くなる。先述したように、ステップＳ２０２において推定した第２のカメラ座標ΣＢにおける第１のカメラ座標ΣＡの姿勢を示す回転行列Ｒ（ハット）ＢＡは誤差を含んでいる可能性が高い。しかしながら、上述のように設定することで、対象物１０４が目標の位置及び姿勢付近にあるときの対象物１０４の姿勢検出では、第１のカメラ１０５により撮像された画像に基づく相違度が評価関数に占める割合が高くなる。そのため、回転行列Ｒ（ハット）ＢＡが含む誤差の影響を排除されて、高精度な姿勢検出ができる。
【０１０８】
なお、姿勢推定部１５４の処理は、上記の例に限ったものではない。姿勢推定部１５４は、第１の相違度が小さくなるほど、第１の相違度が姿勢の推定に貢献する割合を大きくしてもよい。同様に、姿勢推定部１５４は、第２の相違度が小さくなるほど、第２の相違度が姿勢の推定に貢献する割合を大きくしてもよい。
【０１０９】
次に、姿勢推定部１５４は、各軸方向の勾配を次の式（１９）に従って算出する。
【０１１０】
【数１９】
2014161937.tif 000021
【０１１１】
（ステップＳ２１５）次に、姿勢推定部１５４は、例えば、第１のカメラ１０５における対象物１０４の姿勢を次の式（２０）に従って更新する。ここで、式（２０）は、最急降下法による更新処理を表す。
【０１１２】
【数２０】
2014161937.tif 000022
【０１１３】
ここで、Ｃは定数であって適宜設定される。
（ステップＳ２１６）第２判定部１５９は収束判定を行う。具体的には、例えば、第２判定部１５９は、ステップＳ２０９、Ｓ２１２及びＳ２１５で算出した第１のカメラ１０５における対象物１０４の位置及び姿勢について、前回の値との差が予め決められた値より小さいか否か判定する。第２判定部１５９は前回の値との差が予め決められた値より小さい場合（ＹＥＳ）、収束したと判断して処理を終了する。一方、第２判定部１５９は前回の値との差が予め決められた値以上の場合（ＮＯ）、収束していないと判断して処理をステップＳ２０３に戻す。
【０１１４】
以上が、対象物１０４の位置及び姿勢の検出処理の説明である。なお、本検出方式は図６におけるステップＳ１０２、Ｓ１０５及びＳ１０６で用いられるが、ステップＳ１０６ではステップＳ２１６の収束判定は行わず、検出を継続させる。
【０１１５】
以上、本実施形態における制御装置１０１において、変換規則決定部１３０は、姿勢推定の対象となる対象物が目標の位置にある際に対象物１０４の視認性が良い第１のカメラ１０５とは異なる位置に配置された第２のカメラ１０６を基準とする第２のカメラ座標における、第１のカメラ１０５を基準とする第１のカメラ座標の姿勢を示す回転行列を推定する。そして、姿勢推定部１５４は、対象物１０４が目標の位置から予め決められた範囲にある際（例えば、対象物１０４が目標の位置に移動完了した際）に、第１の相違度が対象物１０４の姿勢の推定に貢献する割合を第２の相違度が対象物１０４の姿勢の推定に貢献する割合よりも高くする。そして、姿勢推定部１５４は、第１の相違度算出部１５１が算出した複数の第１の相違度と、第２の相違度算出部１５３が算出した複数の第２の相違度とを参照して、対象物１０４の姿勢を推定する。
【０１１６】
これにより、姿勢推定部１５４は、対象物１０４が目標の位置から予め決められた範囲にある際に、変換規則が含む誤差の影響を受ける第２の相違度が姿勢の検出に貢献する割合が相対的に小さくなるので、変換規則が含む誤差が対象物の姿勢推定に与える影響を小さくすることができる。
その結果、姿勢推定部１５４は、対象物の姿勢推定の精度を向上させることができる。
また、第１のカメラ１０５は、対象物１０４が目標の位置にある際に対象物１０４の視認性が良く、かつ対象物１０４が目標の位置から予め決められた範囲にある際に第１の相違度が姿勢の検出に貢献する割合が相対的に大きくなるので、対象物１０４が目標の位置から予め決められた範囲にある際の対象物の姿勢精度を向上させることができる。
【０１１７】
また、背景技術で説明した従来のステレオカメラを用いた姿勢の推定では、カメラ同士の相対的な位置関係を、何度もキャリブレーションして算出する必要があるため、煩雑で時間のかかる作業となるという問題があった。本実施形態における制御装置１０１は、カメラ同士の相対的な位置関係を算出する処理が必要ないので、簡便に対象物の姿勢を推定することができる。
【０１１８】
また、背景技術で説明したステレオカメラを用いた従来技術において、カメラで撮影した画像から対象物の位置及び姿勢を検出する際に用いる手法は、対象物の形状や表面の状態などに応じて適宜選択する必要があり、多様な対象物に対して適用することが出来ないという問題もあった。
【０１１９】
それに対し、本実施形態の制御装置１０１は、各カメラで検出した対象物１０４の位置から３次元復元により、第１のカメラ１０５における対象物１０４の第１のカメラ１０５の光軸方向の位置ｚＡを算出する。ここで仮に第１のカメラ１０５の光軸方向の相違度からその光軸方向の位置を推定した場合には、第１のカメラ１０５の光軸方向の相違度の変化が小さいために、その光軸方向の位置の推定誤差は変換規則が含む誤差よりも大きくなってしまう。それに対して、本実施形態の制御装置１０１は、平面位置推定部１５７で推定された第１の平面の位置と、平面位置推定部１５７で推定された第２の平面の位置とを参照して第１のカメラ１０５の光軸方向の位置の推定を算出するため、その光軸方向の位置の推定精度を向上させることが出来る。
【０１２０】
なお、姿勢推定部１５４は、対象物１０４が目標の位置から予め決められた位置範囲にあるとき、第２の相違度に係る重み（例えば、第２のカメラ１０６の重みｗＢ）が予め決められた範囲に収まるよう、対象物１０４の移動開始時刻から、対象物１０４が目標の位置から予め決められた位置範囲に到達する時刻まで、第２の相違度に乗じる重み係数を変化させてもよい。具体的には、例えば、姿勢推定部１５４は、第２の相違度に係る重みが対象物１０４が目標の位置から予め決められた位置範囲にあるときに０になるよう、対象物１０４の移動開始時刻から、対象物１０４が目標の位置から予め決められた位置範囲に到達する時刻まで、第２の相違度に係る重みを変化させてもよい。具体的には、例えば、姿勢推定部１５４は、図１０に示すように、第１のカメラ１０５の重みｗＡと第２のカメラ１０６の重みｗＢを時間的に変化させてもよい。図１０は、第１のカメラ１０５の重みｗＡと第２のカメラ１０６の重みｗＢの時間変化の一例である。縦軸は重みｗで、横軸は経過時間である。同図において、第１のカメラ１０５の重みｗＡは、時間によらず一定で１である。それに対し、第２のカメラ１０６の重みｗＢは、経過時間０（すなわちロボット手先位置の移動開始時）で１であるが、時間の経過とともに小さくなっている。そして、目標の位置及び姿勢に到達時に、第１のカメラ１０５の重みｗＡが１のままであるのに対し、第２のカメラ１０６の重みｗＢが０になっている。このように、姿勢推定部１５４は、対象物１０４の移動開始時刻から対象物１０４が目標の位置から予め決められた位置範囲に到達する時刻までの期間で、第２の相違度に乗じる重み係数を時間の経過とともに小さくしてもよい。
【０１２１】
これにより、対象物に関する作業が終了するときに、回転行列Ｒ（ハット）ＢＡが含む誤差の影響を受ける第２の相違度に係る重みを０または０に近づけることができるので、回転行列Ｒ（ハット）ＢＡが含む誤差が対象物１０４の姿勢推定に与える影響をより小さくすることができる。その結果、姿勢推定部１５４は、高精度に対象物の姿勢を推定することができる。
【０１２２】
また、姿勢推定部１５４は、図１０に示すように第２の相違度に係る重み（例えば、第２のカメラ１０６の重みｗＢ）を、時間の経過とともに小さくしてもよい。これにより、時間が経過するに連れて（例えば、対象物１０４が目標位置に近づくに連れて）、回転行列Ｒ（ハット）ＢＡが含む誤差の影響を受ける第２の相違度が姿勢の検出に貢献する割合が小さくなるので、回転行列Ｒ（ハット）ＢＡが含む誤差が対象物１０４の姿勢推定に与える影響を小さくすることができる。その結果、姿勢推定部１５４は、高精度に対象物１０４の姿勢を推定することができる。
【０１２３】
また、姿勢推定部１５４は、第１のカメラ１０５が第２のカメラ１０６よりも対象物１０４の視認性が良く、対象物１０４の位置が移動する場合、対象物１０４の単位時間あたりの移動量が小さくなるに従って第２の相違度に係る重み（例えば、第２のカメラ１０６の重みｗＢ）を小さくしてもよい。ここで、対象物１０４の単位時間あたりの移動量は対象物１０４を動かし始めたときが最も大きく、その後、時間の経過とともに徐々に小さくなるものである。このため、時間が経過するに連れて（例えば、対象物１０４が目標位置に近づくに連れて）、回転行列Ｒ（ハット）ＢＡが含む誤差の影響を受ける第２の相違度が姿勢の検出に貢献する割合が小さくなるので、回転行列Ｒ（ハット）ＢＡが含む誤差が対象物１０４の姿勢推定に与える影響を小さくすることができる。その結果、姿勢推定部１５４は、高精度に対象物の姿勢を推定することができる。
【０１２４】
また、第１のカメラ１０５が第２のカメラ１０６よりも対象物１０４の視認性が良い場合、姿勢推定部１５４は、図１０に示すように第２の相違度に係る重み（例えば、第２のカメラ１０６の重みｗＢ）を、対象物が目標の位置に近づくに従って小さくしてもよい。これにより、対象物に関する作業が終了に近づくに連れて、回転行列Ｒ（ハット）ＢＡが含む誤差の影響を受ける第２の相違度が姿勢の検出に貢献する割合が小さくなるので、回転行列Ｒ（ハット）ＢＡが含む誤差が対象物１０４の姿勢推定に与える影響を小さくすることができる。その結果、姿勢推定部１５４は、高精度に対象物の姿勢を推定することができる。
【０１２５】
＜第２の実施形態＞
続いて、第２の実施形態について説明する。第２の実施形態のロボット１ｂは、目標の位置付近の評価関数の勾配が急になる方の評価関数が導出された画像を撮像したカメラを、位置及び姿勢を決める際の基準となるカメラに設定する。第２の実施形態のロボット１ｂの構成は、第１の実施形態のロボット１に対して、制御装置１０１が制御装置（姿勢検出装置）１０１ｂに変更になったものである。
【０１２６】
図１１は、第２の実施形態における制御装置１０１ｂの構成を示す概略ブロック図である。同図において、図４と共通する要素には同一の符号を付し、その具体的な説明を省略する。図１１の制御装置１０１ｂの構成は、図４の制御装置１０１の構成に対して、処理部１２０が処理部１２０ｂに変更されたものになっている。図１１の処理部１２０ｂの構成は、図４の処理部１２０の構成に対して、位置決め基準カメラ決定部１８０が追加され、ゴール画像取得部１４０がゴール画像取得部１４０ｂに、位置姿勢検出部１５０が位置姿勢検出部１５０ｂに変更されたものになっている。
【０１２７】
ゴール画像取得部１４０ｂは、第１の実施形態におけるゴール画像取得部１４０と同様の機能を有するが、以下の点で異なる。ゴール画像取得部１４０ｂは、対象物１０４が目標の位置及び姿勢にあるときに、第１のカメラ１０５が対象物１０４を撮影した画像（以下、第１のゴール画像ともいう）を示す情報と第２のカメラ１０６が対象物１０４を撮影した画像（以下、第２のゴール画像ともいう）を示す情報とを位置決め基準カメラ決定部１８０へ出力する。
【０１２８】
位置決め基準カメラ決定部１８０は、例えば、第１のゴール画像を撮影した空間を再現する第１の仮想空間において存在する対象物１０４のＣＧモデルの位置及び姿勢から、予め決められた精度が要求される軸周りに正の角度方向に上記ＣＧモデルΔ微小移動させて撮影した画像と、第１のゴール画像との相違度ＥＡ＋を算出する。
また、位置決め基準カメラ決定部１８０は、例えば、第１のゴール画像を撮影した空間を再現する第１の仮想空間において存在する対象物１０４のＣＧモデルの位置及び姿勢から、予め決められた精度が要求される軸周りに負の角度方向に上記ＣＧモデルΔ微小移動させて撮影した画像と、第１のゴール画像との相違度ＥＡ−を算出する。
【０１２９】
また、位置決め基準カメラ決定部１８０は、例えば、第２のゴール画像を撮影した空間を再現する第２の仮想空間において存在する対象物１０４のＣＧモデルの位置及び姿勢から、予め決められた精度が要求される軸周りに正の角度方向に上記ＣＧモデルΔ微小移動させて撮影した画像と、第２のゴール画像との相違度ＥＢ＋を算出する。
また、位置決め基準カメラ決定部１８０は、例えば、第２のゴール画像を撮影した空間を再現する第２の仮想空間において存在する対象物１０４のＣＧモデルの位置及び姿勢から、予め決められた精度が要求される軸周りに負の角度方向に上記ＣＧモデルをΔ微小移動させて撮影した画像と、第２のゴール画像との相違度ＥＢ−を算出する。
【０１３０】
そして、位置決め基準カメラ決定部１８０は、例えば、相違度ＥＡ＋と相違度ＥＡ−の勾配ＧＡ（＝（ＥＡ＋−ＥＡ−）／２Δ）を算出する。また、位置決め基準カメラ決定部１８０は、例えば、相違度ＥＢ＋と相違度ＥＢ−の勾配ＧＢ（＝（ＥＢ＋−ＥＢ−）／２Δ）を算出する。そして、位置決め基準カメラ決定部１８０は、例えば勾配ＧＡと勾配ＧＢのうち、勾配が急な方を導出するのに用いたカメラを位置及び姿勢を決める際の基準となるカメラに設定する。
そして、位置決め基準カメラ決定部１８０は、設定した基準となるカメラを示す基準カメラ情報を位置姿勢検出部１５０ｂへ出力する。
【０１３１】
位置姿勢検出部１５０ｂは、第１の実施形態における位置姿勢検出部１５０と同様の機能を有するが、以下の点で異なる。位置姿勢検出部１５０ｂは、基準カメラ情報が示す基準となるカメラが第１のカメラ１０５の場合、第１の実施形態と同様の処理を行う。位置姿勢検出部１５０ｂは、基準カメラ情報が示す基準となるカメラが第２のカメラ１０６の場合、第１の実施形態における第１のカメラ１０５と第２のカメラ１０６とを入れ替えた処理を行う。
【０１３２】
図１２は、第２の実施形態における制御装置１０１の処理全体の流れの一例を示すフローチャートである。
（ステップＳ３０１）まず、変換規則決定部１３０は、第２のカメラ座標ΣＢにおける第１のカメラ座標ΣＡの原点の位置を示す位置ベクトルＴ（アロー）ＢＡを取得する。変換規則決定部１３０は、取得した位置ベクトルＴ（アロー）ＢＡにマイナスを乗じたベクトルを、第１のカメラ座標ΣＡにおける第２のカメラ座標ΣＢの原点の位置を示す位置ベクトルＴ（アロー）ＡＢとして算出する。
また、変換規則決定部１３０は、第２のカメラ座標ΣＢにおける第１のカメラ座標ΣＡの姿勢を示す回転行列Ｒ（ハット）ＢＡを取得する。変換規則決定部１３０は、取得した回転行列Ｒ（ハット）ＢＡの逆行列を、第１のカメラ座標ΣＡにおける第２のカメラ座標ΣＢの姿勢を示す回転行列Ｒ（ハット）ＡＢとして算出する。
また、変換規則決定部１３０は、ロボット座標ΣＲにおける第１のカメラ座標ΣＡの姿勢を示す回転行列Ｒ（ハット）ＲＡを取得する。そして、変換規則決定部１３０は、取得した回転行列Ｒ（ハット）ＲＡを左から算出した回転行列Ｒ（ハット）ＡＢに乗じることで、ロボット座標ΣＲにおける第２のカメラ座標ΣＢの姿勢を示す回転行列Ｒ（ハット）ＲＢを算出する。
（ステップＳ３０２）次に、ゴール画像取得部１４０ｂは、対象物１０４が目標の位置及び姿勢にあるときに第１のカメラ１０５から撮像される第１のゴール画像と第２のカメラ１０６から撮像される第２のゴール画像を取得する。
（ステップＳ３０３）次に、位置決め基準カメラ決定部１８０は、どちらのカメラを基準に位置決めするか決定する。
（ステップＳ３０４）次に、位置姿勢検出部１５０ｂは、位置決め基準カメラ決定部１８０による決定が、第１のカメラ１０５を基準に位置決めする決定であるか否か判定する。
位置姿勢検出部１５０ｂは、第１のカメラ１０５を基準に位置決めする決定の場合（ＹＥＳ）、ステップＳ３０５に移行し、処理部１２０ｂはステップＳ３０５〜Ｓ３０９の処理を実行する。一方、位置姿勢検出部１５０ｂは、第２のカメラ１０６を基準に位置決めする決定の場合（ＮＯ）、ステップＳ３１０に移行し、処理部１２０ｂはステップＳ３１０〜Ｓ３１４の処理を実行する。
【０１３３】
（ステップＳ３０５）ロボット制御部１６０は、対象物１０４を第１のカメラ１０５及び第２のカメラ１０６の視野内に移動させる。
（ステップＳ３０６）次に、位置姿勢検出部１５０ｂは、ステップＳ３０１で取得した第１のゴール画像から第１のカメラ座標における対象物１０４の位置を示す位置ベクトルＴ（アロー）ｇＡ及び姿勢を示す回転行列Ｒ（ハット）ｇＡを検出する。
【０１３４】
（ステップＳ３０７）次に、位置姿勢検出部１５０ｂは、第１のカメラ１０５及び第２のカメラ１０６を用いて、現在の第１のカメラ座標における対象物１０４の位置を示す位置ベクトルＴ（アロー）ｃＡ及び姿勢を示す回転行列Ｒ（ハット）ｃＡを検出する。
（ステップＳ３０８）次に、ロボット制御部１６０は、ステップＳ３０４で取得したロボット座標ΣＲにおける第１のカメラ座標ΣＡの姿勢を示す回転行列Ｒ（ハット）ＲＡを用いて、ロボットの手先についての並進方向の移動量及び回転方向の移動量を算出する。そして、ロボット制御部１６０は、ロボットの手先を算出した並進方向の移動量ΔＴ（アロー）Ｒだけ並進方向へ移動させ、算出した回転方向の移動量Δθ（アロー）Ｒだけ回転方向へ移動させる。
【０１３５】
（ステップＳ３０９）次に、第１判定部１７０は、ステップＳ３０９でロボット制御部１６０が算出した並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒが予め決められた値より小さいか否かを判定する。予め決められた値より小さい場合（ＹＥＳ）、第１判定部１７０は対象物１０４がステップＳ３０１で作成した第１のゴール画像における対象物１０４の位置及び姿勢に到達したと判断してその処理を終了する。予め決められた値以上の場合（ＮＯ）、第１判定部１７０は目標の位置及び姿勢に到達していないと判断し、ステップＳ３０７に処理を戻す。なお、ロボット制御部１６０は、例えば回転方向の移動量Δθ（アロー）Ｒの各要素Δθｘ、Δθｙ、Δθｚを、式（５）を用いて算出する。
【０１３６】
（ステップＳ３１０）次に、ロボット制御部１６０は、対象物１０４を第１のカメラ１０５及び第２のカメラ１０６の視野内に移動させる。
（ステップＳ３１１）次に、位置姿勢検出部１５０ｂは、ステップＳ３０１で取得した第２のゴール画像から第２のカメラ座標における対象物１０４の位置を示す位置ベクトルＴ（アロー）ｇＢ及び姿勢を示す回転行列Ｒ（ハット）ｇＢを検出する。
【０１３７】
（ステップＳ３１２）次に、位置姿勢検出部１５０ｂは、第１のカメラ１０５及び第２のカメラ１０６を用いて、現在の第２のカメラ座標における対象物１０４の位置を示す位置ベクトルＴ（アロー）ｃＢ及び姿勢を示す回転行列Ｒ（ハット）ｃＢを検出する。
（ステップＳ３１３）次に、ロボット制御部１６０は、ステップＳ３１１で取得したロボット座標ΣＲにおける第２のカメラ座標ΣＢの姿勢を示す回転行列Ｒ（ハット）ＲＢを用いて、ロボットの手先についての並進方向の移動量及び回転方向の移動量を算出する。そして、ロボット制御部１６０は、ロボットの手先を算出した並進方向の移動量ΔＴ（アロー）Ｒだけ並進方向へ移動させ、算出した回転方向の移動量Δθ（アロー）Ｒだけ回転方向へ移動させる。
【０１３８】
（ステップＳ３１４）次に、第１判定部１７０は、ステップＳ３１５でロボット制御部１６０が算出した並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒが予め決められた値より小さいか否かを判定する。予め決められた値より小さい場合（ＹＥＳ）、第１判定部１７０は対象物１０４がステップＳ３０１で作成した第２のゴール画像における対象物１０４の位置及び姿勢に到達したと判断してその処理を終了する。予め決められた値以上の場合（ＮＯ）、第１判定部１７０は目標の位置及び姿勢に到達していないと判断し、ステップＳ３１２に処理を戻す。以上で、本フローチャートの説明を終了する。なお、ロボット制御部１６０は、例えば回転方向の移動量Δθ（アロー）Ｒの各要素Δθｘ、Δθｙ、Δθｚを、式（５）を用いて算出する。
【０１３９】
以上、第２の実施形態における制御装置１０１において、位置決め基準カメラ決定部１８０は、勾配ＧＡと勾配ＧＢを算出し、勾配ＧＡと勾配ＧＢのうち、勾配が急な方を導出するのに用いたカメラを位置及び姿勢を決める際の基準となるカメラに設定する。
これにより、第１の実施形態における効果に加えて、以下の効果を奏する。目標の位置及び姿勢付近において、設定された基準となるカメラが撮影した画像から導出される相違度が評価関数に貢献する割合が大きくなる。その際、目標の位置及び姿勢付近において、別のカメラを基準としたときよりも相違度の変化が大きくなるので、評価関数の変化も大きくなり、更新される対象物１０４の位置及び姿勢の変化量が大きくなるので、対象物１０４を目標の位置及び姿勢により早く到達させることができる。
【０１４０】
なお、位置決め基準カメラ決定部１８０は、相違度を算出する際に、対象物１０４のＣＧモデルの位置及び姿勢から、予め決められた精度が要求される軸周りに正または負の角度方向にΔ微小移動させて撮影したが、これに限ったものではない。位置決め基準カメラ決定部１８０は、予め決められた精度が要求される軸方向について正または負の方向にΔ微小移動させて撮影してもよい。
また、ユーザがロボット１ｂの作業内容に応じて、Δ微小移動させるのをどの軸周り、またはどの軸方向にするのかを入力部１１０に入力し、入力部１１０がその入力を受け付け、処理部１２０ｂが入力部１１０が受け付けたもので設定できるようにしてもよい。
【０１４１】
このことから、位置決め基準カメラ決定部１８０は、第１のゴール画像を撮影した空間を再現する第１の仮想空間において存在する対象物１０４のＣＧモデルの位置及び姿勢から、予め決められた精度が要求される軸周りまたは軸方向に予め決められたように上記ＣＧモデルを移動させて撮影した画像と、第１のゴール画像との相違度を示す第５の相違度を算出する。
位置決め基準カメラ決定部１８０は、第２のゴール画像を撮影した空間を再現する第２の仮想空間において存在する対象物１０４のＣＧモデルの位置及び姿勢から、予め決められた精度が要求される軸周りまたは軸方向に予め決められたように上記ＣＧモデルを移動させて撮影した画像と、第２のゴール画像との相違度を示す第６の相違度を算出する。
また、位置決め基準カメラ決定部１８０は、第５の相違度を算出した際の移動方向における第５の相違度の勾配と、第６の相違度を算出した際の移動方向における第６の相違度の勾配とを比較することで、姿勢推定の基準となるカメラを決定する。その際、位置決め基準カメラ決定部１８０は、決定した基準となるカメラを新たに第１のカメラとしてもよい。
【０１４２】
＜第３の実施形態＞
続いて、第３の実施形態について説明する。予め決められた時間後までにロボット本体１０２の位置及び姿勢を変化させる量を自装置が第１の撮像画像及び第２の撮像画像から決定する処理にかかる時間には揺らぎがある。そのため、その揺らぎを考慮して、自装置が第１の撮像画像及び第２の撮像画像の撮像を開始してからロボット本体１０２の位置及び姿勢の変化量を決定するまでの一連の処理にかかる最大の時間（例えば、１５ミリ秒）まで待ってから、制御装置がロボット本体１０２の制御を開始した場合、対象物１０４が目標の位置及び姿勢に到達するまでにかかる時間が長くなってしまうという問題がある。そこで、本実施形態では、対象物１０４が目標の位置及び姿勢に到達するまでにかかる時間をより短くすることを本実施形態の課題とする。
【０１４３】
本実施形態では一例として、自装置が第１の撮像画像及び第２の撮像画像の撮像を開始してからロボット本体１０２の位置及び姿勢の変化量を決定するまでの一連の処理にかかる平均時間（以下、画像処理平均時間ともいう）が１０ミリ秒であり、その一連の処理にかかる時間（以下、画像処理時間ともいう）の揺らぎが±５ミリ秒であるものとして説明する。また、本実施形態では一例として、ロボットの位置及び姿勢制御が１ミリ秒毎に行われるものとして説明する。
【０１４４】
図１３は、第３の実施形態における制御装置１０１ｃの構成を示す概略ブロック図である。同図において、図４と共通する要素には同一の符号を付し、その具体的な説明を省略する。図１３の制御装置１０１ｃの構成は、図４の制御装置１０１の構成に対して、処理部１２０が処理部１２０ｃに変更されたものになっている。図１３の処理部１２０ｃの構成は、図４の処理部１２０の構成に対して、目標位置姿勢決定部２００が追加され、ロボット制御部１６０がロボット制御部１６０ｃに変更されたものになっている。
【０１４５】
目標位置姿勢決定部２００は、位置姿勢検出部１５０により推定された対象物の位置及び姿勢と対象物が目標の位置にある場合の位置及び姿勢とに基づいて、現在の時刻から予め決められた時間（例えば、１０ミリ秒）経過した後のロボット座標系における位置及び姿勢の変化量を決定する。ここで、予め決められた時間は例えば、第１の撮像画像及び第２の撮像画像を撮像開始してから上記変化量を決定するまでの平均時間である。
【０１４６】
具体的には例えば、目標位置姿勢決定部２００は、並進方向の移動量ΔＴ（アロー）Ｒと、回転方向の移動量Δθ（アロー）Ｒを第１の実施形態と同様の処理で決定する。目標位置姿勢決定部２００は、並進方向の移動量ΔＴ（アロー）Ｒと、回転方向の移動量Δθ（アロー）Ｒを決定する毎に、並進方向の移動量ΔＴ（アロー）Ｒと、回転方向の移動量Δθ（アロー）Ｒをロボット制御部１６０ｃと第１判定部１７０へ出力する。
【０１４７】
ロボット制御部１６０ｃは、ロボット本体１０２を制御する時間間隔（例えば、１ミリ秒）毎に、目標姿勢決定部が決定した変化量のうち最新の変化量に基づいて、上記時間間隔（例えば、１ミリ秒）後のロボット本体１０２の位置及び姿勢を決定し、該決定した位置及び姿勢になるようロボット本体１０２を制御する。その際、ロボット制御部１６０ｃは、上記予め決められた時間（ここでは一例として、１０ミリ秒）のうち、自装置が第１の撮像画像及び第２の撮像画像から上記変化量を決定するまでの最短時間に基づいて決められた時間まではロボットが加速及び減速で移動するようにロボット本体１０２の位置及び姿勢を決定し、その後の時間はロボット本体１０２が等速で移動するようにロボット本体１０２の位置及び姿勢を決定する。
【０１４８】
ここで、本実施形態では、自装置が第１の撮像画像及び第２の撮像画像から上記変化量を決定するまでの最短時間に基づいて決められた時間は一例として、自装置が第１の撮像画像及び第２の撮像画像から上記変化量を決定するまでの最短時間に、第１の撮像画像及び第２の撮像画像を撮像する時間を加算した時間である。第１の撮像画像及び第２の撮像画像の撮像時間は一定であることから、自装置が第１の撮像画像及び第２の撮像画像から上記変化量を決定するまでの最短時間に基づいて決められた時間は一例として、第１の撮像画像及び第２の撮像画像を撮像開始してから上記変化量を決定するまでの最短時間である。
【０１４９】
図１４を用いて、ロボット制御部１６０ｃによるロボット本体１０２の位置及び姿勢の制御処理の一例について説明する。図１４は、ロボット座標系におけるロボット本体１０２の手先位置のｘ座標の時間変化の一例である。同図には、期間Ｔ１においてロボット本体１０２の手先のｘ座標に沿って加速しながら移動し、期間Ｔ２においてロボット本体１０２の手先のｘ座標に沿って等速で移動し、期間Ｔ３においてロボット本体１０２の手先のｘ座標に沿って減速しながら移動し、期間Ｔ４においてロボット本体１０２の手先のｘ座標に沿って加速しながら移動することが示されている。また、１０ミリ秒後に２番目の並進方向の移動量ΔＴ（アロー）Ｒと回転方向の移動量Δθ（アロー）Ｒが決定されて、１５ミリ秒後に、３番目の並進方向の移動量ΔＴ（アロー）Ｒと回転方向の移動量Δθ（アロー）Ｒが決定されていることが示されている。
【０１５０】
図１４の例では、期間Ｔ１と期間Ｔ２では、ロボット制御部１６０ｃは、最初に決定された並進方向の移動量ΔＴ（アロー）Ｒと回転方向の移動量Δθ（アロー）Ｒを用いて、１ミリ秒後のロボット本体１０２の手先位置のｘ座標を決定する。これにより、期間Ｔ１と期間Ｔ２におけるｘ座標が１ミリ秒毎に得られる。ロボット制御部１６０ｃは、決定した１ミリ秒後の手先位置のｘ座標に、ロボット本体１０２の手先を１ミリ秒かけて移動させる。ロボット制御部１６０ｃは、１ミリ秒経過する毎に、ｘ座標の決定とロボット本体１０２の手先の移動処理を繰り返す。これにより、ボット制御部１６０ｃは、前半の５ミリ秒である期間Ｔ１では、ロボット本体１０２の手先位置がｘ座標に沿って加速しながら移動させ、後半の５ミリ秒である期間Ｔ２では、ロボット本体１０２の手先位置がｘ座標に沿って等速で移動させることができる。
【０１５１】
時刻１０ミリ秒で、ロボット制御部１６０ｃは、目標位置姿勢決定部２００から２番目の並進方向の移動量ΔＴ（アロー）Ｒと回転方向の移動量Δθ（アロー）Ｒが入力されるので、その２番目の並進方向の移動量ΔＴ（アロー）Ｒと回転方向の移動量Δθ（アロー）Ｒを用いて、１ミリ秒後のロボット本体１０２の手先位置のｘ座標を決定する。これにより、期間Ｔ３におけるｘ座標が１ミリ秒毎に得られる。ロボット制御部１６０ｃは、決定した１ミリ秒後の手先位置のｘ座標に、ロボット本体１０２の手先を１ミリ秒かけて移動させる。ロボット制御部１６０ｃは、１ミリ秒経過する毎に、ｘ座標の決定とロボット本体１０２の手先の移動処理を繰り返す。これにより、ロボット制御部１６０ｃは、期間Ｔ３で、ロボット本体１０２の手先位置がｘ座標に沿って減速しながら移動させることができる。
【０１５２】
時刻１５ミリ秒で、ロボット制御部１６０ｃは、目標位置姿勢決定部２００から３番目の並進方向の移動量ΔＴ（アロー）Ｒと回転方向の移動量Δθ（アロー）Ｒが入力されるので、その３番目の並進方向の移動量ΔＴ（アロー）Ｒと回転方向の移動量Δθ（アロー）Ｒを用いて、１ミリ秒後のロボット本体１０２の手先位置のｘ座標を決定する。これにより、期間Ｔ４におけるｘ座標が１ミリ秒毎に得られる。ロボット制御部１６０ｃは、決定した１ミリ秒後の手先位置のｘ座標に、ロボット本体１０２の手先を１ミリ秒かけて移動させる。ロボット制御部１６０ｃは、１ミリ秒経過する毎に、ｘ座標の決定とロボット本体１０２の手先の移動処理を繰り返す。これにより、ロボット制御部１６０ｃは、期間Ｔ４で、ロボット本体１０２の手先位置がｘ座標に沿って加速しながら移動させることができる。
【０１５３】
続いて、ロボット本体１０２の手先位置のｘ座標の決定処理の詳細について説明する。ロボット制御部１６０ｃは、次の式（２１）に従ってロボット本体１０２の手先位置のｘ座標を決定する。
【０１５４】
【数２１】
2014161937.tif 000023
【０１５５】
ここで、ｔは時刻で、Ｔは画像処理平均時間で、ｖ１はロボット本体１０２の手先位置のｘ座標に沿った速度で、ｖ０は、画像処理平均時間Ｔにおけるロボット本体１０２の手先位置のｘ座標に沿った初速で、ｘ０は画像処理平均時間Ｔにおけるｘ座標の初期位置である。
ここで、ｖ１は次の式（２２）で表される。
【０１５６】
【数２２】
2014161937.tif 000024
【０１５７】
Δｘは、並進方向の移動量ΔＴ（アロー）Ｒのｘ座標成分で、ｎは正の整数（ここでは、一例として１）で、ΔＴはロボット制御周期（本実施形態では一例として１ミリ秒）ある。
ロボット制御部１６０ｃは、ロボット本体１０２の手先位置のｙ座標、ｚ座標、回転角度θｘ、θｙ、θｚについても同様の処理で決定する。
【０１５８】
図１５は、第３の実施形態における制御装置１０１ｃの処理全体の流れの一例を示すフローチャートである。ステップＳ４０１〜Ｓ４０６は図６のステップＳ１０１〜Ｓ１０６と同じであるので、その説明を省略する。また、ステップＳ４０９は、図６のステップＳ１０８と同じであるので、その説明を省略する。
（ステップＳ４０７）次に、目標位置姿勢決定部２００は、ステップＳ１０１で取得したロボット座標ΣＲにおける第１のカメラ座標ΣＡの姿勢を示す回転行列Ｒ（ハット）ＲＡを用いて、ロボットの手先の並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒを決定する。ロボット制御部１６０ｃは、決定した並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒを第１判定部１７０及びロボット制御部１６０ｃへ出力する。
【０１５９】
図１６は、第３の実施形態におけるロボット制御部１６０ｃの処理の一例を示すフローチャートである。
（ステップＳ５０１）ロボット制御部１６０ｃは、目標位置姿勢決定部２００により新たに並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒを決定されたか否か判定する。新たに決定された場合（ＹＥＳ）、ステップＳ５０２の処理に進む。新たに決定されていない場合（ＮＯ）、ステップＳ５０３の処理に進む。
（ステップＳ５０２）ロボット制御部１６０ｃは、新たな並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒを用いて、１ミリ秒後のロボット手先位置及びロボット手先の姿勢（以下、ロボット手先姿勢ともいう）を決定する。
（ステップＳ５０３）ロボット制御部１６０ｃは、これまでの並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒを用いて、１ミリ秒後のロボット手先位置及びロボット手先の姿勢を決定する。
（ステップＳ５０４）次に、ロボット制御部１６０ｃは、ロボットの手先が決定された１ミリ秒後のロボット手先位置及びロボット手先姿勢になるようロボットの手先を移動させる。
（ステップＳ５０５）次に、ロボット制御部１６０ｃは、第１判定部１７０が最新の並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒが予め決められた値より小さいと判定したか否か判定する。予め決められた値より小さいと判定した場合（ＹＥＳ）、ロボット制御部１６０ｃはその処理を終了する。予め決められた値より小さいと判定していない場合（ＮＯ）、ロボット制御部１６０ｃはステップＳ５０１の処理に戻る。以上で、本フローチャートの処理を終了する。
【０１６０】
以上、第３の実施形態では、ロボット制御部１６０ｃは、並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒが更新される毎に、更新された並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒを用いて、ロボット制御周期間隔でロボット手先位置およびロボット手先姿勢を決定する。そして、ロボット制御部１６０ｃは、そのロボット制御周期で、決定したロボット手先位置およびロボット手先姿勢なるようロボット本体１０２を制御する。その際、ロボット制御部１６０ｃは、予め決められた画像処理平均時間（一例として１０ミリ秒）のうち、画像処理にかかる最短時間（一例として５ミリ秒）まではロボットの手先が加速及び減速で移動するようにロボット手先位置及びロボット手先姿勢を決定し、その後の時間（一例として５〜１０ミリ秒）はロボットの手先が等速で移動するようにロボット手先位置及びロボット手先姿勢を決定する。これにより、画像処理時間に±５ミリ秒の揺らぎがあって、画像処理にかかる時間が５ミリ秒から１０ミリ秒の間で変化しても、その間はロボットの手先が等速で移動するので、その等速で移動しているときの速度を初速としてロボットの加速制御または減速制御を始めることができる。これにより、ロボット制御部１６０ｃは、画像処理が終わると同時に、その画像処理で得られた並進方向の移動量ΔＴ（アロー）Ｒ及び回転方向の移動量Δθ（アロー）Ｒを用いてロボットを制御できるので、第1の実施形態の効果に加えて、対象物１０４を目標の位置及び目標の姿勢にするまでの時間を短縮することができる。
【０１６１】
なお、各実施形態において、姿勢推定部１５４は、第１の相違度に乗じる重み係数と、第２の相違度に乗じる重み係数を、第１の撮像画像と第２の撮像画像のそれぞれに混入している雑音成分に応じて変化させてもよい。ここで、雑音成分とは、撮像装置のイメージセンサーが光電変換する処理以降に混入する雑音信号に由良する雑音成分である。雑音成分は、例えば撮像装置のイメージセンサーが光電変換する際に、光電変換後の画像信号に混入するランダムな雑音信号に由来する雑音成分である。具体的には、例えば、姿勢推定部１５４は、第１の相違度に乗じる重み係数と、第２の相違度とに乗じる重み係数を、第１の撮像画像と第２の撮像画像のそれぞれに混入している正規化ランダムノイズの分散の逆数としてもよい。その際、姿勢推定部１５４は、例えば第１のカメラ１０５及び第２のカメラ１０６それぞれのカメラの異なるフレーム間の画像の相違度であるフレーム間相違度を、第１の撮像画像と第２の撮像画像のそれぞれに混入している正規化ランダムノイズの分散の代用として算出してもよい。より好ましくは、姿勢推定部１５４は、それぞれのカメラの１フレーム前の画像と現在のフレームの画像との相違度を、第１の撮像画像と第２の撮像画像のそれぞれに混入する正規化ランダムノイズの分散の代用として算出してもよい。これは、分散がフレーム間相違度に比例するからである。ここで、フレーム間相違度は、例えば１フレーム前の画像の画素値と現在のフレームの画像においてその画素値に対応する画素の画素値との差分の絶対値を全ての画素間で計算し、計算して得られた値を総和した値である。
【０１６２】
なお、各実施形態において、対象物が複数ある場合、以下のようにしてもよい。第１の相違度算出部１５１は、第１の仮想空間において位置姿勢推定の対象となる対象物以外の対象物全ての画像領域を特定し、該特定した画像領域を演算対象から除いて第１の相違度を算出してもよい。また、第２の相違度算出部１５３は、第２の仮想空間において位置姿勢推定の対象となる対象物以外の対象物全ての画像領域を特定し、該特定した画像領域を演算対象から除いて第２の相違度を算出してもよい。
【０１６３】
具体的には例えば、第１の相違度算出部１５１及び第２の相違度算出部１５３は以下の処理を行ってもよい。第１の相違度算出部１５１及び第２の相違度算出部１５３は例えば、それぞれ第１及び第２の仮想空間のそれぞれに、位置姿勢推定の対象となる対象物以外の対象物全てを各軸の正方向及び負方向に微小移動させた画像を全て配置した場合に、対象物毎に画像領域の和集合を抽出し、該和集合を全てマスクしたマスク画像を取得する。そして、第１の相違度算出部１５１及び第２の相違度算出部１５３は例えば、取得したマスク画像を用いて、位置姿勢推定の対象となる対象物以外の対象物を除去して、それぞれ相違度を算出する。
【０１６４】
簡単のために、ＣＧ画像が平面画像である場合を一例として上述した処理を説明する。図１７は、マスク画像について説明するための図である。同図において、第１の仮想空間において、第１対象物と第２対象物を共に現在の位置から上にΔ微小移動したＣＧ画像Ｇ１３１−１、共に現在の位置から下にΔ微小移動したＣＧ画像Ｇ１３１−２、共に現在の位置から右にΔ微小移動したＣＧ画像Ｇ１３１−３、及び共に現在の位置から左にΔ微小移動したＣＧ画像Ｇ１３１−４の例が示されている。ＣＧ画像Ｇ１３１−１において、第１対象物の画像領域Ｒ１３２−１、第２対象物の画像領域Ｒ１３３−１が示されている。ここで、簡単のために、他のＣＧ画像Ｇ１３１−２〜４における第１対象物の画像領域Ｒ１３２−２〜４、第２対象物の画像領域Ｒ１３３−２〜４は図示されていない。マスク画像Ｇ１３４において、上述したＣＧ画像Ｇ１３１−１〜４における第２対象物の画像領域Ｒ１３３−１〜４の和集合である画像領域である第２対象物のマスク領域Ｒ１３５が示されている。また、マスク画像Ｇ１３５において、上述したＣＧ画像Ｇ１３１−１〜４における第１対象物の画像領域Ｒ１３２−１〜４の和集合である画像領域である第１対象物のマスク領域Ｒ１３７が示されている。
【０１６５】
同図の例の場合、第１の相違度算出部１５１は例えば図１７のマスク画像Ｇ１３４を取得する。そして、第１の相違度算出部１５１は例えばマスク画像Ｇ１３４の領域を除いて、第１の対象物に関する第１の相違度を算出する。第１の相違度算出部１５１は例えば図１７のマスク画像Ｇ１３６を取得する。そして、第１の相違度算出部１５１は例えばマスク画像Ｇ１３６の領域を除いて、第２の対象物に関する第１の相違度を算出する。
同様に、第２の相違度算出部１５３は例えば図１７のマスク画像Ｇ１３４を取得する。そして、第２の相違度算出部１５３は例えばマスク画像Ｇ１３４の領域を除いて、第１の対象物に関する第２の相違度を算出する。第２の相違度算出部１５３は例えば図１６のマスク画像Ｇ１３６を取得する。そして、第２の相違度算出部１５３はマスク画像Ｇ１３６の領域を除いて、第２の対象物に関する第２の相違度を算出する。
【０１６６】
なお、各実施形態において、位置推定部１５６の奥行き更新部１５８は、第１のカメラ１０５における対象物１０４のｚＡ軸方向の検出位置ｚＡを、三角測量を利用して算出したが、これに限ったものではない。位置推定部１５６は、第１の相違度算出部が算出した複数の第３の相違度と、第２の相違度算出部が算出した複数の第４の相違度とに基づいて第１の撮像座標系における対象物の奥行きを推定してもよい。
【０１６７】
なお、複数の装置を備えるシステムが、各実施形態の制御装置（１０１または１０１ｂ）の各処理を、それらの複数の装置で分散して処理してもよい。
また、各実施形態の制御装置（１０１または１０１ｂ）の各処理を実行するためのプログラムをコンピューター読み取り可能な記録媒体に記録して、当該記録媒体に記録されたプログラムをコンピューターシステムに読み込ませ、実行することにより、制御装置（１０１または１０１ｂ）に係る上述した種々の処理を行ってもよい。
【０１６８】
なお、ここでいう「コンピューターシステム」とは、ＯＳや周辺機器等のハードウェアを含むものであってもよい。また、「コンピューターシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。また、「コンピューター読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、フラッシュメモリ等の書き込み可能な不揮発性メモリ、ＣＤ−ＲＯＭ等の可搬媒体、コンピューターシステムに内蔵されるハードディスク等の記憶装置のことをいう。
【０１６９】
さらに「コンピューター読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバーやクライアントとなるコンピューターシステム内部の揮発性メモリ（例えばＤＲＡＭ（Ｄｙｎａｍｉｃ  Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ））のように、一定時間プログラムを保持しているものも含むものとする。また、上記プログラムは、このプログラムを記憶装置等に格納したコンピューターシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピューターシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。また、上記プログラムは、前述した機能の一部を実現するためのものであっても良い。さらに、前述した機能をコンピューターシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であっても良い。
【０１７０】
以上、本発明の実施形態について図面を参照して詳述したが、具体的な構成はこの実施形態に限られるものではない。各実施形態における各構成及びそれらの組み合わせ等は一例であり、本発明の趣旨から逸脱しない範囲内で、構成の付加、省略、置換、およびその他の変更が可能である。また、本発明は実施形態によって限定されることはなく、特許請求の範囲によってのみ限定される。
【符号の説明】
【０１７１】
１０１、１０１ｂ  制御装置（姿勢検出装置）
１０２  ロボット本体
１０３  グリッパ
１０４  対象物
１０５  第１のカメラ（第１の撮像装置）
１０６  第２のカメラ（第２の撮像装置）
１１０  入力部
１２０  処理部１２０、１２０ｂ
１３０  変換規則決定部
１４０、１４０ｂ  ゴール画像取得部
１４５  現在画像取得部
１５０、１５０ｂ  位置姿勢検出部
１５１  第１の相違度算出部
１５２  姿勢取得部
１５３  第２の相違度算出部
１５４  姿勢推定部
１５６  位置推定部
１５７  平面位置推定部
１５８  奥行き更新部
１５９  第２判定部
１６０、１６０ｃ  ロボット制御部
１７０  第１判定部
１８０  位置決め基準カメラ決定部
１９０  記憶部
２００  目標位置姿勢決定部
【図１】
2014161937.tif 000025
【図２】
2014161937.tif 000026
【図３】
2014161937.tif 000027
【図４】
2014161937.tif 000028
【図５】
2014161937.tif 000029
【図６】
2014161937.tif 000030
【図７】
2014161937.tif 000031
【図８】
2014161937.tif 000032
【図９】
2014161937.tif 000033
【図１０】
2014161937.tif 000034
【図１１】
2014161937.tif 000035
【図１２】
2014161937.tif 000036
【図１３】
2014161937.tif 000037
【図１４】
2014161937.tif 000038
【図１５】
2014161937.tif 000039
【図１６】
2014161937.tif 000040
【図１７】
2014161937.tif 000041