JP6357949B2 - 制御システム、ロボットシステム、及び制御方法 - Google Patents

Description

この発明は、制御システム、ロボットシステム、及び制御方法に関する。

近年、正確な３次元形状計測に関する技術の需要が高まっている。正確な３次元形状計測が可能となることにより、工業製品等の傷のチェックや、ビンピッキング、計測結果の３次元プリンターへの利用等、様々な応用が考えられる。

従来、３次元形状計測の方法としては、プロジェクターとカメラを１台ずつ使用した位相シフト法を用いた３次元形状計測が知られている（特許文献１参照）。この方法は、プロジェクターによって投影された正弦波パターンをカメラで撮像することで、撮像画像と投影画像とのピクセル間の対応（すなわち、プロジェクターとカメラのピクセル間の対応）を計算し、三角測量によって、その深度を計算する方法である。

特開２００９‐１１５６１２号公報

しかし、従来の方法では、プロジェクターとカメラが１台ずつであるため、カメラで撮像できない影の部分や、プロジェクターの光が届かない影の部分について、３次元形状を計測することが困難であった。

そこで本発明は、上記従来技術の問題に鑑みてなされたものであり、多様な３次元形状の計測を行うことができる制御システム、ロボットシステム、及び制御方法を提供する。

本発明の一態様は、対象物に所定のパターン光を投影する投影部と、前記投影部により前記所定のパターン光が投影された前記対象物を撮像する第１撮像部と、前記第１撮像部と異なる位置に配置され、前記投影部により前記所定のパターン光が投影された前記対象物を撮像する第２撮像部と、前記第１撮像部が撮像した第１撮像画像上の第１点と、前記第２撮像部が撮像した第２撮像画像上の第２点とにより、前記対象物の３次元形状を算出する算出部と、を含む制御システムである。
この構成により、制御システムは、対象物に所定のパターン光を投影し、投影部により所定のパターン光が投影された対象物を第１撮像部により撮像し、投影部により所定のパターン光が投影された対象物を、第１撮像部と異なる位置に配置された第２撮像部により撮像し、第１撮像部が撮像した第１撮像画像上の第１点と、第２撮像部が撮像した第２撮像画像上の第２点とにより、対象物の３次元形状を算出する。これにより、制御システムは、多様な３次元形状の計測を行うことができる。

また、本発明の他の態様は、制御システムにおいて、前記第１撮像画像と、前記第２撮像画像に基づいて、前記第１撮像画像上のある点が示す位置と、前記第２撮像画像上の各点が示す位置とが同じ位置を示していることの確からしさの指標となる信頼度を算出する信頼度算出部と、前記信頼度算出部により算出された前記信頼度に基づいて、前記第１撮像画像上の各点と、前記第２撮像画像上の各点との対応付けを行う対応付部と、を含む、構成が用いられてもよい。
この構成により、制御システムは、第１撮像画像と、第２撮像画像に基づいて、第１撮像画像上のある点が示す位置と、第２撮像画像上の各点が示す位置とが同じ位置を示していることの確からしさの指標となる信頼度を算出し、算出された信頼度に基づいて、第１撮像画像上の各点と、第２撮像画像上の各点との対応付けを行う。これにより、制御システムは、２つの撮像部の間のキャリブレーションを行うことができる。

また、本発明の他の態様は、制御システムにおいて、前記信頼度算出部は、前記第１撮像画像の各点それぞれに対応する第１尤度関数と、前記第２撮像画像の各点それぞれに対応する第２尤度関数とを導出し、導出された第１尤度関数及び第２尤度関数に基づいて前記信頼度を算出する、構成が用いられてもよい。
この構成により、制御システムは、第１撮像画像の各点それぞれに対応する第１尤度関数と、第２撮像画像の各点それぞれに対応する第２尤度関数とを導出し、導出された第１尤度関数及び第２尤度関数に基づいて信頼度を算出する。これにより、制御システムは、尤度関数に基づいて信頼度を算出し、算出された信頼度に基づいて２つの撮像部の間のキャリブレーションを行うことができる。

また、本発明の他の態様は、制御システムにおいて、前記パターン光は、輝度又は色相がある方向に周期的に変化するパターン光であり、前記信頼度算出部は、前記第１撮像画像上の各点における前記パターン光の輝度又は色相の変化に係る第１位相と、前記第２撮像画像上の各点における前記パターン光の輝度又は色相の変化に係る第２位相とを算出し、算出された前記第１位相に基づいて前記第１尤度関数を導出し、前記第２位相に基づいて前記第２尤度関数を導出する、構成が用いられてもよい。
この構成により、制御システムは、第１撮像画像上の各点におけるパターン光の輝度又は色相の変化に係る第１位相と、第２撮像画像上の各点におけるパターン光の輝度又は色相の変化に係る第２位相とを算出し、算出された第１位相に基づいて第１尤度関数を導出し、第２位相に基づいて第２尤度関数を導出する。これにより、制御システムは、パターン光の輝度又は色相の変化に係る位相に基づいて尤度関数を導出して信頼度を算出することができる。

また、本発明の他の態様は、上記のうちいずれかの制御システムと、ロボットとを具備するロボットシステムであって、前記制御システムは、前記算出部により算出された前記対象物の前記３次元形状に基づいて前記ロボットを制御するロボット制御部を備える、ロボットシステムである。
この構成により、ロボットシステムは、算出部により算出された前記対象物の前記３次元形状に基づいて前記ロボットを制御する。これにより、ロボットシステムは、ロバストな３次元形状の計測を行うことができる。

また、本発明の他の態様は、対象物に所定のパターン光を投影し、前記所定のパターン光が投影された前記対象物を第１撮像部により撮像し、前記所定のパターン光が投影された前記対象物を、前記第１撮像部と異なる位置に配置された第２撮像部により撮像し、前記第１撮像部が撮像した第１撮像画像上の第１点と、前記第２撮像部が撮像した第２撮像画像上の第２点とにより、前記対象物の３次元形状を算出する、を含む制御方法である。
この構成により、制御方法は、対象物に所定のパターン光を投影し、投影部により所定のパターン光が投影された対象物を第１撮像部により撮像し、投影部により所定のパターン光が投影された対象物を、第１撮像部と異なる位置に配置された第２撮像部により撮像し、第１撮像部が撮像した第１撮像画像上の第１点と、第２撮像部が撮像した第２撮像画像上の第２点とにより、対象物の３次元形状を算出する。これにより、制御方法は、ロバストな３次元形状の計測を行うことができる。
また、本発明の他の態様は、対象物に所定のパターン光を投影する投影部と、前記投影部により前記所定のパターン光が投影された前記対象物を撮像する第１撮像部と、前記第１撮像部と異なる位置に配置され、前記投影部により前記所定のパターン光が投影された前記対象物を撮像する第２撮像部と、前記第１撮像部が撮像した第１撮像画像の各点それぞれに対応する第１尤度関数と、前記第２撮像部が撮像した第２撮像画像の各点それぞれに対応する第２尤度関数とを導出し、導出した前記第１尤度関数及び前記第２尤度関数に基づいて、前記第１撮像画像上のある点が示す位置と、前記第２撮像画像上の各点が示す位置とが同じ位置を示していることの確からしさの指標となる信頼度を算出する信頼度算出部と、前記信頼度算出部により算出された前記信頼度に基づいて、前記第１撮像画像上の各点と、前記第２撮像画像上の各点との対応付けを行う対応付部と、前記第１撮像画像上の第１点と、前記第２撮像画像上の第２点とにより、前記対象物の３次元形状を算出する算出部と、を含む制御システムである。
また、本発明の他の態様は、対象物に所定のパターン光を投影し、前記所定のパターン光が投影された前記対象物を第１撮像部により撮像し、前記所定のパターン光が投影された前記対象物を、前記第１撮像部と異なる位置に配置された第２撮像部により撮像し、前記第１撮像部が撮像した第１撮像画像の各点それぞれに対応する第１尤度関数と、前記第２撮像部が撮像した第２撮像画像の各点それぞれに対応する第２尤度関数とを導出し、導出した前記第１尤度関数及び前記第２尤度関数に基づいて、前記第１撮像画像上のある点が示す位置と、前記第２撮像画像上の各点が示す位置とが同じ位置を示していることの確からしさの指標となる信頼度を算出し、算出された前記信頼度に基づいて、前記第１撮像画像上の各点と、前記第２撮像画像上の各点との対応付けを行い、前記第１撮像部が撮像した第１撮像画像上の第１点と、前記第２撮像部が撮像した第２撮像画像上の第２点とにより、前記対象物の３次元形状を算出する、制御方法である。

以上により、制御システム、ロボットシステム、及び制御方法は、対象物に所定のパターン光を投影し、投影部により所定のパターン光が投影された対象物を第１撮像部により撮像し、投影部により所定のパターン光が投影された対象物を、第１撮像部と異なる位置に配置された第２撮像部により撮像し、第１撮像部が撮像した第１撮像画像上の第１点と、第２撮像部が撮像した第２撮像画像上の第２点とにより、対象物の３次元形状を算出する。これにより、制御システム、ロボットシステム、及び制御方法は、ロバストな３次元形状の計測を行うことができる。

本実施形態に係るロボットシステム１の一例を示す構成図である。 投影部５により投影される投影画像ＰＡ及び投影画像ＰＢを例示する図である。 制御装置３０のハードウェア構成の一例を示す図である。 制御装置３０の機能構成の一例を示す図である。 制御装置３０が撮像部間キャリブレーションを行うまでの処理の流れの一例を示すフローチャートである。 信頼度算出部４０による信頼度算出処理の流れの一例を示すフローチャートである。 信頼度算出部４０によるステップＳ２２０の処理を説明するための図である。 尤度関数の関数形の一例を示す図である。 制御装置３０がロボット２０に所定の作業を行わせる処理の流れの一例を示すフローチャートである。

＜実施形態＞
以下、本発明の本実施形態について、図面を参照して説明する。図１は、本実施形態に係るロボットシステム１の一例を示す構成図である。ロボットシステム１は、投影部５と、撮像部１０と、ロボット２０と、制御装置３０を具備する。また、撮像部１０は、第１撮像部１１と、第２撮像部１２を備えるステレオカメラである。なお、撮像部１０は、３台以上の撮像部を備える構成であってもよい。

また、図１において、テーブルＴＢ上には、対象物Ｏが設置されている。ここで、テーブルＴＢとは、対象物Ｏが設置される台等である。対象物Ｏとは、ロボット２０により行われる作業に係る物体であり、例えば、ネジやボルト、歯車、治具等の工業用の部品や、工業部品等を入れる容器等であるが、これらに限られず、ロボット２０により行われる作業に係る物体であれば他の物体であってもよい。なお、対象物Ｏは、投影範囲Ｐ１と撮像範囲Ｐ２とが重なり合った領域の内側に全体が含まれる程度の大きさである。図１では、この対象物Ｏを、直方体形状の物体として表す。

ロボットシステム１は、予め記憶された画像であって一方が４枚で他方が４枚の合せて８枚の画像から１枚ずつ画像を投影画像として選択する。そして、ロボットシステム１は、選択された投影画像をテーブルＴＢ上に設置された対象物Ｏを含む投影範囲Ｐ１に投影するように投影部５を制御する。ここで、前述の８枚の画像とは、それぞれの画像平面上で輝度が周期的に変化し、輝度の周期的変化を表す波が画像平面上の水平方向に延在している画像である。なお、これら８枚の画像はそれぞれ、この輝度の周期的変化を表す波が画像平面上の垂直方向や、斜め方向等に延在している画像であってもよい。

ここで、予め記憶された８枚の画像のうちの一方の４枚の画像は、輝度の周期的変化を表す波が、所定の波数Ａの波である。これら４枚の画像では、輝度の周期的変化を表す波が、互いに位相がπ／２だけ異なる。より具体的には、基準となる画像をｎ＝０として、ｎ＝１の画像では基準となる画像からπ／２だけ位相が異なり、ｎ＝２の画像では基準となる画像からπだけ位相が異なり、ｎ＝３の画像では基準となる画像から３π／２だけ位相が異なる。ここで、以下では、説明の便宜上、ｎ＝０の画像を投影画像ＰＡ０、ｎ＝１の画像を投影画像ＰＡ１、ｎ＝２の画像を投影画像ＰＡ２、ｎ＝３の画像を投影画像ＰＡ３と称して説明する。また、以下では、投影画像ＰＡ０〜ＰＡ３を区別して説明する必要が無い限り、これらをまとめて投影画像ＰＡと称して説明する。なお、投影画像ＰＡは、互いに位相がπ／２とは異なる値だけ異なっていてもよい。

また、予め記憶された８枚の投影画像のうちの他方の４枚の画像は、輝度の周期的変化を表す波が、波数が波数Ａとは異なる所定の波数Ｂの波である。これら４枚の画像では、輝度の周期的変化を表す波が、互いに位相がπ／２だけ異なる。より具体的には、基準となる画像をｎ＝０として、ｎ＝１の画像では基準となる画像からπ／２だけ位相が異なり、ｎ＝２の画像では基準となる画像からπだけ位相が異なり、ｎ＝３の画像では基準となる画像から３π／２だけ位相が異なる。ここで、以下では、説明の便宜上、ｎ＝０の画像を投影画像ＰＢ０、ｎ＝１の画像を投影画像ＰＢ１、ｎ＝２の画像を投影画像ＰＢ２、ｎ＝３の画像を投影画像ＰＢ３と称して説明する。また、以下では、投影画像ＰＢ０〜ＰＢ３を区別して説明する必要が無い限り、これらをまとめて投影画像ＰＢと称して説明する。なお、投影画像ＰＢは、互いに位相がπ／２とは異なる値だけ異なっていてもよい。

ここで、図２を参照して、投影部５により投影される投影画像について説明する。図２は、投影部５により投影される投影画像ＰＡ及び投影画像ＰＢを例示する図である。投影画像ＰＡ０〜ＰＡ３は、輝度の周期的変化を表す波の位相が互いにπ／２だけ異なる。投影画像ＰＡ０〜ＰＡ３中の明度が高い点は、輝度値が高い点であり、明度が低い点は、輝度値が低い点である。

投影画像ＰＡ０〜ＰＡ３における輝度の周期的変化を表す波は、投影画像平面上の水平方向に進行する平面波（正弦波）であるが、前述したように垂直方向に進行する平面波であってもよく、斜めに進行する平面波であってもよく、球面波等であってもよい。ここで、投影画像ＰＡ０〜ＰＡ３は、輝度が周期的に変化する画像に代えて、色彩が周期的に変化する画像等であってもよい。

また、投影画像ＰＢ０〜ＰＢ３が表す輝度の変化の波は、投影画像ＰＡ０〜ＰＡ３が表す輝度の変化の波と波数が異なる。投影画像ＰＢ０〜ＰＢ３は、輝度の周期的変化を表す波の位相が互いにπ／２だけ異なる。投影画像ＰＢ０〜ＰＢ３中の明度が高い点は、輝度値が高い点であり、明度が低い点は、輝度値が低い点である。投影画像ＰＢ０〜ＰＢ３における輝度の周期的変化を表す波は、前述したように投影画像平面上の水平方向に進行する平面波（正弦波）であるが、垂直方向に進行する平面波であってもよく、斜めに進行する平面波であってもよく、球面波等であってもよい。

ここで、投影画像ＰＢ０〜ＰＢ３は、輝度が周期的に変化する画像に代えて、色彩が周期的に変化する画像等であってもよい。なお、投影画像ＰＡと投影画像ＰＢのうちいずれか一方又は両方は、位相が互いに異なる４枚の画像に代えて、位相が互いに異なる３枚以上の画像であれば他の枚数であっても良い。その場合、互いに異なる位相は、π／２とは異なる値であることが望ましい。これらの投影画像ＰＡ及び投影画像ＰＢは、所定のパターン光の一例である。

予め記憶された８枚の画像の中から選択された１枚のある投影画像がテーブルＴＢ上に設置された対象物Ｏを含む投影範囲Ｐ１に投影された後、ロボットシステム１は、テーブルＴＢ上に設置された対象物Ｏを含む撮像範囲Ｐ２を、撮像部１０の第１撮像部１１及び第２撮像部１２により撮像する。以下では、説明の便宜上、第１撮像部１１により撮像される撮像画像を第１撮像画像と称し、第２撮像部１２により撮像される撮像画像を第２撮像画像と称して説明する。

ロボットシステム１は、これらの投影部５による投影と撮像部１０の第１撮像部１１及び第２撮像部１２による撮像とを、前述の予め記憶された８枚の画像それぞれに対して行う。これにより、ロボットシステム１は、８枚の第１撮像画像（投影画像ＰＡに係る第１撮像画像が４枚、投影画像ＰＢに係る第１撮像画像が４枚）と、８枚の第２撮像画像（投影画像ＰＡに係る第２撮像画像が４枚、投影画像ＰＢに係る第２撮像画像が４枚）を取得する。

ロボットシステム１は、第１撮像部１１により撮像された８枚の第１撮像画像と、第２撮像部１２により撮像された８枚の第２撮像画像とを用いたダブル位相シフト法に基づいて、第１撮像部１１により撮像される第１撮像画像平面上の座標と、第２撮像部１２により撮像される第２撮像画像平面上の座標とを対応付けるためのキャリブレーションを行う。なお、本実施形態において、第１撮像画像平面上の座標とは、第１撮像部１１により撮像される第１撮像画像の各点を示す座標を示し、例えば、第１撮像部１１の撮像素子平面上の座標のことである。また、第２撮像画像平面上の座標とは、第２撮像部１２により撮像される第２撮像画像の各点を示す座標を示し、例えば、第２撮像部１２の撮像素子平面上の座標のことである。

なお、以下では、説明の便宜上、投影画像ＰＡ０〜ＰＡ３に係る第１撮像画像をそれぞれ順に、第１撮像画像１Ａ０〜１Ａ３と称し、投影画像ＰＢ０〜ＰＢ３に係る第１撮像画像をそれぞれ順に、第１撮像画像１Ｂ０〜１Ｂ３と称して説明する。また、以下では、投影画像ＰＡ０〜ＰＡ３に係る第２撮像画像をそれぞれ順に、第２撮像画像２Ａ０〜２Ａ３と称し、投影画像ＰＢ０〜ＰＢ３に係る第２撮像画像をそれぞれ順に、第２撮像画像２Ｂ０〜２Ｂ３と称して説明する。

ロボットシステム１は、第１撮像画像平面上の座標と、第２撮像画像平面上の座標とを、投影部５により投影される投影画像平面上の座標との対応付け（予め行われる撮像部と投影部との間のキャリブレーション）を介して対応付ける場合と比べて、より高い精度で第１撮像画像平面上の座標と、第２撮像画像平面上の座標とを対応付けることができる。なお、本実施形態において、投影画像平面上の座標とは、投影部５により投影される各種の投影画像の各点を示す座標を示し、例えば、投影部５の液晶パネル平面上の座標のことである。

このキャリブレーションを終えた後、ロボットシステム１は、テーブルＴＢ上に設置された対象物Ｏを第１撮像部１１及び第２撮像部１２によりステレオ撮像し、撮像された第１撮像画像及び第２撮像画像に基づくステレオ法によって対象物Ｏの３次元点群を生成する。そして、ロボットシステム１は、生成された３次元点群に基づいて対象物Ｏの３次元位置及び姿勢を算出する。これにより、ロボットシステム１は、対象物Ｏにおいて投影部５の光が届かない影の部分がある場合であっても、アクティブステレオ法に代えて、第１撮像画像と第２撮像画像に基づくステレオ法によって精度よく対象物Ｏの位置及び姿勢を算出することができる。以下では、説明の便宜上、対象物Ｏの３次元位置及び姿勢を対象物位置姿勢と称して説明する。

また、ロボットシステム１は、対象物Ｏにおいて第１撮像部１１と第２撮像部１２のうちいずれか一方で撮像できない影の部分がある場合（すなわち、第１撮像部１１及び第２撮像部１２によるステレオ撮像が出来ないような場合）、第１撮像部１１と第２撮像部１２のうちいずれか一方により撮像された撮像画像と、投影部５により投影された投影画像とに基づいたアクティブステレオ法によって対象物位置姿勢を算出する。これにより、ロボットシステム１は、対象物Ｏにおいて第１撮像部１１と第２撮像部１２のうちいずれか一方で撮像できない影の部分がある場合であっても、ステレオ法に代えて、第１撮像部１１と第２撮像部１２のうちいずれか一方と投影部５とに基づくアクティブステレオ法によって精度よく対象物位置姿勢を算出することができる。

ロボットシステム１は、対象物位置姿勢を算出すると、例えば、算出された対象物位置姿勢に基づいてロボット２０に対象物Ｏを把持させる。そして、ロボットシステム１は、ロボット２０に対象物Ｏを図示しない所定の配置位置Ｘに配置させる。以下では、説明の便宜上、キャリブレーションの後に第１撮像部１１及び第２撮像部１２による対象物Ｏのステレオ撮像から、対象物Ｏを配置位置Ｘに配置させるまでのロボットシステム１の一連の動作を所定の作業と称して説明する。

投影部５は、例えば、投影画像を投影するための液晶ライトバルブや投影レンズ、液晶駆動部、光源として超高圧水銀ランプやメタルハライドランプ等を備えるプロジェクターである。投影部５は、例えばケーブルによって制御装置３０と通信可能に接続されている。ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ（Universal Serial Bus）等の規格によって行われる。

なお、投影部５と制御装置３０とは、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって接続されてもよい。投影部５は、通信により制御装置３０から各種画像を取得し、取得した各種画像を投影画像として投影範囲Ｐ１に投影する。図１において、投影部５は、投影範囲Ｐ１が撮像範囲Ｐ２を内側に含むように設置されているが、投影範囲Ｐ１と撮像範囲Ｐ２との大きさは逆であってもよく、投影範囲Ｐ１と撮像範囲Ｐ２とが部分的に重なり合っていてもよい。

第１撮像部１１は、例えば、集光された光を電気信号に変換する撮像素子であるＣＣＤ（Charge Coupled Device）やＣＭＯＳ（Complementary Metal Oxide Semiconductor）等を備えたカメラである。第１撮像部１１は、ケーブルによって制御装置３０と通信可能に接続されている。ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ等の規格によって行われる。

なお、第１撮像部１１と制御装置３０とは、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって接続される構成であってもよい。第１撮像部１１は、投影範囲Ｐ１よりも狭い範囲であって投影部５により投影画像が投影された対象物Ｏを含む撮像範囲Ｐ２を撮像可能な位置に設置される。

第２撮像部１２は、例えば、集光された光を電気信号に変換する撮像素子であるＣＣＤやＣＭＯＳ等を備えたカメラである。第２撮像部１２は、ケーブルによって制御装置３０と通信可能に接続されている。ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ等の規格によって行われる。

なお、第２撮像部１２と制御装置３０とは、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって接続される構成であってもよい。第２撮像部１２は、第１撮像部１１とは異なる位置であって前述した撮像範囲Ｐ２を撮像可能な位置に設置される。

ロボット２０は、例えば、物体を把持可能な爪部を備える把持部ＨＮＤと、マニピュレーターＭＮＰと、図示しない複数のアクチュエーターを備えた単腕のロボットである。単腕のロボットとは、把持部ＨＮＤとマニピュレーターＭＮＰにより構成される１本の腕を有するロボットを示す。また、ロボットシステム１は、単腕のロボットを具備する構成に代えて、スカラーロボット（水平多関節ロボット）や双腕のロボット等であってもよい。スカラーロボットとは、マニピュレーターが水平方向にのみ動き、マニピュレーターの先端のスライド軸のみが上下に動くロボットである。また、双腕のロボットとは、把持部ＨＮＤとマニピュレーターＭＮＰによりそれぞれ構成される２本の腕を有するロボットを示す。

ロボット２０の腕は、６軸垂直多関節型となっており、支持台とマニピュレーターＭＮＰと把持部ＨＮＤとがアクチュエーターによる連携した動作よって６軸の自由度の動作を行うことができる。なお、ロボット２０の腕は、５自由度（５軸）以下で動作するものであってもよいし、７自由度（７軸）以上で動作するものであってもよい。以下では、把持部ＨＮＤ及びマニピュレーターＭＮＰを備えた腕によって行われるロボット２０の動作について説明する。

ロボット２０は、例えばケーブルによって制御装置３０と通信可能に接続されている。
ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ等の規格によって行われる。なお、ロボット２０と制御装置３０とは、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって接続されてもよい。また、ロボットシステム１では、ロボット２０は、図１に示したようにロボット２０の外部に設置された制御装置３０と接続されている構成とするが、この構成に代えて、制御装置３０がロボット２０に内蔵されている構成であってもよい。ロボット２０は、制御装置３０から制御信号を取得し、取得した制御信号に基づいて対象物Ｏを把持し、把持した対象物Ｏを配置位置Ｘに配置する。

制御装置３０は、予め記憶された８枚の画像から１枚ずつ投影画像を選択し、選択された投影画像を投影範囲Ｐ１に投影するように投影部５を制御する。投影範囲Ｐ１に選択された投影画像が投影された後、制御装置３０は、撮像範囲Ｐ２の静止画像を第１撮像画像として撮像するように第１撮像部１１を制御する。また、制御装置３０は、撮像範囲Ｐ２の静止画像を第２撮像画像として撮像するように第２撮像部１２を制御する。これらの処理を投影画像の枚数分（この一例において、８枚）だけ繰り返すことで、制御装置３０は、８枚の第１撮像画像と、８枚の第２撮像画像とを取得する。

そして、制御装置３０は、８枚の第１撮像画像と、８枚の第２撮像画像とを用いたダブル位相シフト法に基づいて、第１撮像部１１によって撮像される第１撮像画像平面上の座標と、第２撮像部１２によって撮像される第２撮像画像平面上の座標とを対応付けるためのキャリブレーションを行う。以下では、説明の便宜上、このキャリブレーションを、撮像部間キャリブレーションと称して説明する。

なお、投影部５により投影される投写画像平面上の座標と、第１撮像部１１により撮像される第１撮像画像平面上の座標とは、予めキャリブレーションによって対応付けられているとする。また、投影部５により投影される投影画像平面上の座標と、第２撮像部１２により撮像される第２撮像画像平面上の座標とは、予めキャリブレーションによって対応付けられているとする。なお、これらのキャリブレーションは、前述した撮像部間キャリブレーションと処理内容が異なるキャリブレーションである。

制御装置３０は、撮像部間キャリブレーションを終えた後、テーブルＴＢ上に設置された対象物Ｏをステレオ撮像するように第１撮像部１１及び第２撮像部１２を制御する。そして、制御装置３０は、撮像された第１撮像画像及び第２撮像画像に基づくステレオ法によって対象物Ｏの３次元点群を生成する。制御装置３０は、生成された３次元点群に基づいて対象物位置姿勢を算出する。制御装置３０は、対象物位置姿勢を算出すると、例えば、算出された対象物位置姿勢に基づいてロボット２０に対象物Ｏを把持させる。そして、制御装置３０は、ロボット２０に対象物Ｏを図示しない所定の配置位置Ｘに配置させる。なお、投影部５と、撮像部１０と、制御装置３０との組み合わせは、制御システムの一例である。

次に、図３を参照して、制御装置３０のハードウェア構成について説明する。図３は、制御装置３０のハードウェア構成の一例を示す図である。制御装置３０は、例えば、ＣＰＵ（Central Processing Unit）３１と、記憶部３２と、入力受付部３３と、通信部３４を備え、通信部３４を介してロボット２０と通信を行う。これらの構成要素は、バスＢｕｓを介して相互に通信可能に接続されている。ＣＰＵ３１は、記憶部３２に格納された各種プログラムを実行する。

記憶部３２は、例えば、ＨＤＤ（Hard Disk Drive）やＳＳＤ（Solid State Drive）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read−Only Memory）、ＲＯＭ（Read−Only Memory）、ＲＡＭ（Random Access Memory）などを含み、制御装置３０が処理する各種情報や画像、プログラム、投影部５に投影させるための８枚の画像（投影画像ＰＡ、及び投影画像ＰＢ）等を格納する。なお、記憶部３２は、制御装置３０に内蔵されるものに代えて、ＵＳＢ等のデジタル入出力ポート等によって接続された外付け型の記憶装置でもよい。

入力受付部３３は、例えば、キーボードやマウス、タッチパッド、その他の入力装置である。なお、入力受付部３３は、表示部として機能することでタッチパネルとして構成されてもよい。
通信部３４は、例えば、ＵＳＢ等のデジタル入出力ポートやイーサネット（登録商標）ポート等を含んで構成される。

次に、図４を参照して、制御装置３０の機能構成について説明する。図４は、制御装置３０の機能構成の一例を示す図である。制御装置３０は、記憶部３２と、画像取得部３５と、制御部３６を備える。制御部３６が備える機能部のうち一部又は全部は、例えば、ＣＰＵ３１が、記憶部３２に記憶された各種プログラムを実行することで実現される。また、これらの機能部のうち一部または全部は、ＬＳＩ（Large Scale Integration）やＡＳＩＣ（Application Specific Integrated Circuit）等のハードウェア機能部であってもよい。

画像取得部３５は、撮像部１０から撮像画像を取得する。より具体的には、画像取得部３５は、第１撮像部１１から第１撮像画像を取得する。画像取得部３５は、取得された第１撮像画像を制御部３６に出力する。また、画像取得部３５は、第２撮像部１２から第２撮像画像を取得する。画像取得部３５は、取得された第２撮像画像を制御部３６に出力する。

制御部３６は、制御装置３０の全体を制御する。制御部３６は、撮像制御部３７と、投影制御部３８と、位相算出部３９と、信頼度算出部４０と、対応付部４１と、３次元点群生成部４２と、３次元位置姿勢導出部４３と、ロボット制御部４４を備える。
撮像制御部３７は、撮像部１０に撮像範囲Ｐ２を撮像させる。より具体的には、撮像制御部３７は、第１撮像部１１に撮像範囲Ｐ２を撮像させる。また、撮像制御部３７は、第２撮像部１２に撮像範囲Ｐ２を撮像させる。
投影制御部３８は、記憶部３２から画像を読み込み、読み込まれた画像を投影画像として投影部５に出力し、投影範囲Ｐ１に投影画像を投影するように投影部５を制御する。

位相算出部３９は、画像取得部３５から取得された８枚の第１撮像画像のうち、第１撮像画像１Ａ０〜１Ａ３に基づいて、第１撮像部１１の第１撮像画像平面上の座標と、投影部５の投影画像平面上の座標とを対応付けるために必要な位相を算出する。ここでいう位相とは、図２に示した４枚の投影画像ＰＡ０〜ＰＡ３のうち基準となるｎ＝０の投影画像（この一例において、投影画像ＰＡ０）が投影された際に、投影画像平面上の座標が示す各点における輝度の周期的変化を表す波の位相である。以下、投影画像ＰＡに係る第１撮像画像に基づいて算出されたこの位相を、第１Ａ位相θ１Ａと称して説明する。

また、位相算出部３９は、画像取得部３５から取得された８枚の第１撮像画像のうち、第１撮像画像１Ｂ０〜１Ｂ３に基づいて、第１撮像部１１の第１撮像画像平面上の座標と、投影部５の投影画像平面上の座標とを対応付けるために必要な位相を算出する。ここでいう位相とは、４枚の投影画像ＰＢ０〜ＰＢ３のうち基準となるｎ＝０の投影画像（この一例において、投影画像ＰＢ０）が投影された際に、投影画像平面上の座標が示す各点における輝度の周期的変化を表す波の位相である。以下、投影画像ＰＢに係る第１撮像画像に基づいて算出されたこの位相を、第１Ｂ位相φ１Ｂと称して説明する。

また、位相算出部３９は、画像取得部３５から取得された８枚の第２撮像画像のうち、第２撮像画像２Ａ０〜２Ａ３に基づいて、第２撮像部１２の第２撮像画像平面上の座標と、投影部５の投影画像平面上の座標とを対応付けるために必要な位相を算出する。ここでいう位相とは、４枚の投影画像ＰＡ０〜ＰＡ３のうち基準となるｎ＝０の投影画像（この一例において、投影画像ＰＡ０）が投影された際に、投影画像平面上の座標が示す各点における輝度の周期的変化を表す波の位相である。以下、投影画像ＰＡに係る第２撮像画像に基づいて算出されたこの位相を、第２Ａ位相θ２Ａと称して説明する。

また、位相算出部３９は、画像取得部３５から取得された８枚の第２撮像画像のうち、第２撮像画像２Ｂ０〜２Ｂ３に基づいて、第２撮像部１２の第２撮像画像平面上の座標と、投影部５の投影画像平面上の座標とを対応付けるために必要な位相を算出する。ここでいう位相とは、４枚の投影画像ＰＢ０〜ＰＢ３のうち基準となるｎ＝０の投影画像（この一例において、投影画像ＰＢ０）が投影された際に、投影画像平面上の座標が示す各点における輝度の周期的変化を表す波の位相である。以下、投影画像ＰＢに係る第２撮像画像に基づいて算出されたこの位相を、第２Ｂ位相φ２Ｂと称して説明する。

信頼度算出部４０は、位相算出部３９により算出された第１Ａ位相θ１Ａと、第１Ｂ位相φ１Ｂと、第２Ａ位相θ２Ａと、第２Ｂ位相φ２Ｂとに基づいて、第１撮像画像平面上のある座標が示す位置と、第２撮像画像平面上の各座標それぞれが示す位置とが同じ位置であることの確からしさを示す指標となる信頼度を算出する。以下では、説明の便宜上、第１撮像画像平面上のある座標Ｍが示す位置と同じ位置を示す第２撮像画像平面上のある座標Ｎのことを、第１撮像画像平面上の座標Ｍに対応する第２撮像画像平面上の座標Ｎと称して説明する。信頼度算出部４０は、この信頼度を算出する処理を繰り返すことで、位相算出部３９により算出された第１Ａ位相θ１Ａと、第１Ｂ位相φ１Ｂと、第２Ａ位相θ２Ａと、第２Ｂ位相φ２Ｂとに基づいて、第１撮像画像座標の各座標それぞれに対して、第２撮像画像座標の各座標それぞれとの間の信頼度をすべて算出する。

対応付部４１は、信頼度算出部４０により算出された信頼度に基づいて、第１撮像画像平面上の各座標に対応する第２撮像画像平面上の座標を検出する。そして、対応付部４１は、検出された第１撮像画像平面上の各座標に対応する第２撮像画像平面上の座標と、第１撮像画像平面上の各座標とを対応付けることで撮像部間キャリブレーションを行う。

より具体的には、対応付部４１は、信頼度算出部４０により算出された信頼度であって第１撮像画像平面上の各座標それぞれに対する第２撮像画像平面上の各座標との間の信頼度に基づいて、第１撮像画像平面上における座標毎に、第１撮像画像平面上における座標と第２撮像画像平面上における座標との組み合わせのうち最も信頼度が高い組み合わせを検出する。そして、対応付部４１は、第１撮像画像平面上における座標毎に、第１撮像画像平面上における座標と第２撮像画像平面上における座標との組み合わせのうち最も信頼度が高い組み合わせに係る２つの座標（すなわち、第１撮像画像平面上の座標と、第２撮像画像平面上の座標）同士を対応付けることで撮像部間キャリブレーションを行う。

３次元点群生成部４２は、画像取得部３５から取得された第１撮像部１１及び第２撮像部１２によるステレオ撮像画像に基づいて対象物Ｏの３次元点群を生成する。
３次元位置姿勢導出部４３は、３次元点群生成部４２により生成された対象物Ｏの３次元点群に基づいて対象物位置姿勢を算出する。
ロボット制御部４４は、３次元位置姿勢導出部４３により算出された対象物位置姿勢に基づいて把持部ＨＮＤにより対象物Ｏを把持するようにロボット２０を制御する。ロボット２０が把持部ＨＮＤにより対象物Ｏを把持した後、ロボット制御部４４は、対象物Ｏを配置位置Ｘに配置するようにロボット２０を制御する。

以下、図５を参照して、制御装置３０が撮像部間キャリブレーションを行うまでの処理について説明する。図５は、制御装置３０が撮像部間キャリブレーションを行うまでの処理の流れの一例を示すフローチャートである。まず、投影制御部３８は、予め記憶部３２に記憶された８枚の画像（すなわち、前述の４枚の投影画像ＰＡ及び４枚の投影画像ＰＢ）を、記憶部３２から読み込む（ステップＳ１００）。

次に、投影制御部３８は、読み込まれた投影画像を１枚ずつ選択し、選択された投影画像毎にステップＳ１２０からステップＳ１４０までの処理を繰り返し行う（ステップＳ１１０）。なお、投影制御部３８は、４枚の投影画像ＰＡから１枚ずつ投影画像を選択した後、４枚の投影画像ＰＢから１枚ずつ投影画像を選択する構成であるとするが、これに代えて、他の順番で投影画像を選択する構成であってもよい。

ステップＳ１１０で投影画像が選択された後、投影制御部３８は、選択された投影画像を投影範囲Ｐ１に投影するように投影部５を制御する（ステップＳ１２０）。次に、撮像制御部３７は、撮像範囲Ｐ２を撮像するように第１撮像部１１及び第２撮像部１２を制御する（ステップＳ１３０）。なお、撮像制御部３７は、第１撮像部１１と第２撮像部１２のうちいずれか一方が先に撮像するように第１撮像部１１及び第２撮像部１２を制御してもよく、第１撮像部１１と第２撮像部１２が同時に撮像するように第１撮像部１１及び第２撮像部１２を制御してもよい。

次に、画像取得部３５は、第１撮像部１１から第１撮像画像を取得し、第２撮像部１２から第２撮像画像を取得する（ステップＳ１４０）。そして、画像取得部３５は、第１撮像部１１及び第２撮像部１２から取得された第１撮像画像及び第２撮像画像を制御部３６に出力する。

ステップＳ１１０からステップＳ１４０までの処理によって８枚の第１撮像画像と、８枚の第２撮像画像とが制御部３６に出力された後、位相算出部３９は、取得された８枚の第１撮像画像に基づいて、第１撮像部１１の第１撮像画像平面上の座標と、投影部５の投影画像平面上の座標とを対応付けるために必要な位相を算出し、取得された８枚の第２撮像画像に基づいて、第２撮像部１２の第２撮像画像平面上の座標と、投影部５の投影画像平面上の座標とを対応付けるために必要な位相を算出する（ステップＳ１５０）。

より具体的には、位相算出部３９は、取得された８枚の第１撮像画像のうち第１撮像画像１Ａ０〜１Ａ３に基づいて位相シフト法により、第１撮像画像１Ａ０が撮像された際の第１撮像画像平面上の各座標における位相を算出する。以下では、第１撮像画像１Ａ０が撮像された際の第１撮像画像平面上の各座標における位相をまとめて第１Ａ位相θ１Ａと称して説明する。

また、位相算出部３９は、取得された８枚の第１撮像画像のうち第１撮像画像１Ｂ０〜１Ｂ３に基づいて、第１撮像画像１Ｂ０が撮像された際の第１撮像画像平面上の各座標における位相を算出する。以下では、第１撮像画像１Ｂ０が撮像された際の第１撮像画像平面上の各座標における位相をまとめて第１Ｂ位相φ１Ｂと称して説明する。

また、位相算出部３９は、取得された８枚の第２撮像画像のうち第２撮像画像２Ａ０〜２Ａ３に基づいて、第２撮像画像２Ａ０が撮像された際の第２撮像画像平面上の各座標における位相を算出する。以下では、第２撮像画像２Ａ０が撮像された際の第２撮像画像平面上の各座標における位相をまとめて第２Ａ位相θ２Ａと称して説明する。

また、位相算出部３９は、取得された８枚の第２撮像画像のうち第２撮像画像２Ｂ０〜２Ｂ３に基づいて、第２撮像画像２Ｂ０が撮像された際の第２撮像画像平面上の各座標における位相を算出する。以下では、第２撮像画像２Ｂ０が撮像された際の第２撮像画像平面上の各座標における位相をまとめて第２Ｂ位相φ２Ｂと称して説明する。

ステップＳ１５０で位相算出部３９により各位相が算出された後、信頼度算出部４０は、ステップＳ１５０で位相算出部３９により算出された第１Ａ位相θ１Ａと、第１Ｂ位相φ１Ｂと、第２Ａ位相θ２Ａと、第２Ｂ位相φ２Ｂとに基づいて、第１撮像画像平面上のある座標が示す位置と、第２撮像画像平面上における各座標のそれぞれが示す位置とが同じ位置であることの確からしさを示す指標となる信頼度を算出する信頼度算出処理を行う（ステップＳ１６０）。この信頼度算出処理において、信頼度算出部４０は、この信頼度を算出する処理を繰り返すことで、位相算出部３９により算出された第１Ａ位相θ１Ａと、第１Ｂ位相φ１Ｂと、第２Ａ位相θ２Ａと、第２Ｂ位相φ２Ｂとに基づいて、第１撮像画像平面上の各座標それぞれに対して、第２撮像画像平面上の各座標との間の信頼度をすべて算出する。ここで、第１Ａ位相θ１Ａと第１Ｂ位相φ１Ｂは、第１位相の一例である。また、第２Ａ位相θ２Ａと第２Ｂ位相φ２Ｂは、第２位相の一例である。

次に、対応付部４１は、ステップＳ１６０で信頼度算出部４０により算出された信頼度であって第１撮像画像平面上の各座標それぞれに対する第２撮像画像平面上の各座標との間の信頼度に基づいて、第１撮像画像平面上における座標毎に、第１撮像画像平面上における座標と第２撮像画像平面上における座標との組み合わせのうち最も信頼度が高い組み合わせに係る座標同士を対応付けることで前述した撮像部間キャリブレーションを行う（ステップＳ１７０）。

ここで、図６を参照して、信頼度算出部４０による信頼度算出処理について説明する。
図６は、信頼度算出部４０による信頼度算出処理の流れの一例を示すフローチャートである。先に簡単な概略を説明すると、信頼度算出処理において、信頼度算出部４０は、ステップＳ２００からステップＳ２２０で抽出される最隣接組座標と、ステップＳ２３０からステップＳ２５０で抽出される最隣接組座標とに基づいて、前述の信頼度を算出する。

まず、信頼度算出部４０は、第１撮像画像平面上の座標を１つずつ選択し、選択された座標毎にステップＳ２１０からステップＳ２２０までの処理を繰り返し行う（ステップＳ２００）。ステップＳ２００で第１撮像画像平面上の座標が１つ選択された後、信頼度算出部４０は、選択された座標が示す点に対応する投影画像平面上の座標を抽出する（ステップＳ２１０）。より具体的には、信頼度算出部４０は、選択された座標が示す点であって第１撮像画像１Ａ０上の点における第１Ａ位相θ１Ａと同じ位相を有する投写画像ＰＡ０上の点を示す座標であって投写画像平面上のＸ座標（すなわち、上記の選択された座標が示す点に対応する投影画像平面上の座標）を抽出する。以下では、説明の便宜上、抽出されたこれらの座標を、座標Ｘ１＿θ１Ａ〜Ｘ４＿θ１Ａと称し、抽出された座標がこれらの４つのみであるとして説明するが、実際にはより多数の座標が抽出される。

ここで、投写画像ＰＡ０平面上のＸ座標毎の位相は、投写画像ＰＡを用いて位相シフト法により予め算出されているとする。なお、信頼度算出部４０は、投影画像ＰＡにおける輝度の周期的変化を表す波が投影画像平面上の水平方向に進行する平面波であるため、投写画像平面上のＸ座標を抽出する構成としたが、これに代えて、投影画像ＰＡにおける輝度の周期的変化を表す波の進行方向に応じた他の座標軸の座標を抽出する構成であってもよい。

また、信頼度算出部４０は、選択された座標が示す点であって第１撮像画像１Ｂ０上の点における第１Ｂ位相φ１Ｂと同じ位相を有する投写画像ＰＢ０平面上の点を示す座標であって投写画像平面上のＸ座標（すなわち、上記の選択された座標が示す点に対応する投影画像平面上の座標）を抽出する。以下では、説明の便宜上、抽出されたこれらの座標を、座標Ｘ５＿φ１Ｂ〜Ｘ７＿φ１Ｂの３つのみであるとして説明するが、実際にはより多数の座標が抽出される。

次に、信頼度算出部４０は、ステップＳ２１０で抽出されたＸ座標Ｘ１＿θ１Ａ〜Ｘ４＿θ１Ａと、抽出されたＸ座標Ｘ５＿φ１Ｂ〜Ｘ７＿φ１ＢとをＸ座標の値の順に並べた際に、最も隣接する組み合わせであってＸ座標Ｘ１＿θ１Ａ〜Ｘ４＿θ１Ａのうちのいずれか１つとＸ座標Ｘ５＿φ１Ｂ〜Ｘ７＿φ１Ｂのうちのいずれか１つとの組み合わせを抽出する（ステップＳ２２０）。

ここで、図７を参照して、この抽出処理について説明する。図７は、信頼度算出部４０によるステップＳ２２０の処理を説明するための図である。図７（Ａ）には、抽出されたＸ座標Ｘ１＿θ１Ａ〜Ｘ４＿θ１Ａと、抽出されたＸ座標Ｘ５＿φ１Ｂ〜Ｘ７＿φ１Ｂとを座標値に基づく順に、投影画像平面上のＸ座標を表す座標軸ＸＰ上に並べてプロットしたグラフの一例を示す。

信頼度算出部４０は、図７（Ａ）に示したグラフから、隣接する組み合わせであってＸ座標Ｘ１＿θ１Ａ〜Ｘ４＿θ１Ａのうちのいずれか１つと、Ｘ座標Ｘ５＿φ１Ｂ〜Ｘ７＿φ１Ｂのうちのいずれか１つとの組み合わせを抽出する。図７（Ａ）において、信頼度算出部４０は、Ｘ座標Ｘ１＿θ１ＡとＸ座標Ｘ５＿φ１Ｂ、Ｘ座標Ｘ５＿φ１ＢとＸ座標Ｘ２＿θ１Ａ、Ｘ座標Ｘ２＿θ１ＡとＸ座標Ｘ６＿φ１Ｂ、Ｘ座標Ｘ６＿φ１ＢとＸ座標Ｘ３＿θ１Ａ、Ｘ座標Ｘ３＿θ１ＡとＸ座標Ｘ７＿φ１Ｂ、Ｘ座標Ｘ７＿φ１ＢとＸ座標Ｘ４＿θ１Ａの６つの組み合わせを抽出する。そして、信頼度算出部４０は、抽出された組み合わせ毎に、組み合わせに係るＸ座標区間の距離（すなわち、図７（Ａ）における区間Ｌ１〜Ｌ６それぞれの距離）を算出する。

信頼度算出部４０は、算出された距離に基づいて、最も距離の小さな区間を有するＸ座標の組み合わせ（この一例において、区間Ｌ６を有するＸ座標Ｘ７＿φ１ＢとＸ座標Ｘ４＿θ１Ａの組み合わせ）を、最も隣接するＸ座標の組み合わせとして抽出する。以下では、説明の便宜上、この最も隣接するＸ座標の組み合わせ（以下、最隣接組座標と称する）を抽出する処理を、最隣接座標組抽出処理と称して説明する。

なお、以下では、ステップＳ２２０における最隣接座標組抽出処理で抽出された最隣接組座標に係る区間の距離をＬ＿１（ＸＡ）によって示す。ここで、引数ＸＡは、距離Ｌ＿１（ＸＡ）に係る区間の位置を示す座標軸ＸＰ上のＸ座標であり、例えば、最も隣接するＸ座標の組み合わせに係る２つのＸ座標の値の平均値である。例えば、この一例において、Ｌ＿１（ＸＡ）＝Ｌ＿１（Ｘ７＿φ１Ｂ ＋ Ｘ４＿θ１Ａ）／２である。
このように、ステップＳ２００からステップＳ２２０の処理を繰り返すことで、信頼度算出部４０は、第１撮像画像平面上の各座標に対して最隣接座標組抽出処理を行う。

次に、信頼度算出部４０は、第２撮像画像平面上の座標を１つずつ選択し、選択された座標毎にステップＳ２４０からステップＳ２５０までの処理を繰り返し行う（ステップＳ２３０）。

ステップＳ２３０で第２撮像画像平面上の座標が１つ選択された後、信頼度算出部４０は、選択された座標が示す点に対応する投影画像平面上の座標を抽出する（ステップＳ２４０）。より具体的には、信頼度算出部４０は、選択された座標が示す点であって第２撮像画像２Ａ０上の点における第２Ａ位相θ２Ａと同じ位相を有する投写画像ＰＡ０上の点を示す座標であって投写画像平面上のＸ座標（すなわち、上記の選択された座標が示す点に対応する投影画像平面上の座標）を抽出する。説明の便宜上、抽出されたこれらのＸ座標を、Ｘ座標Ｘ８＿θ２Ａ〜Ｘ１１＿θ２Ａの４つのみであるとして説明するが、実際にはより多数のＸ座標が抽出される。

ここで、投写画像ＰＡ０平面上のＸ座標毎の位相は、投写画像ＰＡを用いて位相シフト法により予め算出されているとする。なお、信頼度算出部４０は、投影画像ＰＡにおける輝度の周期的変化を表す波が投影画像平面上の水平方向に進行する平面波であるため、投写画像平面上のＸ座標を抽出する構成としたが、これに代えて、投影画像ＰＡにおける輝度の周期的変化を表す波の進行方向に応じた他の座標軸の座標を抽出する構成であってもよい。

また、信頼度算出部４０は、選択された座標が示す点であって第２撮像画像２Ｂ０上の点における第２Ｂ位相φ２Ｂと同じ位相を有する投写画像ＰＢ０上の点を示す座標であって投写画像平面上のＸ座標（すなわち、上記の選択された座標が示す点に対応する投影画像平面上の座標）を抽出する。説明の便宜上、抽出されたこれらのＸ座標を、Ｘ座標Ｘ１２＿φ２Ｂ〜Ｘ１４＿φ２Ｂの３つのみであるとして説明するが、実際にはより多数の座標が抽出される。

次に、信頼度算出部４０は、ステップＳ２４０で抽出されたＸ座標Ｘ８＿θ２Ａ〜Ｘ１１＿θ２Ａと、抽出されたＸ座標Ｘ１２＿φ２Ｂ〜Ｘ１４＿φ２Ｂとを座標値の順に並べた時に、最も隣接する組み合わせであってＸ座標Ｘ８＿θ２Ａ〜Ｘ１１＿θ２Ａのうちのいずれか１つと、Ｘ座標Ｘ１２＿φ２Ｂ〜Ｘ１４＿φ２Ｂのうちのいずれか１つとの組み合わせを抽出する最隣接座標組抽出処理を行う（ステップＳ２５０）。

ここで、再び図７を参照して、ステップＳ２５０における最隣接座標組抽出処理について説明する。図７（Ｂ）には、抽出されたＸ座標Ｘ８＿θ２Ａ〜Ｘ１１＿θ２Ａと、抽出されたＸ座標Ｘ１２＿φ２Ｂ〜Ｘ１４＿φ２Ｂとを座標値の順に、投影画像平面上のＸ座標を表す座標軸ＸＰ上に並べてプロットしたグラフの一例を示す。なお、図７（Ａ）に示したグラフと、図７（Ｂ）に示したグラフとは、原点が一致しており、投影画像平面上のＸ座標を表す座標軸ＸＰ上の位置を図７（Ａ）に示したグラフと、図７（Ｂ）に示したグラフとで比較可能である。

信頼度算出部４０は、図７（Ｂ）に示したグラフから、隣接する組み合わせであってＸ座標Ｘ８＿θ２Ａ〜Ｘ１１＿θ２Ａのうちのいずれか１つと、Ｘ座標Ｘ１２＿φ２Ｂ〜Ｘ１４＿φ２Ｂのうちのいずれか１つとの組み合わせを抽出する。図７（Ｂ）において、信頼度算出部４０は、Ｘ座標Ｘ８＿θ２ＡとＸ座標Ｘ１２＿φ２Ｂ、Ｘ座標Ｘ１２＿φ２ＢとＸ座標Ｘ９＿θ２Ａ、Ｘ座標Ｘ９＿θ２ＡとＸ座標Ｘ１３＿φ２Ｂ、Ｘ座標Ｘ１３＿φ２ＢとＸ座標Ｘ１０＿θ２Ａ、Ｘ座標Ｘ１０＿θ２ＡとＸ座標Ｘ１４＿φ２Ｂ、Ｘ座標Ｘ１４＿φ２ＢとＸ座標Ｘ１１＿θ２Ａの６つの組み合わせを抽出する。そして、信頼度算出部４０は、抽出された組み合わせ毎に、組み合わせに係るＸ座標区間の距離（すなわち、図７（Ｂ）における区間Ｌ７〜Ｌ１２それぞれの距離）を算出する。

信頼度算出部４０は、算出された距離に基づいて、最も距離の小さな区間を有するＸ座標の組み合わせ（この一例において、区間Ｌ１２を有するＸ座標Ｘ１４＿φ２ＢとＸ座標Ｘ１１＿θ２Ａの組み合わせ）を、最隣接組座標として抽出する。なお、この抽出の際、信頼度算出部４０は、抽出された最隣接組座標に係る区間の距離が所定の閾値εよりも大きい場合、抽出された最隣接組座標を破棄し、ステップＳ２３０に戻って次の第２撮像画像平面上の座標を選択する。

なお、以下では、ステップＳ２５０における最隣接座標組抽出処理で抽出された最隣接組座標に係る区間の距離をＬ＿２（ＸＡ）によって示す。引数ＸＡは、距離Ｌ＿１（ＸＡ）の場合と同様であるため、説明を省略する。
このように、ステップＳ２３０からステップＳ２５０の処理を繰り返すことで、信頼度算出部４０は、第２撮像画像平面上の各座標に対して最隣接座標組抽出処理を行う。

ここで、図７（Ａ）に示した区間Ｌ６に係る第１撮像画像平面上の座標が示す位置と、図７（Ｂ）に示した区間Ｌ１２に係る第２撮像画像平面上の座標が示す位置とが同じ位置を示していた場合、区間Ｌ６と区間Ｌ１２とは、座標軸ＸＰ上で重なるはずである。しかし、これは各種の誤差が無い場合であり、一般的には誤差の影響によって、区間Ｌ６に係る第１撮像画像平面上の座標が示す位置と、区間Ｌ１２に係る第２撮像画像平面上の座標が示す位置とが同じ位置を示していた場合であっても、図７に示したように区間Ｌ６と区間Ｌ１２とが座標軸ＸＰ上で重ならずにずれてしまう。

しかし、誤差の影響によってずれてしまうことはあっても、図７（Ａ）に示した区間Ｌ６に係る第１撮像画像平面上の座標が示す位置と、図７（Ｂ）に示した区間Ｌ１２に係る第２撮像画像平面上の座標が示す位置とが同じ位置を示していた場合、区間Ｌ６と区間Ｌ１２とは、座標軸ＸＰ上で一部又は全部が重なる、もしくは隣接する可能性が高い。そこで、信頼度算出部４０は、ステップＳ２６０からステップＳ２７０の処理によって、ステップＳ２２０で抽出された区間（距離Ｌ＿１（ＸＡ）に係る区間）に係る第１撮像画像平面上の座標が示す位置と、第２撮像画像平面上の座標が示す位置とが同じ位置を示していることの確からしさの指標を示す信頼度を、区間Ｌ＿１（ＸＡ）と区間Ｌ＿２（ＸＡ）に基づいて算出する。

次に、信頼度算出部４０は、ステップＳ２２０で算出された第１撮像画像平面上の各座標に係る区間の距離Ｌ＿１（ＸＡ）に基づいて、第１撮像画像平面上の各座標に係る尤度関数Ｐ＿１（ＸＰ）を導出する。また、信頼度算出部４０は、ステップＳ２５０で算出された第２撮像画像平面上の各座標に係る区間の距離Ｌ＿２（ＸＡ）に基づいて、第２撮像画像平面上の各座標に係る尤度関数Ｐ＿２（ＸＰ）を導出する。ここで、尤度関数Ｐ＿１（ＸＰ）及び尤度関数Ｐ＿２（ＸＰ）について説明する。

ステップＳ２２０で算出された区間の距離Ｌ＿１（ＸＡ）に係る第１撮像画像平面上の２つの座標は、第１撮像画像平面上の同じ座標に係る位相から得られた組み合わせであるため、第１撮像部１１と投影部５の間のキャリブレーションの誤差や、位相シフト法の計算の誤差が０になるような理想的な環境では、その組み合わせの区間の距離Ｌ＿１（ＸＡ）が０、すなわち重なって検出されるはずのものである。理想的な環境において重なって検出される位置を真実の位置とすると、この真実の位置は、算出された距離Ｌ＿１（ＸＡ）に係る区間の間である確率が高い。この確率を表す確率密度関数が尤度関数Ｐ＿１（ＸＰ）である。

また、ステップＳ２５０で算出された区間の距離Ｌ＿２（ＸＡ）に係る第２撮像画像平面上の２つの座標は、第２撮像画像平面上の同じ座標に係る位相から得られた組み合わせであるため、第２撮像部１２と投影部５の間のキャリブレーションの誤差や、位相シフト法の計算の誤差が０になるような理想的な環境では、その組み合わせの区間の距離Ｌ＿２（ＸＡ）が０、すなわち重なって検出されるはずのものである。理想的な環境において重なって検出される位置を真実の位置とすると、この真実の位置は、算出された距離Ｌ＿２（ＸＡ）の間である確率が高い。この確率を表す確率密度関数が尤度関数Ｐ＿２（ＸＰ）である。

尤度関数Ｐ＿１（ＸＰ）及び尤度関数Ｐ＿２（ＸＰ）は、確率密度関数であるため、以下の式（１）、式（２）によって表される条件を満たす。

ここで、ｉ＝１，２である。また、ＸＰは、座標軸ＸＰ上のＸ座標を示す。また、積分区間０〜ＸＬは、投影画像平面上のＸ座標の全区間を示す。式（１）は、前述の全区間での確率の総和が１であることを要請する条件である。また、式（２）は、確率が正定値であることを要請する条件である。さらに、尤度関数Ｐ＿１（ＸＰ）及び尤度関数Ｐ＿２（ＸＰ）は、例えば、上記の式（１）及び式（２）に加えて、付加的な条件を満たすように作ることができる。本実施形態では、そのような一例として、以下の式（３）及び式（４）を満たすように尤度関数Ｐ＿１（ＸＰ）及び尤度関数Ｐ＿２（ＸＰ）を用いる場合について説明するが、他の条件を満たすような尤度関数Ｐ＿１（ＸＰ）及び尤度関数Ｐ＿２（ＸＰ）を用いてもよい。なお、尤度関数Ｐ＿１（ＸＰ）は、第１尤度関数の一例である。また、尤度関数Ｐ＿２（ＸＰ）は、第２尤度関数の一例である。

ここで、Ｘ１１及びＸ１２は、区間Ｌ＿ｉ（ＸＡ）に係る第ｉ撮像画像平面上の２つの座標を示す。ここで、ｉ＝１，２である。また、ｊ，ｋはそれぞれ、Ｊ番目のＸ座標を示す添え字であり、Ｋ番目のＸ座標を示す添え字である。式（３）は、前述の真実の位置が算出された区間Ｌ＿２（ＸＡ）の間である確率が最も高くなることを要請する条件である。また、式（４）は、算出された区間Ｌ＿２（ＸＡ）の間の中心を示す座標ＸＡから離れると、前述の真実の位置が存在する確率が低くなることを要請する条件である。

上記の式（１）〜式（４）によって表される条件を満たす尤度関数Ｐ＿１（ＸＰ）及び尤度関数Ｐ＿２（ＸＰ）として、信頼度算出部４０は、例えば、図８に示すような関数形の関数を導出する。図８は、尤度関数の関数形の一例を示す図である。図８に示したように、尤度関数Ｐ＿１（ＸＰ）及び尤度関数Ｐ＿２（ＸＰ）の関数形は、階段関数の関数形によって表される。尤度関数Ｐ＿ｉ（ＸＰ）は、前述の閾値εの範囲内でのみ値を持つ階段関数であり、閾値εが示す区間の範囲内であって、図７に示した距離Ｌ＿ｉ（ＸＡ）に係る区間の内側が最も高い値となる階段関数である。ここで、ｒは、距離Ｌ＿ｉ（ＸＡ）に係る区間の外側であって、閾値εが示す区間の内側における尤度関数の値を決定する因子であり、制御部３６に予め登録された値であるとするが、これに代えて、ユーザーにより登録可能な値であってもよい。Ａｒは、区間Ｌ＿ｉ（ＸＡ）の内側における尤度関数の値を決定する因子であり、上記の式（３）に基づいて、Ａｒ＝Ｌ＿ｉ（ＸＡ）＋ｒ（ε−Ｌ＿ｉ（ＸＡ））から算出される。

ステップＳ２６０で第１撮像画像平面上の各座標に係る尤度関数Ｐ＿１（ＸＰ）と、第２撮像画像平面上の各座標に係る尤度関数Ｐ＿２（ＸＰ）とが導出された後、信頼度算出部４０は、第１撮像画像平面上の座標から１つずつ座標を選択し、選択された座標毎に以下の式（５）に基づいて、すべての第２撮像画像平面上の座標それぞれとの間の信頼度Ｃ（ｒ１，ｒ２）をすべて算出する（ステップＳ２７０）。

ここで、ｒ１は、第１撮像画像平面上の座標を示す。また、ｒ２は、第２撮像画像平面上の座標を示す。上記の式（５）における信頼度Ｃ（ｒ１，ｒ２）は、選択された第１撮像画像平面上の座標に係る尤度関数と、すべての第２撮像画像平面上の座標それぞれに係る尤度関数との間の相関関数である。すなわち、この信頼度Ｃ（ｒ１，ｒ２）の値が最も高い第２撮像画像平面上の座標が示す位置は、選択された第１撮像画像平面上の座標が示す位置と同じ位置を示している可能性が最も高い。このような理由により、ステップＳ１７０において対応付部４１は、信頼度Ｃ（ｒ１，ｒ２）が最も高い第２撮像画像平面上の座標が示す位置と、選択された第１撮像画像平面上の座標とを対応付けることで、撮像部間キャリブレーションを行うことができる。

以下、図９を参照して、制御装置３０がロボット２０に所定の作業を行わせる処理について説明する。図９は、制御装置３０がロボット２０に所定の作業を行わせる処理の流れの一例を示すフローチャートである。以下では、図５及び図６に示したフローチャートに係る処理によって撮像部間キャリブレーションが終了していることを前提として説明する。まず、撮像制御部３７は、対象物Ｏを含む撮像範囲Ｐ２をステレオ撮像するように撮像部１０を制御する（ステップＳ３００）。

次に、画像取得部３５は、撮像部１０によりステレオ撮像された第１撮像画像及び第２撮像画像を撮像部１０から取得する（ステップＳ３１０）。次に、３次元点群生成部４２は、ステップＳ３１０で画像取得部３５により取得された第１撮像画像及び第２撮像画像に基づいて、対象物Ｏの３次元点群を生成する（ステップＳ３２０）。次に、３次元位置姿勢導出部４３は、ステップＳ３２０で３次元点群生成部４２により生成された対象物Ｏの３次元点群に基づいて、対象物位置姿勢を算出する（ステップＳ３３０）。

次に、ロボット制御部４４は、ステップＳ３３０で３次元位置姿勢導出部４３により算出された対象物位置姿勢に基づいて、把持部ＨＮＤにより対象物Ｏを把持するようにロボット２０を制御する。そして、ロボット制御部４４は、把持部ＨＮＤにより把持された対象物Ｏを所定の配置位置Ｘに配置するようにロボット２０を制御する（ステップＳ３４０）。

以上説明したように、本実施形態におけるロボットシステム１は、対象物Ｏに投影画像を投影し、投影部５により投影画像が投影された対象物Ｏを第１撮像部１１により撮像し、投影部５により投影画像が投影された対象物Ｏを、第１撮像部１１と異なる位置に配置された第２撮像部１２により撮像し、第１撮像部１１が撮像した第１撮像画像上の第１点と、第２撮像部１２が撮像した第２撮像画像上の第２点とにより、対象物Ｏの３次元位置及び姿勢を算出する。これにより、ロボットシステム１は、多様な３次元形状の計測を行うことができる。

また、ロボットシステム１は、第１撮像画像と、第２撮像画像に基づいて、第１撮像画像上のある点が示す位置と、第２撮像画像上の各点が示す位置とが同じ位置を示していることの確からしさの指標となる信頼度を算出し、算出された信頼度に基づいて、第１撮像画像上の各点と、第２撮像画像上の各点との対応付けを行う。これにより、ロボットシステム１は、高い精度で撮像部間キャリブレーションを行うことができる。

また、ロボットシステム１は、第１撮像画像の各点それぞれに対応する尤度関数Ｐ＿１（ＸＰ）と、第２撮像画像の各点それぞれに対応する尤度関数Ｐ＿２（ＸＰ）とを導出し、導出されたこれらの尤度関数に基づいて信頼度を算出する。これにより、ロボットシステム１は、尤度関数に基づいて信頼度を算出し、算出された信頼度に基づいて高い精度で撮像部間キャリブレーションを行うことができる。

また、ロボットシステム１は、第１撮像画像上の各点における投影画像の輝度又は色相の変化に係る位相と、第２撮像画像上の各点における投影画像の輝度又は色相の変化に係る位相とを算出し、算出された第１撮像画像に係る位相に基づいて尤度関数Ｐ＿１（ＸＰ）を導出し、第２撮像画像に係る位相に基づいて第２尤度関数を導出する。これにより、制御システムは、パターン光の輝度又は色相の変化に係る位相に基づいて尤度関数を導出して信頼度を算出することができる。

以上、この発明の実施形態を、図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない限り、変更、置換、削除等されてもよい。

また、以上に説明した装置（例えば、ロボットシステム１の制御装置３０）における任意の構成部の機能を実現するためのプログラムを、コンピューター読み取り可能な記録媒体に記録し、そのプログラムをコンピューターシステムに読み込ませて実行するようにしてもよい。なお、ここでいう「コンピューターシステム」とは、ＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｓｔｅｍ）や周辺機器等のハードウェアを含むものとする。また、「コンピューター読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＣＤ（Ｃｏｍｐａｃｔ Ｄｉｓｋ）−ＲＯＭ等の可搬媒体、コンピューターシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピューター読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバーやクライアントとなるコンピューターシステム内部の揮発性メモリー（ＲＡＭ：Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）のように、一定時間プログラムを保持しているものも含むものとする。

また、上記のプログラムは、このプログラムを記憶装置等に格納したコンピューターシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピューターシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。
また、上記のプログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、上記のプログラムは、前述した機能をコンピューターシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。

１ ロボットシステム、５ 投影部、１０ 撮像部、１１ 第１撮像部、１２ 第２撮像部、２０ ロボット、３０ 制御装置、３１ ＣＰＵ、３２ 記憶部、３３ 入力受付部、３４ 通信部、３５ 画像取得部、３６ 制御部、３７ 撮像制御部、３８ 投影制御部、３９ 位相算出部、４０ 信頼度算出部、４１ 対応付部、４２ ３次元点群生成部、４３ ３次元位置姿勢導出部、４４ ロボット制御部

Claims (4)

1. 対象物に所定のパターン光を投影する投影部と、
   前記投影部により前記所定のパターン光が投影された前記対象物を撮像する第１撮像部と、
   前記第１撮像部と異なる位置に配置され、前記投影部により前記所定のパターン光が投影された前記対象物を撮像する第２撮像部と、
   前記第１撮像部が撮像した第１撮像画像の各点それぞれに対応する第１尤度関数と、前記第２撮像部が撮像した第２撮像画像の各点それぞれに対応する第２尤度関数とを導出し、導出した前記第１尤度関数及び前記第２尤度関数に基づいて、前記第１撮像画像上のある点が示す位置と、前記第２撮像画像上の各点が示す位置とが同じ位置を示していることの確からしさの指標となる信頼度を算出する信頼度算出部と、
   前記信頼度算出部により算出された前記信頼度に基づいて、前記第１撮像画像上の各点と、前記第２撮像画像上の各点との対応付けを行う対応付部と、
   前記第１撮像画像上の第１点と、前記第２撮像画像上の第２点とにより、前記対象物の３次元形状を算出する算出部と、
   を含む制御システム。
2. 請求項１に記載の制御システムであって、
   前記パターン光は、輝度又は色相がある方向に周期的に変化するパターン光であり、
   前記信頼度算出部は、前記第１撮像画像上の各点における前記パターン光の輝度又は色相の変化に係る第１位相と、前記第２撮像画像上の各点における前記パターン光の輝度又は色相の変化に係る第２位相とを算出し、算出された前記第１位相に基づいて前記第１尤度関数を導出し、前記第２位相に基づいて前記第２尤度関数を導出する、
   制御システム。
3. 請求項１又は２に記載の制御システムと、ロボットとを具備するロボットシステムであって、
   前記制御システムは、
   前記算出部により算出された前記対象物の前記３次元形状に基づいて前記ロボットを制御するロボット制御部を備える、
   ロボットシステム。
4. 対象物に所定のパターン光を投影し、
   前記所定のパターン光が投影された前記対象物を第１撮像部により撮像し、
   前記所定のパターン光が投影された前記対象物を、前記第１撮像部と異なる位置に配置された第２撮像部により撮像し、
   前記第１撮像部が撮像した第１撮像画像の各点それぞれに対応する第１尤度関数と、前記第２撮像部が撮像した第２撮像画像の各点それぞれに対応する第２尤度関数とを導出し、導出した前記第１尤度関数及び前記第２尤度関数に基づいて、前記第１撮像画像上のある点が示す位置と、前記第２撮像画像上の各点が示す位置とが同じ位置を示していることの確からしさの指標となる信頼度を算出し、
   算出された前記信頼度に基づいて、前記第１撮像画像上の各点と、前記第２撮像画像上の各点との対応付けを行い、
   前記第１撮像部が撮像した第１撮像画像上の第１点と、前記第２撮像部が撮像した第２撮像画像上の第２点とにより、前記対象物の３次元形状を算出する、
   制御方法。