(57)【要約】【課題】物体の三次元空間における位置及び姿勢の推定に必要な時間を短縮してロボットを制御することができるロボットシステムを提供する。【解決手段】ロボットと、物体を撮像する撮像部と、ロボットを動作させる制御部と、を含み、撮像部は、第１撮像素子と第２撮像素子とを含む複数の撮像素子を備え、制御部は、第１撮像素子により第１方向から物体を撮像することと、第２撮像素子により第１方向とは異なる第２方向から物体を撮像することによって、ロボットを動作させる、ロボットシステムである。【選択図】図３
2015100868.tif 000002
【特許請求の範囲】
【請求項１】
ロボットと、
物体を撮像する撮像部と、
前記ロボットを動作させる制御部と、
を含み、
前記撮像部は、第１撮像素子と第２撮像素子とを含む複数の撮像素子を備え、
前記制御部は、前記第１撮像素子により第１方向から前記物体を撮像することと、前記第２撮像素子により前記第１方向とは異なる第２方向から前記物体を撮像することによって、前記ロボットを動作させる、
ロボットシステム。
【請求項２】
請求項１に記載のロボットシステムであって、
前記制御部は、前記撮像部により撮像された複数の撮像画像に基づいて、前記物体の三次元ヤコビ行列を算出し、前記算出された三次元ヤコビ行列と、基準となる基準三次元ヤコビ行列とを比較して、前記物体の位置及び姿勢を推定し、前記推定された位置及び姿勢に基づいて、前記ロボットを動作させる、
ロボットシステム。
【請求項３】
請求項１又は２に記載のロボットシステムであって、
前記撮像部は、ハーフミラーと、複数のミラーを更に備え、前記ハーフミラーと前記複数のミラーとによって、前記第１方向と前記第２方向とから入射する前記物体からの光を前記第１撮像素子と前記第２撮像素子に集める、
ロボットシステム。
【請求項４】
請求項３に記載のロボットシステムであって、
前記撮像部は、前記ハーフミラーにより前記第１方向と前記第２方向とから入射する前記物体からの光を分岐させることで、前記第１方向と前記第２方向とから入射する前記物体からの光を前記第１撮像素子と前記第２撮像素子に集める、
ロボットシステム。
【請求項５】
請求項３に記載のロボットシステムであって、
前記撮像部は、撮像素子の面に平行な面上に配列された、互いに異なる焦点を有する複数のレンズを更に備え、前記複数のレンズによって得られる奥行き方向の情報を含む画像を撮像する、
ロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、ロボットシステムに関する。
【背景技術】
【０００２】
現在、産業用ロボットによって作業が行われる際、産業用ロボットが扱う対象となる物体の位置は、治具を用いる等の工夫によって、事前に決定されている必要がある。一方で、事前に物体の位置を固定できないような作業であり、産業用ロボットを活用したい作業は多い。物体の位置を固定できないような作業とは、例えば、物体同士の組み付けや、物体へのラベル貼り等である。これらの作業を産業用ロボットが行うためには、産業用ロボットが物体の三次元空間における位置及び姿勢を、正確且つ高速に認識する必要がある。近年、このような必要性から、カメラによって撮像された画像を用いることで、物体の三次元空間における位置及び姿勢を認識するシステムの研究・開発が盛んに行われている。
【０００３】
これに関連し、空間中に配置された指標を含む画像を撮像し、撮像画像に含まれる指標に基づいて、その指標の分布範囲を導出し、導出された分布範囲の大きさに応じたアルゴリズムに基づいて、三次元空間における物体の位置及び姿勢を推定する位置姿勢計測装置が知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００６−２４２９４３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来の位置姿勢計測装置等では、三次元空間における位置及び姿勢を計測するための非線形最適化の手法として、多くの反復計算を必要とするニュートン法等を用いる例が挙げられている。しかしながら、ニュートン法を利用した手法では、短時間で物体の三次元空間における位置及び姿勢を推定することができない場合がある。
【０００６】
そこで本発明は、上記従来技術の問題に鑑みてなされたものであり、物体の三次元空間における位置及び姿勢の推定に必要な時間を短縮してロボットを制御することができるロボットシステムを提供する。
【課題を解決するための手段】
【０００７】
［１］本発明は、上記の課題を解決するためになされたものであり、ロボットと、物体を撮像する撮像部と、前記ロボットを動作させる制御部と、を含み、前記撮像部は、第１撮像素子と第２撮像素子とを含む複数の撮像素子を備え、前記制御部は、前記第１撮像素子により第１方向から前記物体を撮像することと、前記第２撮像素子により前記第１方向とは異なる第２方向から前記物体を撮像することによって、前記ロボットを動作させる、ロボットシステムである。
【０００８】
この構成により、ロボットシステムは、第１撮像素子と第２撮像素子とを含む複数の撮像素子を備える撮像部を備え、第１撮像素子により第１方向から物体を撮像することと、第２撮像素子により第１方向とは異なる第２方向から物体を撮像することによって、ロボットを動作させるため、物体の三次元空間における位置及び姿勢の推定に必要な時間を短縮してロボットを制御することができる。
【０００９】
［２］また、本発明は、［１］に記載のロボットシステムであって、前記制御部は、前記撮像部により撮像された複数の撮像画像に基づいて、前記物体の三次元ヤコビ行列を算出し、前記算出された三次元ヤコビ行列と、基準となる基準三次元ヤコビ行列とを比較して、前記物体の位置及び姿勢を推定し、前記推定された位置及び姿勢に基づいて、前記ロボットを動作させる、ロボットシステムである。
【００１０】
この構成により、ロボットシステムは、制御部は、撮像部により撮像された複数の撮像画像に基づいて、物体の三次元ヤコビ行列を算出し、算出された三次元ヤコビ行列と、基準となる基準三次元ヤコビ行列とを比較して、物体の位置及び姿勢を推定し、推定された位置及び姿勢に基づいて、ロボットを動作させるため、例えば、ＥＳＭ（Ｅｆｆｉｃｉｅｎｔ  Ｓｅｃｏｎｄ−ｏｒｄｅｒ  Ｍｉｎｉｍｉｚａｔｉｏｎ  Ｍｅｔｈｏｄ）法を用いた推定方法によって物体の位置及び姿勢を推定し、従来のニュートン法等の非線形最小化手法を用いた推定法に比べて、物体の位置及び姿勢の推定に必要な時間を短縮してロボットを制御することができる。
【００１１】
［３］また、本発明は、［１］又は［２］に記載のロボットシステムであって、前記撮像部は、ハーフミラーと、複数のミラーを更に備え、前記ハーフミラーと前記複数のミラーとによって、前記第１方向と前記第２方向とから入射する前記物体からの光を前記第１撮像素子と前記第２撮像素子に集める、ロボットシステムである。
【００１２】
この構成により、ロボットシステムは、撮像部が、ハーフミラーと複数のミラーとによって、第１方向と第２方向とから入射する物体からの光を第１撮像素子と第２撮像素子に集めるため、撮像部の位置を変えることなく、異なる方向から撮像された複数の画像を取得することができ、その結果として、物体の位置及び姿勢の推定に必要な時間を短縮することができる。また、撮像画像にぶれが生じることが無く、推定の精度を向上させることができる。
【００１３】
［４］また、本発明は、［３］に記載のロボットシステムであって、前記撮像部は、前記ハーフミラーにより前記第１方向と前記第２方向とから入射する前記物体からの光を分岐させることで、前記第１方向と前記第２方向とから入射する前記物体からの光を前記第１撮像素子と前記第２撮像素子に集める、ロボットシステムである。
【００１４】
この構成により、ロボットシステムは、撮像部が、ハーフミラーにより第１方向と第２方向とから入射する物体からの光を分岐させることで、第１方向と第２方向とから入射する物体からの光を第１撮像素子と第２撮像素子に集めるため、１台の撮像部によって同時に異なる方向から物体を撮像することができ、物体の位置及び姿勢の推定をより短時間で行うことができる。
【００１５】
［５］また、本発明は、［３］に記載のロボットシステムであって、前記撮像部は、撮像素子の面に平行な面上に配列された、互いに異なる焦点を有する複数のレンズを更に備え、前記複数のレンズによって得られる奥行き方向の情報を含む画像を撮像する、ロボットシステムである。
【００１６】
この構成により、ロボットシステムは、撮像部が、撮像素子の面に平行な面上に配列された、互いに異なる焦点を有する複数のレンズを更に備え、複数のレンズによって得られる奥行き方向の情報を含む画像を撮像するため、ハーフミラーや複数のミラーにより構成される光学系を必要とせず、物体の位置及び姿勢の推定をより短時間で行うことができる。
【図面の簡単な説明】
【００１７】
【図１】第１の実施形態におけるロボットシステム１の利用状況の一例を示す図である。
【図２】物体位置姿勢推定装置３０のハードウェア構成の一例を示す図である。
【図３】物体位置姿勢推定装置３０の機能構成の一例を示す図である。
【図４】撮像部１０が備える専用の光学系ｏｐｔの一例を表す図である。
【図５】図４で示した専用の光学系ｏｐｔを備えた撮像部１０によって、異なる方向から撮像された物体の画像ｐｉｃ１〜ｐｉｃ４が撮像される状況の一例を説明する図である。
【図６】物体位置姿勢推定装置３０の動作の流れの一例を説明するフローチャートである。
【図７】第２の実施形態におけるロボットシステム１の利用状況の一例を示す図である。
【発明を実施するための形態】
【００１８】
＜第１の実施形態＞
以下、本発明の実施形態について、図面を参照して詳細に説明する。図１は、第１の実施形態におけるロボットシステム１の利用状況の一例を示す図である。第１の実施形態におけるロボットシステム１は、例えば、撮像部１０と、物体位置姿勢推定装置３０と、ロボット本体５０とを具備する。撮像部１０は、例えば、集光された光を電気信号に変換する撮像素子であるＣＣＤ（Ｃｈａｒｇｅ  Ｃｏｕｐｌｅｄ  Ｄｅｖｉｃｅ）やＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ  Ｍｅｔａｌ  Ｏｘｉｄｅ  Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）等の撮像素子を備えたカメラである。撮像部１０は、静止画像や動画像を撮像する。以下では、説明を簡略化するため、撮像部１０は、静止画像を撮像するものとする。
【００１９】
撮像部１０は、ケーブルＣｂｌによって、物体位置姿勢推定装置３０と通信可能に接続されている。ケーブルＣｂｌを介した有線通信は、例えば、ＨＤＭＩ（Ｈｉｇｈ−Ｄｅｆｉｎｉｔｉｏｎ  Ｍｕｌｔｉｍｅｄｉａ  Ｉｎｔｅｒｆａｃｅ、登録商標）、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ  Ｓｅｒｉａｌ  Ｂｕｓ）等の規格によって行われる。なお、撮像部１０と物体位置姿勢推定装置３０とは、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって接続されてもよい。撮像部１０は、テーブルｔｂｌ上に置かれた対象物体ｂｘを互いに異なる方向から撮像し、撮像した画像を、ケーブルＣｂｌを介して物体位置姿勢推定装置３０へ出力する。異なる方向から撮像する方法についての詳細は、後述する。なお、それぞれの方向から撮像された画像は、それぞれの方向（例えば、第１方向と、第１方向とは異なる第２方向）に対応した複数の画像でもよく、異なる方向に対応した複数の画像が１つの撮像素子によりまとめて撮像された１つの画像でもよい。また、複数の画像が結合されて１つの画像として扱われるものであってもよい。特許請求の範囲における「複数の撮像画像」とは、これらのいずれであってもよい。以下では、説明を簡略化するため、それぞれの方向から撮像された画像が、それぞれの方向に対応した複数の画像であるものとして説明する。
【００２０】
物体位置姿勢推定装置３０は、ケーブルＥｔｈによって、ロボット本体５０と通信可能に接続されている。ケーブルＥｔｈを介した有線通信は、例えば、イーサネット（登録商標）等の通信規格によって行われる。なお、物体位置姿勢推定装置３０とロボット本体５０とは、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって接続されてもよい。また、図１において、物体位置姿勢推定装置３０は、ロボット本体５０の外部に設置されているが、ロボット本体５０の内部に搭載されてもよい。物体位置姿勢推定装置３０は、撮像部１０から取得した撮像画像に基づいて、ロボット本体５０に取り付けられた把持部ｈｎｄの位置及び姿勢を制御し、把持部ｈｎｄによって対象物体ｂｘを予め決められた移動先へ移動させる。
【００２１】
ロボット本体５０は、例えば、把持部ｈｎｄを可動に備えた６軸垂直多関節ロボットであり、支持台とアーム部と把持部ｈｎｄとの連携した動作によって、６軸の自由度の動作を行うことができる。把持部ｈｎｄは、物体を把持又は挟持可能な爪部を備える。なお、ロボット本体５０は、５自由度以下で動作するものであってもよく、７自由度以上で動作するものであってもよい。
【００２２】
次に、図２を参照することで、物体位置姿勢推定装置３０のハードウェア構成について説明する。図２は、物体位置姿勢推定装置３０のハードウェア構成の一例を示す図である。物体位置姿勢推定装置３０は、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）３１と、記憶部３２と、入力受付部３３と、通信部３４とを備え、通信部３４を介して他の撮像部１０、ロボット本体５０と通信を行う。ＣＰＵ３１は、記憶部３２に格納された各種プログラムを実行する。記憶部３２は、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）やＳＳＤ（Ｓｏｌｉｄ  Ｓｔａｔｅ  Ｄｒｉｖｅ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ  Ｅｒａｓａｂｌｅ  Ｐｒｏｇｒａｍｍａｂｌｅ  Ｒｅａｄ−Ｏｎｌｙ  Ｍｅｍｏｒy）、ＲＯＭ（Ｒｅａｄ−Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）などを含み、物体位置姿勢推定装置３０が処理する情報やプログラムを格納する。なお、記憶部３２は、物体位置姿勢推定装置３０に内蔵されるものに代えて、外付け型の記憶装置でもよい。
【００２３】
入力受付部３３は、キーボードやマウス、タッチパッド、その他の入力装置である。なお、入力受付部３３は、表示部として機能してもよく、さらに、タッチパネルとして構成されてもよい。通信部３４は、ＨＤＭＩやＵＳＢ等のデジタル入出力ポートとともに、イーサネット（登録商標）ポートを含んで構成される。
【００２４】
次に、図３を参照することで、物体位置姿勢推定装置３０の機能構成について説明する。図３は、物体位置姿勢推定装置３０の機能構成の一例を示す図である。物体位置姿勢推定装置３０は、例えば、記憶部３２と、入力受付部３３と、画像取得部３５と、制御部３６とを備える。画像取得部３５は、撮像部１０によって対象物体ｂｘが異なる方向から撮像された画像を取得し、取得した画像を、後述する画像記憶部３２１に記憶させる。これらのうち、画像取得部３５、ヤコビ行列算出部３７、位置姿勢推定部４１は、例えば、ＣＰＵ３１が記憶部３２に格納されたプログラムを実行することにより機能するソフトウェア機能部である。また、これらのうち一部または全部は、ＬＳＩ（Ｌａｒｇｅ  Ｓｃａｌｅ  Ｉｎｔｅｇｒａｔｉｏｎ）やＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ  Ｓｐｅｃｉｆｉｃ  Ｉｎｔｅｇｒａｔｅｄ  Ｃｉｒｃｕｉｔ）等のハードウェア機能部であってもよい。
【００２５】
記憶部３２は、例えば、画像記憶部３２１と、モデル記憶部３２２とを備える。画像記憶部３２１は、画像取得部３５が取得した画像を記憶する。モデル記憶部３２２は、対象物体ｂｘ又は対象物体ｂｘの形状、色彩、模様等を三次元モデル化して得られた三次元モデルデータ（以下、リファレンスモデルと称する）を予め記憶する。リファレンスモデルは、撮像部１０により撮像された対象物体ｂｘとの比較対象となるものであり、例えば、
三次元コンピューターグラフィックス（Ｃｏｍｐｕｔｅｒ  Ｇｒａｐｈｉｃｓ；ＣＧ）で表現される。この三次元ＣＧは、例えば、ポリゴンデータの集合として表される。
【００２６】
制御部３６は、例えば、ヤコビ行列算出部３７と、位置姿勢推定部４１と、駆動制御部４３とを備える。ヤコビ行列算出部３７は、画像記憶部３２１に記憶された画像（対象物体ｂｘが異なる方向から撮像された画像）に基づいて、対象物体ｂｘの三次元ヤコビ行列を算出する。三次元ヤコビ行列は、例えば、対象物体ｂｘの位置及び姿勢を推定するために利用される非線形最小化手法に必要なパラメーターを含む行列である。非線形最小化手法とは、例えば、ＥＳＭ（Ｅｆｆｉｃｉｅｎｔ  Ｓｅｃｏｎｄ−ｏｒｄｅｒ  Ｍｉｎｉｍｉｚａｔｉｏｎ  Ｍｅｔｈｏｄ）法やレーベンバーグ・マルカート法、ニュートン法、ガウス・ニュートン法等である。これらの非線形最小化手法のうち、ＥＳＭ法は、他の手法に比べて反復計算の回数が少ないため、対象物体ｂｘの位置及び姿勢をより高速に推定したい場合に、好適に用いることができる。そこで、以下では、非線形最小化手法としてＥＳＭ法を採用し、算出した三次元ヤコビ行列は、ＥＳＭ法に用いるパラメーターであるものとして説明する。
【００２７】
また、ヤコビ行列算出部３７は、ロボット本体５０によって対象物体ｂｘを移動させた後の位置及び姿勢（リファレンスモデルの仮想的な位置及び姿勢）を、ユーザーからの入力として入力受付部３３から取得する。ヤコビ行列算出部３７は、例えば、一時的な記憶領域を備えており、ユーザーからの入力を記憶する。そして、ヤコビ行列算出部３７は、モデル記憶部３２２に記憶されたリファレンスモデルと、ユーザーからの入力に基づいて、対象物体ｂｘの位置及び姿勢をＥＳＭ法によって推定するための基準となる三次元ヤコビ行列（特許請求の範囲における基準三次元ヤコビ行列の一例）を算出する。また、ヤコビ行列算出部３７は、画像から算出した三次元ヤコビ行列と、リファレンスモデルから算出した基準となる三次元ヤコビ行列とを、位置姿勢推定部４１へ出力する。
【００２８】
位置姿勢推定部４１は、取得した画像から算出した三次元ヤコビ行列と、リファレンスモデルから算出した基準となる三次元ヤコビ行列とに基づいて、ＥＳＭ法によって対象物体ｂｘのカメラ座標系における位置及び姿勢を推定する。ＥＳＭ法による位置及び姿勢の推定についての詳細は、後述する。カメラ座標系とは、位置姿勢推定部４１が仮想空間上に設ける座標系のことであり、例えば、撮像部１０によって撮像される画像内における位置を表すために用いられる座標系である。位置姿勢推定部４１は、推定した対象物体ｂｘのカメラ座標系における位置及び姿勢を表す情報を、駆動制御部４３へ出力する。
【００２９】
駆動制御部４３は、取得した対象物体ｂｘのカメラ座標系における位置及び姿勢を表す情報に基づいて、対象物体ｂｘのロボット座標系における位置及び姿勢を算出する。ロボット座標系とは、ロボット本体５０が稼働する際に、把持部の位置を表すために用いられる座標系である。駆動制御部４３は、位置姿勢推定部４１に適用するカメラ座標系と、ロボット本体５０に適用するロボット座標系とのキャリブレーション（較正）処理を行う。
【００３０】
駆動制御部４３は、算出した対象物体ｂｘのロボット座標系における位置及び姿勢に基づいて、ロボット本体５０が備える把持部ｈｎｄの位置及び姿勢を計算する。そして、駆動制御部４３は、この計算結果に基づいて、ロボット本体５０の各可動部の動作を制御する。
【００３１】
次に、図４と図５とを参照することで、異なる方向から対象物体ｂｘを撮像する方法の一例について説明する。図４は、撮像部１０が備える専用の光学系ｏｐｔの一例を表す図である。対象物体ｂｘからの反射光は、光学系ｏｐｔを介して、撮像素子ｃｃｄ１、ｃｃｄ２に入射する。なお、撮像素子ｃｃｄ１は、第１撮像素子の一例である。また、撮像素子ｃｃｄ２は、第２撮像素子の一例である。対象物体ｂｘからの反射光は、まずハーフミラーｈｍｒへと入射する。ハーフミラーｈｍｒは、入射光の約５０％を透過させ、残りの５０％を透過光とは直交する方向へ反射させる。図４において、実線の矢印は、入射光ｌｌ１の光路を表し、一点鎖線の矢印は、入射光ｌｌ２の光路を表している。なお、入射光ｌｌ１がハーフミラーｈｍｒに入射する方向は、第１方向の一例である。また、入射光ｌｌ２がハーフミラーｈｍｒに入射する方向は、第２方向の一例である。ハーフミラーｈｍｒにより反射された入射光ｌｌ１は、ミラーｍｒ１、ｍｒ２によって反射され、撮像素子ｃｃｄ１へと到達する。一方、ハーフミラーｈｍｒを透過した入射光ｌｌ２は、ミラーｍｒ３、ｍｒ４によって反射され、撮像素子ｃｃｄ２へと到達する。このとき、ｍｒ３の反射面の傾きによって、入射光ｌｌ１と、入射光ｌｌ２とのそれぞれによって撮像される対象物体ｂｘの像は、単純に反転しているわけではなく、微小変化量分だけ対象物体ｂｘを移動させてから撮像したような像となっている。なお、ミラーｍｒ１〜４は、複数のミラーの一例である。
【００３２】
ここで、ミラーｍｒ１、ｍｒ２、及び撮像素子ｃｃｄ１によって構成される撮像系と、ミラーｍｒ３、ｍｒ４、及び撮像素子ｃｃｄ２によって構成される撮像系とは、ヤコビ行列算出部３７は、この微小変化量に基づいて、三次元ヤコビ行列を算出することができる。具体的には、三次元ヤコビ行列の各要素は、対象物体ｂｘを撮像する方向を微小変化させた場合に、その変化させた方向への微小変化に対する対象物体ｂｘを表す画像中のピクセルの輝度が変化する時の変化率を表す値である。ヤコビ行列算出部３７は、対象物体ｂｘ自体を動かすのに代えて、異なる方向から撮像した対象物体ｂｘの画像を比較し、差分を求めることによって、上記変化率を算出し、この変化率に基づいて三次元ヤコビ行列の各要素を決定する。
【００３３】
なお、図４において、画像ｐｉｃ１、ｐｉｃ２はそれぞれ、撮像素子ｃｃｄ１、ｃｃｄ２に入射したそれぞれの入射光ｌｌ１、ｌｌ２による像である。ここで、図４において、画像ｐｉｃ１と、画像ｐｉｃ２とが、専用の光学系ｏｐｔを通過した際に反転してしまうことを明確に示すため、対象物体ｂｘには、矢印ａｒを描いている。撮像部１０は、このような専用の光学系ｏｐｔを、左右方向に対して対象物体ｂｘを反転させるものと、上下方向に対して対象物体ｂｘを反転させるものとの２つを備えることで、ヤコビ行列算出部３７が三次元ヤコビ行列を算出するために必要な情報を、画像として提供することができる。
【００３４】
図５は、図４で示した光学系ｏｐｔを備えた撮像部１０によって、異なる方向から撮像された物体の画像ｐｉｃ１〜ｐｉｃ４が撮像される状況の一例を説明する図である。対象物体ｂｘからの反射光は、入射光ｌｌ１〜ｌｌ４として、専用の光学系ｏｐｔへ入射する。専用の光学系ｏｐｔに入射した入射光ｌｌ１〜ｌｌ４は、それぞれの光路を通過し、レンズｌｎｓを介して、撮像素子ｓｎｓｒに入射する。撮像素子ｓｎｓｒは、４つの撮像素子が格子状に並べられたものである。画像ｐｉｃ１〜ｐｉｃ４は、それぞれの撮像素子で撮像された画像である。
【００３５】
次に、図６を参照することで、物体位置姿勢推定装置３０の動作の流れを説明する。図６は、物体位置姿勢推定装置３０の動作の流れの一例を説明するフローチャートである。
（ステップＳ１００）まず、入力受付部３３は、ユーザーからの入力を受け付けることで、リファレンスモデルの仮想的な位置及び姿勢を表す情報を、ヤコビ行列算出部３７に出力して設定する。
（ステップＳ１２０）次に、モデル記憶部３２２は、入力受付部３３を介して、リファレンスモデルを記憶する。
【００３６】
（ステップＳ１４０）次に、ヤコビ行列算出部３７は、リファレンスモデルの三次元ヤコビ行列を算出する。ヤコビ行列算出部３７は、リファレンスモデルの三次元ヤコビ行列の各要素を、ステップＳ１００で設定されたリファレンスモデルの位置及び姿勢から、リファレンスモデルを仮想的に微小量分だけ動かすシミュレーションに基づいて決定する。なお、ステップＳ１００〜Ｓ１４０の処理は、ステップＳ１６０以下の処理の直前に行われる必要はなく、予めリファレンスモデルの三次元ヤコビ行列が記憶部３２に格納されていてよい。
（ステップＳ１６０）次に、画像取得部３５は、撮像部１０から、異なる方向から撮像された対象物体ｂｘの画像を取得し、取得した画像を画像記憶部３２１に記憶させる。
【００３７】
（ステップＳ１８０）次に、ヤコビ行列算出部３７は、画像記憶部３２１に記憶された異なる方向から撮像された対象物体ｂｘの画像に基づいて、前述した方法により各方向からの画像を比較することで、対象物体ｂｘの三次元ヤコビ行列の各要素を決定する。
（ステップＳ２００）次に、ヤコビ行列算出部３７は、ステップＳ１８０で三次元ヤコビ行列の全ての要素が決定できたか否かを判定する。ヤコビ行列算出部３７により、三次元ヤコビ行列の全ての要素が決定できていないと判定された場合（ステップＳ２００−Ｎｏ）、画像取得部３５は、ステップＳ１６０へ戻り、撮像部１０から再び対象物体ｂｘの画像を取得する。ヤコビ行列算出部３７により、三次元ヤコビ行列の全ての要素が決定できたと判定された場合（ステップＳ２００−Ｙｅｓ）、ヤコビ行列算出部３７は、算出したリファレンスモデルの三次元ヤコビ行列と、対象物体ｂｘの三次元ヤコビ行列とを、位置姿勢推定部４１へ出力する。そして、位置姿勢推定部４１は、取得したリファレンスモデルの三次元ヤコビ行列と、対象物体ｂｘの三次元ヤコビ行列とに基づいて、ＥＳＭ法を利用してカメラ座標系における対象物体ｂｘの位置及び姿勢を推定する（ステップＳ２２０）。
【００３８】
ここで、ＥＳＭ法を利用したカメラ座標系における対象物体ｂｘの位置及び姿勢の推定方法について、簡単に説明する。ＥＳＭ法を用いた推定方法とは、例えば、対象物体ｂｘの画像（以下、実画像と称する）を表す輝度マップと、リファレンスモデルを利用した画像であって、対象物体ｂｘを移動先に移動させた場合の画像（以下、テンプレート画像と称する）の輝度マップとの差分が最小となるような移動量（６自由度の移動量、すなわち３軸方向の並進と、３軸まわりの回転角）を導出する方法である。実画像の輝度マップを表す関数（例えば、画像の各ピクセルの輝度を表す値を要素に持つベクトル）をＳとし、テンプレート画像の輝度マップを表す関数をＳ＊とすると、上記移動量Δｚ（ボールド体）は、以下の（１）〜（３）式で表すことができる。なお、（ボールド体）は、カッコの前の文字がボールド体で表されるベクトル、あるいは、行列であることを表すものとする。
【００３９】
【数１】
2015100868.tif 000003
【００４０】
【数２】
2015100868.tif 000004
【００４１】
【数３】
2015100868.tif 000005
【００４２】
ここで、Ｊ（ボールド体）は、対象物体ｂｘの三次元ヤコビ行列であり、Ｊ（ボールド体）＊は、リファレンスモデルの三次元ヤコビ行列である。対象物体ｂｘの移動先における位置及び姿勢は、予めユーザーから入力されているため、既知である。それ故、位置姿勢推定部４１は、上記（１）式から移動量Δｚ（ボールド体）を求め、求めた移動量Δｚと、対象物体ｂｘの移動先における位置及び姿勢とに基づいて、移動前の対象物体ｂｘの位置及び姿勢を推定することができる。位置姿勢推定部４１は、推定した対象物体ｂｘの位置及び姿勢を、駆動制御部４３へ出力する。
【００４３】
（ステップＳ２４０）次に、駆動制御部４３は、取得した対象物体ｂｘの位置及び姿勢の座標を、カメラ座標系の座標からロボット座標系の座標へと変換し、変換した座標に基づいて、ロボット本体５０の把持部により対象物体ｂｘを、移動先へと移動させる。
【００４４】
以上説明したように、第１の実施形態におけるロボットシステム１は、撮像素子ｃｃｄ１と撮像素子ｃｃｄ２とを含む複数の撮像素子を備える撮像部１０を備え、撮像素子ｃｃｄ１によりハーフミラーｈｍｒに入射する方向から対象物体ｂｘを撮像することと、撮像素子ｃｃｄ２により入射光ｌｌ２がハーフミラーｈｍｒに入射する方向から対象物体ｂｘを撮像することによって、ロボット本体５０を動作させるため、対象物体ｂｘの三次元空間における位置及び姿勢の推定に必要な時間を短縮してロボット本体５０を制御することができる。
【００４５】
また、ロボットシステム１は、制御部３６が、撮像部１０により撮像された複数の撮像画像に基づいて、対象物体ｂｘの三次元ヤコビ行列を算出し、算出された三次元ヤコビ行列と、基準となる基準三次元ヤコビ行列とを比較して、対象物体ｂｘの位置及び姿勢を推定し、推定された位置及び姿勢に基づいて、ロボット本体５０を動作させるため、例えば、ＥＳＭ法を用いた推定方法によって対象物体ｂｘの位置及び姿勢を推定し、従来のニュートン法等の非線形最小化手法を用いた推定法に比べて、対象物体ｂｘの位置及び姿勢の推定に必要な時間を短縮してロボット本体５０を制御することができる。
【００４６】
また、ロボットシステム１は、撮像部１０が、ハーフミラーｈｍｒとミラーｍｒ１〜４とによって、入射光ｌｌ１がハーフミラーｈｍｒに入射する方向と、入射光ｌｌ２がハーフミラーｈｍｒに入射する方向とから入射する対象物体ｂｘからの光を撮像素子ｃｃｄ１と撮像素子ｃｃｄ２に集めるため、撮像部１０の位置を変えることなく、異なる方向から撮像された複数の画像を取得することができ、その結果として、対象物体ｂｘの位置及び姿勢の推定に必要な時間を短縮することができる。また、撮像画像にぶれが生じることが無く、推定の精度を向上させることができる。
【００４７】
また、ロボットシステム１は、撮像部１０が、ハーフミラーｈｍｒにより入射光ｌｌ１がハーフミラーｈｍｒに入射する方向と、入射光ｌｌ２がハーフミラーｈｍｒに入射する方向とから入射する対象物体ｂｘからの光を分岐させることで、入射光ｌｌ１がハーフミラーｈｍｒに入射する方向と入射光ｌｌ２がハーフミラーｈｍｒに入射する方向とから入射する対象物体ｂｘからの光を撮像素子ｃｃｄ１と撮像素子ｃｃｄ２に集めるため、１台の撮像部１０によって同時に異なる方向から対象物体ｂｘを撮像することができ、対象物体ｂｘの位置及び姿勢の推定をより短時間で行うことができる。
【００４８】
＜第２の実施形態＞
以下、第２の実施形態について説明する。構成については、図２、３を援用し、同じ機能部に対して同一の符号を付して説明する。図７は、第２の実施形態におけるロボットシステム１の利用状況の一例を示す図である。第２の実施形態におけるロボットシステム１は、撮像部１０が、ロボット本体５０の把持部付近に設置され、ロボット本体５０の把持部とともに移動する。第２の実施形態の撮像部１０は、第１の実施形態で備えていた専用の光学系ｏｐｔを備えることに代えて、ロボット本体５０の把持部が微小変化量分だけ移動しながら対象物体ｂｘを、異なる方向から撮像する。
【００４９】
以上説明したように、第２の実施形態におけるロボットシステム１は、撮像部１０がロボット本体５０の把持部とともに移動し、異なる方向から対象物体ｂｘを撮像することで、異なる方向から撮像可能な撮像部１０により撮像された物体の二次元画像を取得し、取得した画像に基づいて対象物体ｂｘの三次元ヤコビ行列を算出し、算出された三次元ヤコビ行列と、リファレンスモデルの三次元ヤコビ行列とを比較して、対象物体ｂｘの位置及び姿勢をＥＳＭ法を用いた推定法によって推定するため、対象物体ｂｘの位置及び姿勢の推定に必要な時間を、従来のニュートン法等の非線形最小化手法を用いた推定法に比べて短縮してロボット本体５０を制御することができる。
【００５０】
＜第３の実施形態＞
以下、第３の実施形態について説明する。構成については、図２、３を援用し、同じ機能部に対して同一の符号を付して説明する。第３の実施形態におけるロボットシステム１は、撮像部１０がライトフィールドカメラであり、第２の実施形態と同様に、第１の実施形態で備えていた専用の光学系ｏｐｔを備えていない。ライトフィールドカメラとは、撮像素子の手前に、異なる焦点を持つマイクロレンズが、撮像素子の面に平行な面上で配列されており、この構成によって得られる奥行き方向の情報を含む画像を利用することで、１台でステレオ撮像を行うことが可能なカメラである。従って、第３の実施形態におけるヤコビ行列算出部３７は、ライトフィールドカメラである撮像部１０によってステレオ撮像された三次元画像に基づいて、対象物体ｂｘの三次元ヤコビ行列を算出する。
【００５１】
以上説明したように、第３の実施形態におけるロボットシステム１は、撮像部１０がライトフィールドカメラを撮像部１０として備え、撮像部１０がステレオ撮像することで得られる三次元画像に基づいて、ヤコビ行列算出部３７が対象物体ｂｘの三次元ヤコビ行列を算出するため、第１の実施形態と同様の効果を得ることができる。
【００５２】
また、ロボットシステム１は、撮像部１０が、撮像素子の面に平行な面上に配列された、互いに異なる焦点を有する複数のレンズを更に備え、複数のレンズによって得られる奥行き方向の情報を含む画像を撮像するため、ハーフミラーｈｍｒや複数のミラーｍｒ１〜４により構成される光学系を必要とせず、物体の位置及び姿勢の推定をより短時間で行うことができる。
【００５３】
以上、この発明の実施形態を、図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない限り、変更、置換、削除等されてもよい。
【符号の説明】
【００５４】
１  ロボットシステム、１０  撮像部、３０  物体位置姿勢推定装置、３１  ＣＰＵ、３２  記憶部、３３  入力受付部、３４  通信部、３５  画像取得部、３７  ヤコビ行列算出部、４１  位置姿勢推定部、４３  駆動制御部、５０  ロボット本体、３２１  画像記憶部、３２２  モデル記憶部
【図１】
2015100868.tif 000006
【図２】
2015100868.tif 000007
【図３】
2015100868.tif 000008
【図４】
2015100868.tif 000009
【図５】
2015100868.tif 000010
【図６】
2015100868.tif 000011
【図７】
2015100868.tif 000012