(57)【要約】【課題】本発明は、ステレオ画像である２つの広角画像とステレオ画像である２つの望遠画像とを同時に取得することができ、特に２つの望遠画像を取得する際に２つの望遠光学系の光軸上で測定対象物を確実に捕捉することができる小型化可能なステレオカメラを備え、正確な距離情報に基づいてロボットを駆動させることができるロボットシステム及びロボットシステムの制御方法を提供することを目的とする。【解決手段】ロボットシステムは、ロボットとステレオカメラから構成されている。ステレオカメラは、第１の撮像部及び第２の撮像部から第１の広角画像と第２の広角画像、及び第１の望遠画像と第２の望遠画像を同時に取得し、特に第１の撮影光学系を構成する広角光学系と望遠光学系とは光軸が一致し、同様に第２の撮影光学系も同様に構成されている。【選択図】図５
2018134712.tif 000002
【特許請求の範囲】
【請求項１】
それぞれ光軸が一致し、かつ異なる領域に配置された広角光学系と望遠光学系とを有する第１の撮影光学系と、前記広角光学系及び前記望遠光学系を介して入射する光束をそれぞれ瞳分割して選択的に受光する複数の画素を有する第１の指向性センサとを含む第１の撮像部と、前記第１の撮影光学系と同一構成の第２の撮影光学系と、前記第１の指向性センサと同一構成の第２の指向性センサとを含む第２の撮像部であって、前記第１の撮像部に対して基線長だけ離間して配置された前記第２の撮像部と、前記第１の指向性センサ及び前記第２の指向性センサから第１の広角画像及び第２の広角画像と、第１の望遠画像及び第２の望遠画像とを取得する画像取得部と、を備えるステレオカメラ及びロボットから構成されるロボットシステムであって、
前記第１の撮像部及び前記第２の撮像部をそれぞれ水平方向及び垂直方向に回動させるパンチルト機構と、
前記画像取得部により取得した前記第１の広角画像及び前記第２の広角画像に基づいて前記パンチルト機構を制御し、前記第１の撮影光学系及び前記第２の撮影光学系の前記望遠光学系で測定対象物をそれぞれ捕捉させるパンチルト制御部と、
前記画像取得部により取得した前記第１の望遠画像及び前記第２の望遠画像に基づいて前記測定対象物と前記ロボットの基準部位との相対的な三次元距離情報を算出する距離情報算出部と、
前記距離情報算出部で算出された前記相対的な三次元距離情報に基づいて、前記ロボットの駆動を制御するロボット駆動制御部と、
を備えるロボットシステム。
【請求項２】
前記パンチルト制御部は、前記第１の撮影光学系及び前記第２の撮影光学系の前記望遠光学系により、前記測定対象物の一部を捕捉させる請求項１に記載のロボットシステム。
【請求項３】
前記画像取得部が取得した前記第１の広角画像又は前記第２の広角画像に基づいて、前記測定対象物の動きを予測する予測部を更に備える請求項１又は２に記載のロボットシステム。
【請求項４】
前記パンチルト制御部は、前記予測部の予測結果に応じて前記パンチルト機構を制御する請求項３に記載のロボットシステム。
【請求項５】
前記ロボット駆動制御部は、前記予測部の予測結果に応じて前記ロボットの動きを制御する請求項３又は４に記載のロボットシステム。
【請求項６】
前記パンチルト制御部は、前記第１の撮影光学系及び前記第２の撮影光学系の前記望遠光学系により前記測定対象物と前記ロボットの特定部位とを捕捉させ、
前記距離情報算出部は、前記測定対象物と前記ロボットの特定部位との前記相対的な三次元距離情報を算出する請求項１から５のいずれか１項に記載のロボットシステム。
【請求項７】
前記パンチルト制御部は、前記第１の撮影光学系及び前記第２の撮影光学系の前記望遠光学系により前記測定対象物と前記ロボットのエンドエフェクタとを捕捉させ、
前記距離情報算出部は、前記測定対象物と前記エンドエフェクタとの前記相対的な三次元距離情報を算出する請求項１から６のいずれか１項に記載のロボットシステム。
【請求項８】
前記ロボットが移動するエリアのエリア情報を取得するエリア情報取得部と、
前記第１の広角画像又は前記第２の広角画像に基づいて、前記エリア情報取得部で取得した前記エリア情報における前記測定対象物の位置を検出する位置検出部と、
前記距離情報算出部で算出された前記相対的な三次元距離情報及び位置検出部の検出結果により、前記ロボットの自己位置を推定する自己位置推定部と、を備える請求項１から７のいずれか１項に記載のロボットシステム。
【請求項９】
前記ステレオカメラは前記ロボットに備えられている請求項１から８のいずれか１項に記載のロボットシステム。
【請求項１０】
前記ステレオカメラは、前記ロボットとは別体で設けられている請求項１から８のいずれか１項に記載のロボットシステム。
【請求項１１】
前記距離情報算出部は、前記第１の望遠画像及び前記第２の望遠画像に基づいてそれぞれ特徴量が一致する２つの対応点を検出する対応点検出部を備え、前記対応点検出部により検出した前記２つの対応点の前記第１の指向性センサ及び前記第２の指向性センサにおける視差量と、前記基線長と、前記第１の撮影光学系及び前記第２の撮影光学系のそれぞれの光軸方向と、前記望遠光学系の焦点距離とに基づいて前記対応点の三次元距離情報を算出する請求項１から１０のいずれか１項に記載のロボットシステム。
【請求項１２】
前記第１の撮影光学系及び前記第２の撮影光学系は、それぞれ円形の中央光学系からなる前記広角光学系と、前記中央光学系に対して同心円状に配設された環状光学系からなる前記望遠光学系とにより構成された請求項１から１１のいずれか１項に記載のロボットシステム。
【請求項１３】
前記第１の撮影光学系及び前記第２の撮影光学系のそれぞれの前記望遠光学系の焦点調節を行う第１の焦点調節部を有する請求項１から１２のいずれか１項に記載のロボットシステム。
【請求項１４】
それぞれ光軸が一致し、かつ異なる領域に配置された広角光学系と望遠光学系とを有する第１の撮影光学系と、前記広角光学系及び前記望遠光学系を介して入射する光束をそれぞれ瞳分割して選択的に受光する複数の画素を有する第１の指向性センサとを含む第１の撮像部と、前記第１の撮影光学系と同一構成の第２の撮影光学系と、前記第１の指向性センサと同一構成の第２の指向性センサとを含む第２の撮像部であって、前記第１の撮像部に対して基線長だけ離間して配置された前記第２の撮像部と、前記第１の指向性センサ及び前記第２の指向性センサから第１の広角画像及び第２の広角画像と、第１の望遠画像及び第２の望遠画像とを取得する画像取得部と、を備えるステレオカメラ及びロボットから構成されるロボットシステムの制御方法であって、
前記第１の撮像部及び前記第２の撮像部をそれぞれ水平方向及び垂直方向に回動させるパンチルト機構を制御し、前記画像取得部により取得した前記第１の広角画像及び前記第２の広角画像に基づいて、前記第１の撮影光学系及び前記第２の撮影光学系の前記望遠光学系で測定対象物をそれぞれ捕捉させるパンチルト制御ステップと、
前記画像取得部により取得した前記第１の望遠画像及び前記第２の望遠画像に基づいて前記測定対象物と前記ロボットの基準部位との相対的な三次元距離情報を算出する距離情報算出ステップと、
前記距離情報算出ステップで算出された前記相対的な三次元距離情報に基づいて、前記ロボットの駆動を制御するロボット駆動制御ステップと、
を含むロボットシステムの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットシステム及びロボットシステムの制御方法に関し、特にステレオカメラを備えたロボットシステム及びそのロボットシステムの制御方法に関する。
【背景技術】
【０００２】
従来、複数（４台）のカメラを搭載した複数のカメラシステム用雲台装置が提案されている（特許文献１）。
【０００３】
特許文献１に記載の複数のカメラシステム用雲台装置は、２台の複眼カメラユニットを搭載し、各複眼カメラユニットをパン方向及びチルト方向に制御可能とする２台の第１の雲台手段（子雲台）と、２台の第１の雲台手段（２台の複眼カメラユニット）を搭載し、２台の複眼カメラユニットをパン方向又はチルト方向に制御可能とする第２の雲台手段（親雲台）と、から成り、２台の複眼カメラユニットを第２の雲台上で離間して搭載している。
【０００４】
各複眼カメラユニットは、広角レンズカメラとズームカメラとを含み、広角レンズカメラの光軸とズームカメラの光軸は、デフォルトで同じ方向を向いている。
【０００５】
ズームレンズカメラは、広角レンズカメラと同じ光軸方向を向いているため、広角レンズの画角内で被写体の詳細をズームアップし、詳細な望遠画像を撮像することができる。更に、複数のカメラシステム用雲台装置は、捕捉された被写体に向けて第２の雲台手段を制御することで、装置を被写体に正対させることができ、装置が被写体を捕捉し、正対した時点で装置と被写体との間の距離を三角測量法により測定できるようにしている。
【０００６】
一方、光軸が一致する広角光学系及び望遠光学系を有する単一の撮像部により広角画像と望遠画像とを同時に取得し、かつ追尾対象の物体を望遠画像で確実に捉えることができる自動追尾撮像装置が提案されている（特許文献２）。
【０００７】
特許文献２に記載の自動追尾撮像装置は、共通する光軸上に配置された広角光学系である中央光学系と、望遠光学系である環状光学系からなる撮影光学系と、広角光学系及び望遠光学系を介して入射する光束をそれぞれ瞳分割して選択的に受光する指向性センサと、パンチルト機構と、指向性センサから取得した広角画像及び望遠画像のうちの少なくとも広角画像に基づいて追尾対象の物体を検出する物体検出部と、物体検出部により検出された物体の画像中における位置情報に基づいてパンチルト機構を制御するパンチルト制御部とから構成されている。
【０００８】
特許文献２に記載の自動追尾撮像装置は、光軸が一致する広角光学系と望遠光学系とを有する単一の撮像部により広角画像と望遠画像とを同時に取得することができるため、装置の小型化、低コスト化を図ることができ、また、広角光学系と望遠光学系の光軸が一致しているため、少なくとも広角画像に基づいて追尾対象の物体を検出し、検出された物体の画像中における位置情報に基づいてパンチルト機構を制御することにより、物体を望遠画像内に入れること（自動追尾すること）ができ、また、物体が高速で移動しても広角画像により物体を捉えることができるため、トラッキング逸脱が発生することがない。
【０００９】
また特許文献３には、頭部にステレオカメラを備えたロボットが記載されている。特許文献３に記載された技術では、ステレオカメラで取得されたステレオ画像に基づいて距離情報が算出され、その距離情報に基づいてロボットの移動が制御されている。
【先行技術文献】
【特許文献】
【００１０】
【特許文献１】特開２０１１−１０９６３０号公報
【特許文献２】特開２０１５−１５４３８６号公報
【特許文献３】特開２００３−２６６３４５号公報
【発明の概要】
【発明が解決しようとする課題】
【００１１】
特許文献１に記載の複数のカメラシステム用雲台装置は、２台の複眼カメラユニット（２台の広角レンズカメラと２台のズームカメラ）を搭載しているため、ステレオ画像である２つの広角画像とステレオ画像である２つの望遠画像とを同時に撮像することができるが、各複眼カメラユニットは、広角レンズカメラとズームカメラとを含むため、大型化するという問題がある。
【００１２】
また、複眼カメラユニットを構成する広角レンズカメラの光軸とズームカメラの光軸は、同じ方向を向いているが、両者の光軸は平行であって、一致していない。従って、広角レンズカメラが被写体（主要被写体）を光軸上で捕捉しているときに、ズームカメラをズームアップして主要被写体の詳細を撮像する場合、主要被写体は、ズームアップされた望遠画像の中心（ズームカメラの光軸）からずれた位置に撮像され、また、ズームカメラのズーム倍率が高倍率の場合には、ズームカメラの画角から主要被写体が外れ、ズームカメラにより主要被写体を捉えることができなくなるという問題がある。
【００１３】
一方、特許文献２に記載の自動追尾撮像装置は、光軸が一致する広角光学系と望遠光学系とを有する単一の撮像部により広角画像と望遠画像とを同時に取得することができるため、広角画像の中心で追尾対象の物体を捕捉するようにパンチルト機構を制御すると、望遠画像の中心で追尾対象の物体を捕捉することができるが、主要被写体の距離情報等を取得することができず、また、主要被写体の距離情報に基づいてズームカメラを主要被写体に合焦させる自動焦点調節方法を適用することができない。
【００１４】
また、特許文献３に記載されたロボットには、通常のステレオカメラが設けられていないので、広角光学系及び望遠光学系を有するステレオカメラが設けられているわけではないために、ステレオカメラから得られる距離情報の精度が落ちる場合がある。すなわち、広角光学系のみを有するステレオカメラでは、画角が大きいので被写体を精度よく捉えることができるが、広角光学系で被写体を捉えるので分解能に起因して距離情報の精度が落ちてしまう場合がある。一方、望遠広角系のみを有するステレオカメラでは、被写体の距離情報の精度は良いものの、画角が小さいのでうまく被写体を捉えることができない場合が発生する。
【００１５】
本発明はこのような事情に鑑みてなされたもので、その目的は、ステレオ画像である２つの広角画像とステレオ画像である２つの望遠画像とを同時に取得することができ、特に２つの望遠画像を取得する際に２つの望遠光学系の光軸上で測定対象物を確実に捕捉することができる小型化可能なステレオカメラを備え、正確な距離情報に基づいてロボットを駆動させることができるロボットシステム及びロボットシステムの制御方法を提供することである。
【課題を解決するための手段】
【００１６】
上記目的を達成するために、本発明の一の態様であるロボットシステムは、それぞれ光軸が一致し、かつ異なる領域に配置された広角光学系と望遠光学系とを有する第１の撮影光学系と、広角光学系及び望遠光学系を介して入射する光束をそれぞれ瞳分割して選択的に受光する複数の画素を有する第１の指向性センサとを含む第１の撮像部と、第１の撮影光学系と同一構成の第２の撮影光学系と、第１の指向性センサと同一構成の第２の指向性センサとを含む第２の撮像部であって、第１の撮像部に対して基線長だけ離間して配置された第２の撮像部と、第１の指向性センサ及び第２の指向性センサから第１の広角画像及び第２の広角画像と、第１の望遠画像及び第２の望遠画像とを取得する画像取得部と、を備えるステレオカメラ及びロボットから構成されるロボットシステムであって、第１の撮像部及び第２の撮像部をそれぞれ水平方向及び垂直方向に回動させるパンチルト機構と、画像取得部により取得した第１の広角画像及び第２の広角画像に基づいてパンチルト機構を制御し、第１の撮影光学系及び第２の撮影光学系の望遠光学系で測定対象物をそれぞれ捕捉させるパンチルト制御部と、画像取得部により取得した第１の望遠画像及び第２の望遠画像に基づいて測定対象物とロボットの基準部位との相対的な三次元距離情報を算出する距離情報算出部と、距離情報算出部で算出された相対的な三次元距離情報に基づいて、ロボットの駆動を制御するロボット駆動制御部と、を備える。
【００１７】
本態様によれば、互いに基線長だけ離間して配置された第１の撮像部と第２の撮像部とは、パンチルト機構によりそれぞれ水平方向及び垂直方向に回動することができ、また、第１の撮像部は、それぞれ光軸が一致し、かつ異なる領域に配置された広角光学系と望遠光学系とを有する第１の撮影光学系と、広角光学系及び望遠光学系を介して入射する光束をそれぞれ瞳分割して選択的に受光する複数の画素を有する第１の指向性センサとを含み、第２の撮像部は、第１の撮像部と同様に第２の撮影光学系と第２の指向性センサを含んで構成されている。
【００１８】
そして、第１の撮像部及び第２の撮像部からステレオ画像である第１の広角画像と第２の広角画像、及びステレオ画像である第１の望遠画像と第２の望遠画像を同時に取得することができ、特に第１の撮影光学系を構成する広角光学系と望遠光学系とは光軸が一致し、同様に第２の撮影光学系を構成する広角光学系と望遠光学系とは光軸が一致しているため、第１の広角画像及び第２の広角画像に基づいて第１の撮影光学系及び第２の撮影光学系のそれぞれの光軸上（光軸が交差するクロスポイント）で測定対象物を捕捉するようにパンチルト機構を制御することにより、第１の望遠画像及び第２の望遠画像の中心位置（光軸に対応する位置）に測定対象物を位置させることができる。また、第１の広角画像及び第２の広角画像に基づいてパンチルト機構を制御するため、測定対象物を見失うことなく確実に捕捉することができる。
【００１９】
また、第１の望遠画像及び第２の望遠画像は、ステレオ画像として撮像されるため、測定対象物の距離情報を算出することができるが、第１の望遠画像及び第２の望遠画像は、第１の広角画像及び第２の広角画像に比べて空間分解能が高いため、精度の高い距離情報を算出することができる。ここでパンチルト制御部は、第１の広角画像及び第２の広角画像に基づいて第１の撮影光学系及び第２の撮影光学系の望遠光学系により測定対象物を捕捉させているので、第１の望遠画像及び第２の望遠画像に基づいて測定対象物の距離情報を算出することができる。そして、ロボットは算出された精度の高い距離情報に基づいて正確に駆動制御される。
【００２０】
好ましくは、パンチルト制御部は、第１の撮影光学系及び第２の撮影光学系の望遠光学系により、測定対象物の一部を捕捉させる。
【００２１】
本態様によれば、パンチルト制御部が第１の撮影光学系及び第２の撮影光学系の望遠光学系により測定対象物の一部が捕捉されるので、測定対象物の精度の高い距離情報を取得することができる。
【００２２】
好ましくは、ロボットシステムは、画像取得部が取得した第１の広角画像又は第２の広角画像に基づいて、測定対象物の動きを予測する予測部を更に備える。
【００２３】
本態様によれば、予測部により、第１の広角画像又は第２の広角画像に基づいて、測定対象物の動きが予測されるので、動きが早い測定対象物であっても広角画像で捉えることが可能であり、より正確な動きの予測を行うことができる。
【００２４】
好ましくは、パンチルト制御部は、予測部の予測結果に応じてパンチルト機構を制御する。
【００２５】
本態様によれば、パンチルト制御部が予測部の予測結果に応じてパンチルト機構を制御するので、測定対象物が動く場合であっても、正確に測定対象物を捕捉することができる。
【００２６】
好ましくは、ロボット駆動制御部は、予測部の予測結果に応じてロボットの動きを制御する。
【００２７】
本態様によれば、ロボット駆動制御部が予測部の予測結果に応じてロボットの動きを制御するので、測定対象物が動く場合であってステレオカメラをパン又はチルトを行うだけでは、捕捉が困難な場合であっても、ロボットを移動させることによって捕捉させることができる。
【００２８】
好ましくは、パンチルト制御部は、第１の撮影光学系及び第２の撮影光学系の望遠光学系により測定対象物とロボットの特定部位とを捕捉させ、距離情報算出部は、測定対象物とロボットの特定部位との相対的な三次元距離情報を算出する。
【００２９】
本態様によれば、パンチルト制御部により、第１の撮影光学系及び第２の撮影光学系の望遠光学系により測定対象物とロボットの特定部位とを捕捉させ、距離情報算出部により、測定対象物とロボットの特定部位との相対的な三次元距離情報が算出される。これにより、測定対象物とロボットの特定部位との正確な距離情報を取得することができるので、ロボットと測定対象物との間の距離を正確に制御させることができる。
【００３０】
好ましくは、パンチルト制御部は、第１の撮影光学系及び第２の撮影光学系の望遠光学系により測定対象物とロボットのエンドエフェクタとを捕捉させ、距離情報算出部は、測定対象物とエンドエフェクタとの相対的な三次元距離情報を算出する。
【００３１】
本態様によれば、パンチルト制御部により、第１の撮影光学系及び第２の撮影光学系の望遠光学系により測定対象物とロボットのエンドエフェクタとを捕捉させ、距離情報算出部により、測定対象物とエンドエフェクタとの相対的な三次元距離情報を算出する。これにより、測定対象物とエンドエフェクタとの正確な距離情報を取得することができるので、ロボットと測定対象物との間の距離を正確に制御させることができる。
【００３２】
好ましくは、ロボットシステムは、ロボットが移動するエリアのエリア情報を取得するエリア情報取得部と、第１の広角画像又は第２の広角画像に基づいて、エリア情報取得部で取得したエリア情報における測定対象物の位置を検出する位置検出部と、距離情報算出部で算出された相対的な三次元距離情報及び位置検出部の検出結果により、ロボットの自己位置を推定する自己位置推定部と、を備える。
【００３３】
本態様によれば、エリア情報取得部によりロボットが移動するエリアのエリア情報が取得され、位置検出部により、第１の広角画像又は第２の広角画像に基づいて、エリア情報取得部で取得したエリア情報における測定対象物の位置が検出され、自己位置推定部により距離情報算出部で算出された相対的な三次元距離情報及び位置検出部の検出結果に基づきロボットの自己位置が推定される。これにより本態様では、正確なロボットの位置情報を取得することができる。
【００３４】
好ましくは、ステレオカメラはロボットに備えられている。
【００３５】
本態様によれば、ステレオカメラはロボットに備えられているので、ロボットが移動する場所の回りの正確な距離情報を取得することができる。
【００３６】
好ましくは、ステレオカメラは、ロボットとは別体で設けられている。
【００３７】
本態様によれば、ステレオカメラは、ロボットとは別体で設けられているので、ロボットの回りの距離情報を取得しつつ、望遠画像でロボットの移動する環境の距離情報も取得することができる。
【００３８】
好ましくは、距離情報算出部は、第１の望遠画像及び第２の望遠画像に基づいてそれぞれ特徴量が一致する２つの対応点を検出する対応点検出部を備え、対応点検出部により検出した２つの対応点の第１の指向性センサ及び第２の指向性センサにおける視差量と、基線長と、第１の撮影光学系及び第２の撮影光学系のそれぞれの光軸方向と、望遠光学系の焦点距離とに基づいて対応点の三次元距離情報を算出する。
【００３９】
本態様によれば、第１の撮像光学系及び第２の撮像光学系のそれぞれの光軸上の測定対象物の距離情報だけでなく、第１の望遠画像及び第２の望遠画像から対応点が検出された被写体の距離情報も算出することができる。
【００４０】
好ましくは、第１の撮影光学系及び第２の撮影光学系は、それぞれ円形の中央光学系からなる広角光学系と、中央光学系に対して同心円状に配設された環状光学系からなる望遠光学系とにより構成されている。
【００４１】
本態様によれば、円形の中央光学系からなる広角光学系と、中央光学系に対して同心円状に配設された環状光学系からなる望遠光学系とにより撮像される２つの画像間には視差が発生せず、また、それぞれ回転対称の形状であるため、撮影光学系として好ましい。
【００４２】
好ましくは、第１の撮影光学系及び第２の撮影光学系のそれぞれの望遠光学系の焦点調節を行う第１の焦点調節部を有する。
【００４３】
本態様によれば、望遠光学系は、広角光学系に比べて被写界深度が浅く、ボケやすいため、焦点調節を行うことが好ましい。
【００４４】
本発明の他の態様であるロボットシステムの制御方法は、それぞれ光軸が一致し、かつ異なる領域に配置された広角光学系と望遠光学系とを有する第１の撮影光学系と、広角光学系及び望遠光学系を介して入射する光束をそれぞれ瞳分割して選択的に受光する複数の画素を有する第１の指向性センサとを含む第１の撮像部と、第１の撮影光学系と同一構成の第２の撮影光学系と、第１の指向性センサと同一構成の第２の指向性センサとを含む第２の撮像部であって、第１の撮像部に対して基線長だけ離間して配置された第２の撮像部と、第１の指向性センサ及び第２の指向性センサから第１の広角画像及び第２の広角画像と、第１の望遠画像及び第２の望遠画像とを取得する画像取得部と、を備えるステレオカメラ及びロボットから構成されるロボットシステムの制御方法であって、第１の撮像部及び第２の撮像部をそれぞれ水平方向及び垂直方向に回動させるパンチルト機構を制御し、画像取得部により取得した第１の広角画像及び第２の広角画像に基づいて、第１の撮影光学系及び第２の撮影光学系の望遠光学系で測定対象物をそれぞれ捕捉させるパンチルト制御ステップと、画像取得部により取得した第１の望遠画像及び第２の望遠画像に基づいて測定対象物とロボットの基準部位との相対的な三次元距離情報を算出する距離情報算出ステップと、距離情報算出ステップで算出された相対的な三次元距離情報に基づいて、ロボットの駆動を制御するロボット駆動制御ステップと、を含む。
【発明の効果】
【００４５】
本発明によれば、ステレオ画像である第１の広角画像と第２の広角画像、及びステレオ画像である第１の望遠画像と第２の望遠画像を同時に取得することができ、特に第１の撮影光学系を構成する広角光学系と望遠光学系とは光軸が一致し、同様に第２の撮影光学系を構成する広角光学系と望遠光学系とは光軸が一致しているため、第１の広角画像及び第２の広角画像に基づいて第１の撮影光学系及び第２の撮影光学系の望遠光学系で測定対象物を捕捉するようにパンチルト機構を制御することにより、正確な距離情報に基づいてロボットを駆動させることができる。尚、第１の広角画像及び第２の広角画像に基づいてパンチルト機構を制御するため、測定対象物を見失うことなく確実に捕捉することができる。
【００４６】
また、第１の望遠画像及び第２の望遠画像は、ステレオ画像として撮像されるため、測定対象物の距離情報を算出することができるが、第１の望遠画像及び第２の望遠画像は、第１の広角画像及び第２の広角画像に比べて空間分解能が高いため、精度の高い距離情報を算出することができる。
【図面の簡単な説明】
【００４７】
【図１】カメラシステムを構成するロボットを示す概念図である。
【図２】本発明に係るステレオカメラの外観斜視図である。
【図３】ステレオカメラの第１の撮像部の第１の実施形態を示す断面図である。
【図４】図３に示したマイクロレンズアレイ及びイメージセンサの要部拡大図である。
【図５】ステレオカメラの内部構成の実施形態を示すブロック図である。
【図６】ステレオカメラにより撮像される広角画像及び望遠画像の一例を示す図である。
【図７】特定の測定対象物（人物の顔）が第１の望遠画像の中心にくるようにパンチルト制御された状態を示す第１の望遠画像を示すイメージ図である。
【図８】測定対象物等の距離情報の算出方法を説明するために用いた図である。
【図９】第１の焦点調節部を示すブロック図である。
【図１０】パンチルト制御部のブロック図である。
【図１１】測定対象物の動きを予測する場合のロボットシステムにおけるブロック図である。
【図１２】第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２上で測定対象物を捕捉させるパンチルト制御を示すフローチャートである。
【図１３】第１の望遠画像と第２の望遠画像との間で特徴量が一致する対応点の距離情報を取得する処理を示すフローチャートである。
【図１４】動体である測定対象物を検出する動体検出方法の一例を説明するために用いた図である。
【図１５】動体である測定対象物を検出する動体検出方法の一例を説明するために用いた図である。
【図１６】動体である測定対象物を検出する動体検出方法の一例を説明するために用いた図である。
【図１７】ステレオカメラにより撮像される広角画像及び望遠画像の一例を示す図である。
【図１８】ロボットの変形例を示す概念図である。
【図１９】自走型ロボットが自己位置推定を行いながら自走している場合を示す概念図である。
【図２０】ロボットシステムが備える自己位置推定における機能ブロック図である。
【図２１】ロボットの変形例を示す概念図である。
【図２２】ロボットの変形例を示す概念図である。
【発明を実施するための形態】
【００４８】
以下、添付図面に従って本発明に係るロボットシステム及びロボットシステムの制御方法の好ましい実施の形態について説明する。
【００４９】
図１は、本発明のカメラシステムを構成するロボットを示す概念図である。ロボットシステム５００は、主にステレオカメラ１（図２）とロボット３００から構成されている。図１に示されたロボット３００は、ステレオカメラ１が備えられた人型ロボットであり、前進、後退、及び左右への移動が行われる。また、ロボット３００はエンドエフェクタ３０４を駆動させることにより、様々な作業が行われる。例えばロボット３００は、人の介助、又は人の行う作業の手伝いを行う。従って、ロボット３００の駆動は正確な距離情報に基づき行われる必要がある。
【００５０】
ロボット３００の頭部３０２にはステレオカメラ１（図２を参照）が設けられている。ステレオカメラ１にはパンチルト機構３２（図５を参照）が設けられて、ロボット３００の移動範囲内の環境をステレオカメラ１に撮影させることが可能である。
【００５１】
＜ステレオカメラの外観＞
図２は本発明に係るステレオカメラ１の外観斜視図である。
【００５２】
図２に示すようにステレオカメラ１は、主として左目用の第１のパンチルトカメラ１０Ｌと、右目用の第２のパンチルトカメラ１０Ｒと、第１のパンチルトカメラ１０Ｌと第２のパンチルトカメラ１０Ｒとを基線長Ｄだけ離間して保持する保持部材３７と、保持部材３７を水平方向に回動させるパン機構を含むパン装置３８と、を有している。
【００５３】
第１のパンチルトカメラ１０Ｌは、第１の撮像部１１Ｌ、第１のパンチルト装置３０Ｌ（図５）、及び第１のカメラ本体２Ｌから構成され、第２のパンチルトカメラ１０Ｒは、第２の撮像部１１Ｒ、第２のパンチルト装置３０Ｒ（図５）、及び第２のカメラ本体２Ｒから構成されている。
【００５４】
第１のパンチルト装置３０Ｌは、台座４Ｌと、台座４Ｌに固定されるとともに、第１の撮像部１１Ｌを回動自在に保持する保持部８Ｌとを有している。
【００５５】
台座４Ｌは、第１のカメラ本体２Ｌの垂直方向Ｚの軸を中心に回転自在に配設され、パン駆動部３４（図５）により垂直方向Ｚの軸を中心にして回転（パン動作）する。
【００５６】
保持部８Ｌは、水平方向Ｘの軸と同軸上に設けられたギア８Ａを有し、チルト駆動部３６（図５）からギア８Ａを介して駆動力が伝達されることにより、第１の撮像部１１Ｌを上下方向に回動（チルト動作）させる。
【００５７】
同様に、第２のパンチルト装置３０Ｒは、台座４Ｒと、台座４Ｒに固定されるとともに、第２の撮像部１１Ｒを回動自在に保持する保持部８Ｒとを有している。
【００５８】
台座４Ｒは、第２のカメラ本体２Ｒの垂直方向Ｚの軸を中心に回転自在に配設され、パン駆動部３４により垂直方向Ｚの軸を中心にして回転（パン動作）する。
【００５９】
保持部８Ｒは、水平方向Ｘの軸と同軸上に設けられたギア８Ａを有し、チルト駆動部３６からギア８Ａを介して駆動力が伝達されることにより、第２の撮像部１１Ｒを上下方向にチルト動作させる。
【００６０】
従って、第１のパンチルトカメラ１０Ｌは、パン動作及びチルト動作することで、第１のパンチルトカメラ１０Ｌの撮影方向（第１の撮影光学系１２Ｌ（図３参照）の光軸Ｌ１の方向）を所望の方向に向けることができ、同様に第２のパンチルトカメラ１０Ｒは、パン動作及びチルト動作することで、第１のパンチルトカメラ１０Ｌとは独立して第２のパンチルトカメラ１０Ｒの撮影方向（第２の撮影光学系１２Ｒ（図３参照）の光軸Ｌ２の方向）を所望の方向に向けることができる。
【００６１】
＜第１の撮像部１１Ｌの構成＞
図３は、ステレオカメラ１の第１の撮像部１１Ｌの第１の実施形態を示す断面図である。
【００６２】
図３に示すように、第１の撮像部１１Ｌは、第１の撮影光学系１２Ｌと第１の指向性センサ１７Ｌとから構成されている。
【００６３】
＜第１の撮影光学系１２Ｌ＞
第１の撮影光学系１２Ｌは、それぞれ同一の光軸上に配置された、円形の中央光学系からなる広角光学系１３と、広角光学系１３に対して同心円状に配設された環状光学系からなる望遠光学系１４とから構成されている。
【００６４】
広角光学系１３は、第１レンズ１３ａ、第２レンズ１３ｂ、第３レンズ１３ｃ、第４レンズ１３ｄ、及び共通レンズ１５から構成された広角レンズであり、第１の指向性センサ１７Ｌを構成するマイクロレンズアレイ１６上に広角画像を結像させる。
【００６５】
望遠光学系１４は、第１レンズ１４ａ、第２レンズ１４ｂ、反射光学系としての第１反射ミラー１４ｃ、第２反射ミラー１４ｄ、及び共通レンズ１５から構成された望遠レンズであり、第１の指向性センサ１７Ｌを構成するマイクロレンズアレイ１６上に望遠画像を結像させる。
【００６６】
第１レンズ１４ａ、及び第２レンズ１４ｂを介して入射した光束は、第１反射ミラー１４ｃ及び第２反射ミラー１４ｄにより２回反射された後、共通レンズ１５を通過する。第１反射ミラー１４ｃ及び第２反射ミラー１４ｄにより光束が折り返されることにより、焦点距離の長い望遠光学系（望遠レンズ）１４の光軸方向の長さを短くしている。
【００６７】
＜第１の指向性センサ１７Ｌ＞
第１の指向性センサ１７Ｌは、マイクロレンズアレイ１６とイメージセンサ１８とから構成されており、入射する光束をそれぞれ瞳分割して選択的に受光する複数の画素を有する。
【００６８】
図４は、図３に示したマイクロレンズアレイ１６及びイメージセンサ１８の要部拡大図である。
【００６９】
マイクロレンズアレイ１６は、複数のマイクロレンズ（瞳結像レンズ）１６ａが２次元状に配列されて構成されており、各マイクロレンズの水平方向及び垂直方向の間隔は、イメージセンサ１８の光電変換素子である受光セル１８ａの３つ分の間隔に対応している。即ち、マイクロレンズアレイ１６の各マイクロレンズは、水平方向及び垂直方向の各方向に対して、２つ置きの受光セルの位置に対応して形成されたものが使用される。
【００７０】
また、マイクロレンズアレイ１６の各マイクロレンズ１６ａは、第１の撮影光学系１２Ｌの広角光学系１３及び望遠光学系１４に対応する、円形の中央瞳像（第１の瞳像）１７ａ及び環状瞳像（第２の瞳像）１７ｂを、イメージセンサ１８の対応する受光領域の受光セル１８ａ上に結像させる。
【００７１】
図４に示すマイクロレンズアレイ１６及びイメージセンサ１８によれば、マイクロレンズアレイ１６の１マイクロレンズ１６ａ当たりに付き、格子状（正方格子状）の３×３個の受光セル１８ａが割り付けられている。以下、１つのマイクロレンズ１６ａ及び１つのマイクロレンズ１６ａに対応する受光セル群（３×３個の受光セル１８ａ）を単位ブロックという。
【００７２】
中央瞳像１７ａは、単位ブロックの中央の受光セル１８ａのみに結像し、環状瞳像１７ｂは、単位ブロックの周囲の８個の受光セル１８ａに結像する。
【００７３】
上記構成の第１の撮像部１１Ｌによれば、後述するように広角光学系１３に対応する第１の広角画像と、望遠光学系１４に対応する第１の望遠画像とを同時に撮像することができる。
【００７４】
ステレオカメラ１の第２の撮像部１１Ｒは、図３及び図４に示した第１の撮像部１１Ｌと同様に構成されており、広角光学系１３と望遠光学系１４とを有する第２の撮影光学系１２Ｒと、第２の指向性センサとを有し、広角光学系１３に対応する第２の広角画像と、望遠光学系１４に対応する第２の望遠画像とを同時に撮像することができる。
【００７５】
＜ステレオカメラ１の内部構成＞
図５は、ステレオカメラ１の内部構成の実施形態を示すブロック図である。尚、図２で説明したようにステレオカメラ１は、左目用の第１のパンチルトカメラ１０Ｌと、右目用の第２のパンチルトカメラ１０Ｒとを備えているが、両カメラの構成は同じであるため、図５では、主として第１のパンチルトカメラ１０Ｌについて説明し、第２のパンチルトカメラ１０Ｒの詳細な説明は省略する。
【００７６】
図５に示すようにステレオカメラ１を構成する左目用の第１のパンチルトカメラ１０Ｌは、図３で説明した広角光学系１３及び望遠光学系１４を有する第１の撮影光学系１２Ｌと、図４で説明したマイクロレンズアレイ１６及びイメージセンサ１８を有する第１の指向性センサ１７Ｌとからなる第１の撮像部１１Ｌを備えている。
【００７７】
この第１の撮像部１１Ｌは、少なくとも望遠光学系１４の焦点調節を自動的に行う第１の焦点調節部（焦点調節機構１９を含む）を備えることが好ましい。焦点調節機構１９は、例えば、望遠光学系１４の全体又は一部の光学系を光軸方向に移動させるボイスコイルモータ等により構成することができる。また、第１の焦点調節部は、後述する焦点制御部１９０が測定対象物（主要被写体）の距離情報を取得し、取得した距離情報に基づいて焦点調節機構１９を駆動することにより、望遠光学系１４の第１の焦点調節を行うことができる。
【００７８】
また、広角光学系１３については、別途、広角光学系１３の焦点調節を自動的に行う第２の焦点調節部を設けるようにしてよいし、パンフォーカスとしてもよい。広角光学系１３に適用される第２の焦点調節部としては、コントラストＡＦ（Autofocus）方式、あるいは像面位相差ＡＦ方式のものが考えられる。コントラストＡＦ方式は、第１の広角画像及び第２の広角画像のＡＦ領域のコントラストが最大になるように広角光学系１３の全体又は一部の光学系を光軸方向に移動させて焦点調節を行う方式であり、像面位相差ＡＦ方式は、第１の広角画像及び第２の広角画像のＡＦ領域の像面位相差が０になるように広角光学系１３の全体又は一部の光学系を光軸方向に移動させることにより広角光学系１３の焦点調節を行う方式である。
【００７９】
第１のパンチルト装置３０Ｌは、図２に示したように第１の撮像部１１Ｌを第１のカメラ本体２Ｌに対して水平方向（パン方向）に回転させるパン機構及び垂直方向（チルト方向）に回動させるチルト機構（以下、「パンチルト機構」と称す）３２、パン駆動部３４、及びチルト駆動部３６等を備えている。
【００８０】
パンチルト機構３２は、パン方向の回転角（パン角度）の基準位置を検出するホームポジションセンサ、チルト方向の傾き角（チルト角度）の基準位置を検出するホームポジションセンサを有している。
【００８１】
パン駆動部３４及びチルト駆動部３６は、それぞれステッピングモータ及びモータドライバを有し、パンチルト機構３２に駆動力を出力し、パンチルト機構３２を駆動する。
【００８２】
第１の撮像部１１Ｌは、第１の撮影光学系１２Ｌ及び第１の指向性センサ１７Ｌを介して時系列の第１の広角画像及び第１の望遠画像を撮像するものであり、第１の撮影光学系１２Ｌを介して第１の指向性センサ１７Ｌ（イメージセンサ１８）の各受光セル（光電変換素子）の受光面に結像された被写体像は、その入射光量に応じた量の信号電圧（又は電荷）に変換される。
【００８３】
イメージセンサ１８に蓄積された信号電圧（又は電荷）は、受光セルそのもの若しくは付設されたキャパシタに蓄えられる。蓄えられた信号電圧（又は電荷）は、Ｘ−Ｙアドレス方式を用いたＭＯＳ（（Metal Oxide Semiconductor）型撮像素子（いわゆるComplementary Metal Oxide Semiconductor）ＣＭＯＳセンサ）の手法を用いて、受光セル位置の選択とともに読み出される。
【００８４】
これにより、イメージセンサ１８から広角光学系１３に対応する中央の受光セルのグループの第１の広角画像を示す画像信号と、望遠光学系１４に対応する周囲８個の受光セルのグループの第１の望遠画像を示す画像信号とを読み出すことができる。尚、イメージセンサ１８からは、所定のフレームレート（例えば、１秒当たりのフレーム数２４ｐ，３０ｐ，又は６０ｐ）で、第１の広角画像及び第１の望遠画像を示す画像信号が連続して読み出されるが、図示しないシャッターボタンが操作されると、それぞれ１枚の静止画となる第１の広角画像及び第１の望遠画像を示す画像信号が読み出される。
【００８５】
イメージセンサ１８から読み出された画像信号（電圧信号）は、相関二重サンプリング処理（センサ出力信号に含まれるノイズ（特に熱雑音）等を軽減することを目的として、受光セル毎の出力信号に含まれるフィードスルー成分レベルと信号成分レベルとの差をとることにより正確な画素データを得る処理）により受光セル毎の画像信号がサンプリングホールドされ、増幅されたのちＡ／Ｄ（Analog/Digital）変換器２０に加えられる。Ａ／Ｄ変換器２０は、順次入力する画像信号をデジタル信号に変換して画像取得部２２に出力する。尚、ＭＯＳ型センサでは、Ａ／Ｄ変換器が内蔵されているものがあり、この場合、イメージセンサ１８から直接デジタル信号が出力される。
【００８６】
第１の画像取得部２２Ｌは、イメージセンサ１８の受光セル位置を選択して画像信号を読み出すことにより、第１の広角画像を示す画像信号と第１の望遠画像を示す画像信号とを同時に、又は選択的に取得することができる。
【００８７】
即ち、イメージセンサ１８の中央瞳像１７ａが入射する受光セルの画像信号を選択的に読み出すことにより、１マイクロレンズ当たり１個の受光セル（３×３の受光セルの中央の受光セル）の第１の広角画像を示す画像信号（ベイヤ配列のモザイク画像を示す画像信号）を取得することができ、一方、イメージセンサ１８の環状瞳像１７ｂが入射する受光セルの画像信号を選択的に読み出すことにより、１マイクロレンズ当たり８個の受光セル（３×３の受光セルの周囲の受光セル）の第１の望遠画像を示す画像信号を取得することができる。
【００８８】
尚、イメージセンサ１８から全ての画像信号を読み出してバッファメモリに一時的に記憶させ、バッファメモリに記憶させた画像信号から、第１の広角画像と第１の望遠画像の２つ画像の画像信号のグループ分けを行ってもよい。
【００８９】
第１の画像取得部２２Ｌにより取得された第１の広角画像及び第１の望遠画像を示す画像信号は、それぞれデジタル信号処理部４０及び第１の被写体検出部５０Ｌに出力される。
【００９０】
第１の被写体検出部５０Ｌは、第１の画像取得部２２Ｌが取得した第１の広角画像を示す画像信号に基づいて測定対象物を検出し、検出した測定対象物の画像内における位置情報を第１のパンチルト制御部６０Ｌに出力する。
【００９１】
第１の被写体検出部５０Ｌにおける測定対象物の検出方法としては、人物の顔認識を行う技術に代表される物体認識技術により特定の物体（測定対象物）を検出する方法、又は動体を測定対象物として検出する動体検出方法がある。
【００９２】
物体認識による物体の検出方法は、特定の物体の見え方の特徴を物体辞書として予め登録しておき、撮影された画像から位置や大きさを変えながら切り出した画像と、物体辞書とを比較しながら物体を認識する方法である。
【００９３】
ロボット駆動制御部３０８は、距離情報算出部７０で算出された相対的な三次元距離情報に基づいて、ロボット３００の駆動を制御する。ここでロボット３００の駆動とは、ロボット３００の前進、後退、左右の移動、及びロボット３００が行う作業に関するロボット３００のエンドエフェクタ３０４の駆動である。
【００９４】
図６は、撮像された第１の広角画像及び第１の望遠画像の一例を示す図である。尚、第１の広角画像中の破線で示した領域は、第１の望遠画像の撮影範囲を示している。
【００９５】
いま、図７に示す第１の広角画像及び第１の望遠画像が撮像され、かつ人物の「顔」を測定対象物とし、顔認識技術により人物の「顔」を検出する場合、第１の被写体検出部５０Ｌは、第１の広角画像内の「顔」を検出することができる。尚、第１の被写体検出部５０Ｌが、第１の望遠画像から人物の「顔」を検出する場合、図６に示す第１の望遠画像には、人物の顔の一部しか入っていないため、第１の望遠画像からは「顔」を検出することができない。特に、望遠光学系１４の撮影倍率が高倍率の場合（画角が非常に小さい場合）、第１の望遠画像内に所望の測定対象物が入っていない確率が高くなる。
【００９６】
第１の被写体検出部５０Ｌにより検出された測定対象物（本例では、人物の「顔」）の、第１の広角画像内の位置情報は、第１のパンチルト制御部６０Ｌに出力される。
【００９７】
第１のパンチルト制御部６０Ｌは、第１の被写体検出部５０Ｌから入力する第１の広角画像内の測定対象物の位置情報に基づいて第１のパンチルト装置３０Ｌを制御する部分であり、測定対象物の第１の広角画像内の位置情報（例えば、測定対象物が人物の「顔」の場合には、顔領域の重心位置）が、第１の広角画像の中心位置（光軸Ｌ１上の位置）に移動するように、パン駆動部３４、チルト駆動部３６を介してパンチルト機構３２（即ち、第１の撮像部１１Ｌの撮影方向）を制御する。
【００９８】
そして、第１の撮影光学系１２Ｌの光軸Ｌ１上で測定対象物が捕捉されるように（測定対象物が第１の広角画像の中心位置に入るように）、上記のパンチルト機構３２を制御すると、第１の望遠画像の中心位置に測定対象物が入るようになる。第１の撮影光学系１２Ｌを構成する広角光学系１３及び望遠光学系１４のそれぞれの光軸Ｌ１は、一致しているからである（図３参照）。
【００９９】
図７は、特定の測定対象物（本例では、人物の「顔」）が第１の広角画像の中心にくるように、パンチルト制御された状態を示す第１の望遠画像を示すイメージ図である。
【０１００】
一方、ステレオカメラ１を構成する右目用の第２のパンチルトカメラ１０Ｒは、左目用の第１のパンチルトカメラ１０Ｌと同様に、第２の撮影光学系１２Ｒ及び第２の指向性センサ１７Ｒ（図示せず）を有する第２の撮像部１１Ｒと、第２のパンチルト装置３０Ｒとを備えている。
【０１０１】
第２の撮像部１１Ｒは、第２の撮影光学系１２Ｒ及び第２の指向性センサ１７Ｒを介して時系列の第２の広角画像及び第２の望遠画像を撮像し、第２の指向性センサ１７Ｒからは、第２の広角画像を示す画像信号と、第２の望遠画像を示す画像信号とが同時に出力可能になっている。
【０１０２】
第２の画像取得部２２Ｒは、第２の撮像部１１Ｒから第２の広角画像を示す画像信号と第２の望遠画像を示す画像信号とを同時に、又は選択的に取得する。
【０１０３】
第２の画像取得部２２Ｒにより取得された第２の広角画像及び第２の望遠画像を示す画像信号は、それぞれデジタル信号処理部４０及び第２の被写体検出部５０Ｒに出力される。
【０１０４】
第２の被写体検出部５０Ｒは、第２の画像取得部２２Ｒが取得した第２の広角画像を示す画像信号に基づいて測定対象物を検出し、検出した測定対象物の画像内における位置情報を第２のパンチルト制御部６０Ｒに出力する。
【０１０５】
第２のパンチルト制御部６０Ｒは、第２の被写体検出部５０Ｒから入力する第２の広角画像内の測定対象物の位置情報に基づいて第２のパンチルト装置３０Ｒを制御する部分であり、測定対象物の第２の広角画像内の位置情報が、第２の広角画像の中心位置（光軸Ｌ２上の位置）に移動するように、パン駆動部３４、チルト駆動部３６を介してパンチルト機構３２（即ち、第２の撮像部１１Ｒの撮影方向）を制御する。
【０１０６】
そして、第２の撮影光学系１２Ｒの光軸Ｌ２上で測定対象物が捕捉されるように（測定対象物が第２の広角画像の中心位置に入るように）、上記のパンチルト機構３２を制御すると、第２の望遠画像の中心位置に測定対象物が入るようになる。第２の撮影光学系１２Ｒを構成する広角光学系１３及び望遠光学系１４のそれぞれの光軸Ｌ２は、一致しているからである（図３参照）。
【０１０７】
デジタル信号処理部４０には、第１の画像取得部２２Ｌから第１の広角画像及び第１の望遠画像を示すデジタルの画像信号が入力し、また、第２の画像取得部２２Ｒから第２の広角画像及び第２の望遠画像を示すデジタルの画像信号が入力しており、デジタル信号処理部４０は、入力する各画像信号に対して、オフセット処理、ガンマ補正処理等の所定の信号処理を行う。
【０１０８】
また、デジタル信号処理部４０は、表示部４２が立体表示する機能を有する場合、ステレオ画像である第１の広角画像及び第２の広角画像を示す画像信号から立体広角画像表示用の画像信号を生成し、又はステレオ画像である第１の望遠画像及び第２の望遠画像の画像信号から立体望遠画像の表示用データを生成し、生成した表示用データを表示部４２に出力することで、立体広角画像又は立体望遠画像を表示部４２に表示させる。
【０１０９】
また、デジタル信号処理部４０は、表示部４２が立体表示する機能を有しない場合、例えば、第１の広角画像又は第１の望遠画像を示す画像信号から広角画像又は望遠画像の表示用データを生成し、生成した表示用データを表示部４２に出力することで、広角画像又は望遠画像を表示部４２に表示させる。
【０１１０】
更に、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２上で測定対象物が捕捉されている場合、デジタル信号処理部４０は、ステレオ画像である第１の広角画像及び第２の広角画像を示す画像信号から記録用データを生成し、又はステレオ画像である第１の望遠画像及び第２の望遠画像を示す画像信号から記録用データを生成し、生成した記録用データを記録部４４に出力することで、記録用データを記録媒体（ハードディスク、メモリカード等）に記録させる。尚、記録部４４は、第１の望遠画像及び第２の望遠画像のみを記録するようにしてもよい。また、表示部４２は、記録部４４に記録された記録用データに基づいて必要な画像を再生することもできる。
【０１１１】
＜距離情報算出部７０＞
次に、距離情報算出部７０について説明する。
【０１１２】
距離情報算出部７０は、視差量検出部７２と、距離画像算出部７４とを備えている。
【０１１３】
視差量検出部７２には、第１の画像取得部２２Ｌにより取得された第１の望遠画像を示す画像信号と、第２の画像取得部２２Ｒより取得された第２の望遠画像を示す画像信号とが加えられている。視差量検出部７２は、測定対象物が第１の望遠画像及び第２の望遠画像に捉えられている場合、好ましくは第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２上で測定対象物が捕捉されている場合（光軸Ｌ１、Ｌ２が交差し、かつ交差しているクロスポイントに測定対象物が位置している場合）に、第１の望遠画像及び第２の望遠画像を示す画像信号を入力する。
【０１１４】
視差量検出部７２は、入力した第１の望遠画像及び第２の望遠画像を示す画像信号に基づいて第１の望遠画像と第２の望遠画像との間で、特徴量が一致する対応点を検出する対応点検出部７２ａを有し、対応点検出部７２ａにより検出した２つの対応点の第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒにおける視差量を検出する。
【０１１５】
対応点検出部７２ａによる対応点の検出は、第１の望遠画像の対象画素を基準とする所定のブロックサイズの画像と第２の望遠画像とのブロックマッチングにより、相関が最も高い対象画素に対応する第２の望遠画像上の画素位置を検出することにより行うことができる。
【０１１６】
視差量検出部７２により検出された、第１の望遠画像及び第２の望遠画像の２つの対応点の第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒにおける視差量を示す情報は、距離画像算出部７４に出力される。
【０１１７】
距離画像算出部７４の他の入力には、第１のパンチルト制御部６０Ｌ及び第２のパンチルト制御部６０Ｒから第１のパンチルト機構を含む第１のパンチルト装置３０Ｌ及び第２のパンチルト機構を含む第２のパンチルト装置３０Ｒのそれぞれのパン角度及びチルト角度を示す角度情報（第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２の方向を示す情報）が入力されており、距離画像算出部７４は、これらの入力情報に基づいて対応点の距離情報を含む３次元空間情報を算出する。
【０１１８】
次に、図８を用いて測定対象物等の距離情報の算出方法について説明する。
【０１１９】
図８は、第１の撮像部１１Ｌ(第１の撮影光学系１２Ｌ)及び第２の撮像部１１Ｒ(第２の撮影光学系１２Ｒ)のパン角度及びチルト角度が制御され、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２が交差するクロスポイントＡに測定対象物が位置している場合に関して示している。尚、説明を簡単にするために、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２は水平とする。また、以下ではクロスポイントＡに測定対象物が位置している場合について説明を行うが、測定対象物は第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの望遠光学系１４に捕捉されていればステレオ画像に基づいて、測定対象物の３次元距離情報は取得することが可能である。
【０１２０】
図８に示す各記号の意味は、下記の通りである。
【０１２１】
Ａ：光軸Ｌ１、Ｌ２が交差するクロスポイント（測定対象物の位置）
Ｂ：第１の望遠画像及び第２の望遠画像における任意の対応点
α１：第１の撮影光学系１２Ｌのパン角度
α２：第２の撮影光学系１２Ｒのパン角度
Ｄ：基線長
Δｘ１：対応点Ｂの第１の指向性センサ１７Ｌにおける視差量
Δｘ２：対応点Ｂの第２の指向性センサ１７Ｒにおける視差量
δ１：対応点Ｂの光軸Ｌ１に対する水平方向の角度
δ２：対応点Ｂの光軸Ｌ２に対する水平方向の角度
ｆ：第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒに含まれる望遠光学系１４の焦点距離
図８において、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒの各位置と、クロスポイントＡ（測定対象物）とからなる三角形を考えると、三角形の底辺の長さは、基線長Ｄであり、既知である。
【０１２２】
三角形の底角をそれぞれβ１、β２とすると、底角β１、β２は、それぞれパン角度α１及びパン角度α２に基づいて算出することができる。
【０１２３】
従って、既知の底辺（基線長Ｄ）と２つの底角β１、β２から、測定点である三角形の頂点のクロスポイントＡ（測定対象物）の距離情報を算出することができる。なお、この場合のロボット３００の基準部位は基線長Ｄの中間点であり、距離情報とは測定対象物と基準部位との距離である。また基準部位は、特に限定されるものではなく、予め距離が既知な部位であればどこでもよい。
【０１２４】
一方、対応点Ｂの光軸Ｌ１に対する水平方向の角度δ１及び光軸Ｌ２に対する水平方向の角度δ２は、対応点Ｂの第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒにおける視差量Δｘ１、Δｘ２と、望遠光学系１４の焦点距離ｆとに基づいて、次式により算出することができる。
【０１２５】
［数１］
δ１＝arctan(Δｘ１/ｆ）
δ２＝arctan(Δｘ２/ｆ）
対応点Ｂの第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒにおける視差量Δｘ１、Δｘ２は、第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒの中心位置(光軸Ｌ１、Ｌ２に対応する位置)からの、対応点Ｂが結像した画素の画素位置をｎ１，ｎ２、第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒの画素ピッチをｐとすると、次式により算出することができる。
【０１２６】
［数２］
Δｘ１＝ｎ１×ｐ
Δｘ２＝ｎ２×ｐ
第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒの画素ピッチｐは既知であるため、視差量Δｘ１、Δｘ２は、対応点Ｂの第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒ上の位置を検出することで、［数２］式により算出することができる。
【０１２７】
図８において、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒの各位置と、任意の対応点Ｂとからなる三角形を考えると、上記と同様に三角形の底辺の長さ（基線長Ｄ）と、２つの底角γ１、γ２とにより、三角形の頂点である対応点Ｂの距離情報を算出することができる。
【０１２８】
尚、２つの底角γ１、γ２は、それぞれパン角度α１、α２と、［数１］式により算出される角度δ１、角度δ２とにより求めることができる。
【０１２９】
図５に戻って、距離画像算出部７４は、第１の望遠画像と第２の望遠画像の対応点毎に距離情報を算出することより、距離画像を算出することができる。
【０１３０】
ここで、距離画像とは、ステレオカメラ１によって得られる被写体までの距離値（距離情報）の２次元分布画像のことであり、距離画像の各画素は距離情報を有する。また、クロスポイントＡ（測定対象物）の距離情報は、視差量Δｘ１、Δｘ２が０となる特異点の距離情報である。
【０１３１】
距離画像算出部７４により算出された距離画像は、距離画像記録部７６に記録される。例えば、構造物を被写体とする場合、その構造物の三次元情報を取得することができ、取得した三次元情報を距離画像記録部７６に記録することで、三次元情報を適宜活用することができる。
【０１３２】
＜第１の焦点調節部＞
次に、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒに含まれる望遠光学系１４の焦点調節を自動的に行う第１の焦点調節部について説明する。
【０１３３】
図９は、第１の焦点調節部を示すブロック図である。
【０１３４】
図９に示す第１の焦点調節部は、第１の撮像部１１Ｌ及び第２の撮像部１１Ｒにそれぞれ設けられ、望遠光学系１４の全体又は一部の光学系を光軸方向に移動させる焦点調節機構１９と、焦点調節機構１９を制御する焦点制御部１９０とから構成されている。
【０１３５】
焦点制御部１９０は、視差量検出部１９２、距離情報算出部１９４、及び合焦位置算出部１９６等を備えている。
【０１３６】
視差量検出部１９２には、第１の被写体検出部５０Ｌ及び第２の被写体検出部５０Ｒからそれぞれ第１の広角画像内及び第２の広角画像内における測定対象物の位置情報が入力しており、視差量検出部１９２は、入力した測定対象物の位置情報に基づいて、第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒにおける測定対象物の視差量を検出する。
【０１３７】
尚、前述したように第１の被写体検出部５０Ｌ及び第２の被写体検出部５０Ｒにより検出された測定対象物の第１の広角画像内及び第２の広角画像内の位置情報は、第１のパンチルト制御部６０Ｌ及び第２のパンチルト制御部６０Ｒにおいて、第１のパンチルト装置３０Ｌ及び第２のパンチルト装置３０Ｒを制御するために使用されるが、焦点制御部１９０では、視差量検出のために使用される。また、本例の焦点制御部１９０は、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２上で測定対象物が捕捉される前であっても、望遠光学系１４の焦点調節を行うが、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２上で測定対象物が捕捉されている状態で望遠光学系１４の焦点調節を行う場合には、視差量検出部１９２は不要になる。第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２上で測定対象物が捕捉されている場合、視差量は０だからである。
【０１３８】
視差量検出部１９２により検出された視差量は、距離情報算出部１９４に出力される。距離情報算出部１９４の他の入力には、第１のパンチルト制御部６０Ｌから第１のパンチルト装置３０Ｌのパン角度及びチルト角度を示す角度情報（第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２の方向を示す情報）が入力されており、距離情報算出部１９４は、これらの入力情報に基づいて測定対象物の距離情報を算出する。尚、距離情報算出部１９４により測定対象物の距離情報の算出は、図５に示した距離情報算出部７０による対応点の距離情報の算出と同様に行われるが、前述した［数１］式に示した視差量Δｘ１、Δｘ２は、第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒにおける第１の広角画像及び第２の広角画像における測定対象物の視差量であり、かつ焦点距離ｆは、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒに含まれる広角光学系１３の焦点距離である点で相違する。
【０１３９】
距離情報算出部１９４により算出された測定対象物の距離情報は、合焦位置算出部１９６に出力される。合焦位置算出部１９６は、距離情報算出部１９４から入力する測定対象物の距離情報に基づいて、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの望遠光学系１４の全体又は一部の光学系の光軸方向の移動位置であって、測定対象物に合焦する合焦位置を算出する。尚、望遠光学系１４の全体又は一部の光学系の光軸方向の移動位置と、その移動位置により合焦する被写体の距離情報とは一対一に対応するため、合焦位置算出部１９６は、測定対象物の距離情報が取得できれば、その測定対象物に合焦する合焦位置を算出することができる。
【０１４０】
焦点制御部１９０は、合焦位置算出部１９６により算出された合焦位置に基づいて焦点調節機構１９を制御し、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの望遠光学系１４の全体又は一部の光学系を光軸方向の合焦位置に移動させ、望遠光学系１４の焦点調節（第１の焦点調節）を行う。
【０１４１】
尚、本例の焦点制御部１９０は、第１の広角画像及び第２の広角画像にそれぞれ含まれる測定対象物の第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒにおける測定対象物の視差量等に基づいて測定対象物の距離情報を取得し、取得した距離情報に基づいて第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの望遠光学系１４の焦点調節を行うようにしたが、これに限らず、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの広角光学系１３の焦点調節を行うコントラストＡＦ、像面位相差ＡＦ等の焦点調節部（第２の焦点調節部）が設けられている場合、第２の焦点調節部による広角光学系１３の合焦情報（例えば、合焦状態にある広角光学系１３の全体又は一部の光学系の光軸方向の移動位置）に基づいて測定対象物の距離情報を取得し、取得した距離情報に基づいて第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの望遠光学系１４の焦点調節を行うようにしてもよい。
【０１４２】
図１０はパンチルト制御部６０のブロック図である。このパンチルト制御部６０は、図５に示した第１のパンチルト制御部６０Ｌ及び第２のパンチルト制御部６０Ｒに加えて、図２に示したパン装置３８を制御するパン制御部６２を備えている。
【０１４３】
パン制御部６２は、第１の広角画像及び第２の広角画像に基づいてパン機構を含むパン装置３８を制御し、第１の撮像部１１Ｌと第２の撮像部１１Ｒとを測定対象物に正対させる。
【０１４４】
第１のパンチルト制御部６０Ｌ及び第２のパンチルト制御部６０Ｒは、それぞれ第１の被写体検出部５０Ｌ及び第２の被写体検出部５０Ｒが検出した測定対象物の、第１の広角画像内及び第２の広角画像内の測定対象物の位置情報に基づいて第１のパンチルト装置３０Ｌ及び第２のパンチルト装置３０Ｒを制御するが、パン制御部６２は、第１のパンチルト制御部６０Ｌ及び第２のパンチルト制御部６０Ｒから第１のパンチルト装置３０Ｌ及び第２のパンチルト装置３０Ｒのそれぞれのパン角度α１及びパン角度α２（図８参照）を示す角度情報を取得する。
【０１４５】
図８において、パン角度α１及びパン角度α２は、時計回り方向を正とすると、パン制御部６２は、パン角度α１及びパン角度α２を示す角度情報に基づいて、パン角度α１が正、パン角度α２が負になり、かつパン角度α１とパン角度α２の絶対値が等しくなるように、パン装置３８（第１のパンチルト装置３０Ｌ及びと第２のパンチルト装置３０Ｒを保持する保持部材３７）を水平方向に回動させる。即ち、パン制御部６２によりパン装置３８が回動させられると、第１のパンチルト装置３０Ｌ及び第２のパンチルト装置３０Ｒは、それぞれ測定対象物を望遠画像内で捕捉すべく、パン方向に駆動制御されるため、パン装置３８の回動位置を制御することにより、パン角度α１が正、パン角度α２が負になり、かつパン角度α１とパン角度α２の絶対値を等しくすることができる。なお、第１のパンチルト装置３０Ｌ及び第２のパンチルト装置３０Ｒは、それぞれ測定対象物を光軸Ｌ１、Ｌ２上で捕捉すべくパン方向に駆動制御されてもよい。
【０１４６】
このように第１のパンチルト装置３０Ｌ及びと第２のパンチルト装置３０Ｒを保持する保持部材３７（ステレオカメラ１全体）を水平方向に回動させることにより、測定対象物に対して第１の撮像部１１Ｌと第２の撮像部１１Ｒとの距離を等距離にすることができ、測定対象物等の距離情報をより精度良く算出することができる。
【０１４７】
＜測定対象物の動きの予測＞
次に、ロボットシステム５００における測定対象物の動きの予測について説明する。
【０１４８】
図１１は、測定対象物の動きを予測して追尾する場合のロボットシステム５００におけるブロック図である。図１１には、図５で示した第１の画像取得部２２Ｌ、第２の画像取得部２２Ｒ、第１の被写体検出部５０Ｌ、第２の被写体検出部５０Ｒ、ロボット駆動制御部３０８、図１０で説明をしたパンチルト制御部６０の他に、予測部３１０が記載されている。
【０１４９】
予測部３１０は、画像取得部２２が取得した第１の広角画像及び／又は第２の広角画像に基づいて、測定対象物の動きを予測する。予測部３１０が行う測定対象物の動きの予測は、公知の技術によって行われる。例えば第１の画像取得部２２Ｌから出力される連続する第１の広角画像において被写体検出部（第１の被写体検出部５０Ｌ及び／または第２の被写体検出部５０Ｒ）により測定対象物を検出し、その検出された測定対象物を連続する画像においてトラッキングすることにより、測定対象物の動きを予測する。尚、予測部３１０が行う測定対象物の動きの予測は、第２の広角画像に基づいて行われてもよいし、第１の広角画像及び第２の広角画像で行われてもよい。
【０１５０】
予測部３１０は、予測結果をパンチルト制御部６０及び／又はロボット駆動制御部３０８に送信する。
【０１５１】
パンチルト制御部６０は、予測部３１０の予測結果に応じてパンチルト機構を制御する。またロボット駆動制御部３０８は、予測部３１０の予測結果に応じてロボット３００の動きを制御する。なお、測定対象物の追尾は、第１の望遠画像および第２の望遠画像に測定対象物が距離情報を取得できる範囲に写っていれば行われる必要がない。すなわち、測定対象物の移動に合わせ必ずパンチルト機構の制御が行われる必要はなく、第１の望遠画像及び第２の望遠画像に測定対象物が距離情報を取得できる範囲に写っていればよいパンチルト機構の制御を行わなくてもよい。
【０１５２】
＜ステレオカメラの制御方法＞
次に、上述した追尾を行う場合のステレオカメラ１の制御方法について説明する。
【０１５３】
図１２は、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２上で測定対象物を追尾して捕捉させる場合のパンチルト制御を示すフローチャートである。
【０１５４】
図１２に示すように、第１の撮像部１１Ｌ及び第１の画像取得部２２Ｌを介して第１の広角画像を取得するとともに、第２の撮像部１１Ｒ及び第２の画像取得部２２Ｒを介して第２の広角画像を取得する（ステップＳ１０）。
【０１５５】
続いて、第１の被写体検出部５０Ｌ及び第２の被写体検出部５０Ｒにより、取得した第１の広角画像内及び第２の広角画像内の測定対象物を検出し（ステップＳ１２）、検出した測定対象物の第１の広角画像内及び第２の広角画像内のそれぞれの位置情報（第１、第２の位置情報）を算出する（ステップＳ１４）。
【０１５６】
ステップＳ１４で算出した測定対象物の第１の広角画像内及び第２の広角画像内のそれぞれの第１の位置情報及び第２の位置情報に基づいて第１のパンチルト装置３０Ｌ及び第２のパンチルト装置３０Ｒを制御する（ステップＳ１６）。
【０１５７】
次に、測定対象物の追尾が終了したか否かを判別する（ステップＳ１８）。即ち、測定対象物が移動し、ステレオ画像である第１の広角画像と第２の広角画像、及びステレオ画像である第１の望遠画像と第２の望遠画像を動画として取得する場合には、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２上で常に測定対象物を捕捉（測定対象物を追尾）する必要がある。
【０１５８】
従って、測定対象物の自動追尾が終了したか否かを判別し（ステップＳ１８）、終了していないと判別すると、ステップＳ１０に遷移させる。これにより、上記ステップＳ１０からステップＳ１８の処理が繰り返され、測定対象物を自動追尾した撮像が行われる。
【０１５９】
一方、測定対象物の追尾が終了したと判別されると、測定対象物を追尾させるパンチルト制御を終了させる。尚、測定対象物の自動追尾が終了したか否かの判別は、電源のオン／オフにより行ってもよいし、パンチルト動作を終了させるスイッチ入力等により行ってもよい。また、測定対象物を静止画として撮影する場合、静止画の撮影の終了時点を測定対象物の自動追尾の終了とすることができる。
【０１６０】
図１３は、第１の望遠画像と第２の望遠画像との間で特徴量が一致する対応点の距離情報を取得し、ロボット３００を駆動させる処理を示すフローチャートである。
【０１６１】
先ず、パンチルト制御部６０はパンチルト機構を制御して、画像取得部２２により取得した第１の広角画像及び第２の広角画像に基づいて、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒの望遠光学系１４で測定対象物をそれぞれ捕捉させる（ステップＳ２０：パンチルト制御ステップ）。その後、第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２上で測定対象物が捕捉されている場合に、第１の撮像部１１Ｌ及び第１の画像取得部２２Ｌを介して第１の望遠画像を取得するとともに、第２の撮像部１１Ｒ及び第２の画像取得部２２Ｒを介して第２の望遠画像を取得する（ステップＳ２２）。
【０１６２】
続いて、視差量検出部７２の対応点検出部７２ａにより、取得した第１の望遠画像及び第２の望遠画像との間で、特徴量が一致する対応点を検出し（ステップＳ２４）、検出した２つの対応点の第１の指向性センサ１７Ｌ及び第２の指向性センサ１７Ｒにおける視差量を検出する（ステップＳ２６）。
【０１６３】
次に、距離画像算出部７４は、図８で説明したようにステップＳ２６により算出された対応点の視差量と、第１のパンチルト装置３０Ｌ及び第２のパンチルト装置３０Ｒのそれぞれのパン角度及びチルト角度を示す角度情報（第１の撮影光学系１２Ｌ及び第２の撮影光学系１２Ｒのそれぞれの光軸Ｌ１、Ｌ２の方向を示す情報）と、第１のパンチルトカメラ１０Ｌと第２のパンチルトカメラ１０Ｒとの基線長とに基づいて対応点の距離情報を算出する（ステップＳ２８：距離情報算出ステップ）。
【０１６４】
尚、距離画像算出部７４は、第１の望遠画像と第２の望遠画像の対応点毎に距離情報を算出することより、距離画像を算出することができる。また、測定対象物の光軸Ｌ１、Ｌ２上の点も対応点であるため、測定対象物の距離情報も算出されるが、この場合の光軸Ｌ１、Ｌ２上の点の視差量は０である。
【０１６５】
ステップＳ２８により算出された距離情報（距離画像）は、ロボット駆動制御部３０８、又は距離画像記録部７６に出力されて記録され、あるいは外部機器に出力される（ステップＳ３０）。
【０１６６】
次にロボット駆動制御部３０８は、距離情報算出部７０で出力された距離情報に基づいてロボット３００の駆動を制御する（ステップＳ３２：ロボット駆動制御ステップ）。
【０１６７】
上述の各構成及び機能は、任意のハードウェア、ソフトウェア、或いは両者の組み合わせによって適宜実現可能である。例えば、上述の処理ステップ（処理手順）をコンピュータに実行させるプログラム、そのようなプログラムを記録したコンピュータ読み取り可能な記録媒体（非一時的記録媒体）、或いはそのようなプログラムをインストール可能なコンピュータに対しても本発明を適用することが可能である。
【０１６８】
＜動体検出方法＞
次に、測定対象物が動体の場合、その測定対象物（動体）を検出する動体検出方法の一例について、図１４から図１６を用いて説明する。尚、第１の被写体検出部５０Ｌ及び第２の被写体検出部５０Ｒは、それぞれ動体検出部として同様の機能を持つため、動体検出部として機能し得る第１の被写体検出部５０Ｌについてのみ説明する。
【０１６９】
動体検出部として機能する場合の第１の被写体検出部５０Ｌは、図１４及び図１５に示すように時系列の２枚の第１の広角画像（前回取得した第１の広角画像（図１４）と、今回取得した第１の広角画像（図１５））とを取得し、時系列の２枚の第１の広角画像の差分をとった差分画像（図１６）を検出する。
【０１７０】
図１４及び図１５に示す例では、物体Ａ，Ｂのうちの物体Ａが移動し、物体Ｂは停止しており、物体Ａが測定対象物（動体）である。
【０１７１】
従って、図１６に示すように差分画像Ａ１，Ａ２は、物体Ａの移動により生じた画像である。
【０１７２】
ここで、差分画像Ａ１，Ａ２の重心位置を算出し、それぞれ位置Ｐ１，Ｐ２とし、これらの位置Ｐ１，Ｐ２を結ぶ線分の中点を、位置Ｇとする。そして、この位置Ｇを物体Ａ（動体である測定対象物）の第１の広角画像内の位置とする。
【０１７３】
このようして算出した第１の広角画像内の物体Ａの位置Ｇが、第１の広角画像の中心位置（光軸Ｌ１上の位置）に移動するように、パンチルト機構３２（即ち、第１の撮像部１１Ｌによる撮影方向）を繰り返し制御することにより、物体Ａが第１の広角画像（第１の望遠画像）の中心位置に移動（収束）する。
【０１７４】
尚、第１の撮像部１１Ｌが移動（パンチルト機構３２により移動、又はステレオカメラ１を車載することにより移動）する場合、時系列の画像間の背景も移動することになるが、この場合には、時系列の画像間の背景が一致するように画像をシフトさせ、シフト後の画像間の差分画像をとることにより、第１の撮像部１１Ｌの移動にかかわらず、実空間内を移動する物体Ａを検出することができる。更に、動体検出方法は、上記の実施形態に限らない。
【０１７５】
＜測定対象物とエンドエフェクタの捕捉＞
次に、ロボットシステム５００のステレオカメラ１による測定対象物とエンドエフェクタ３０４の捕捉の具体例に関して説明する。
【０１７６】
図１７は、図６と同様に撮像された第１の広角画像及び第１の望遠画像の一例を示す図である。図１７では、第１の撮影光学系１２Ｌの広角光学系１３で取得された第１の広角画像４０１と第１の撮影光学系１２Ｌの望遠光学系１４で取得された第１の望遠画像４０２が示されている。第１の広角画像４０１及び第１の望遠画像４０２には、ロボット３００のエンドエフェクタ３０４と測定対象物であるコップ３２２が捉えられている。特に第１の望遠画像４０２では、ロボット３００のエンドエフェクタ３０４の指部３３０Ａ〜３３０Ｃ、及びコップ３２２の把手がズームアップされた状態で捉えられている。
【０１７７】
パンチルト制御部６０は、パンチルト機構を駆動させて第１の撮影光学系１２Ｌ又は第２の撮影光学系１２Ｒの望遠光学系１４により、測定対象物の一部を捕捉させる。この場合、ステレオカメラ１は広角光学系１３と望遠光学系１４とは光軸が同一であるので、望遠光学系１４で捉えられた測定対象物は広角光学系１３においても捉えられる。
すなわち、図１７に示すように、第１の望遠画像４０２においてロボット３００のエンドエフェクタ３０４の指部３３０Ａ〜３３０Ｃとコップ３２２の把手とを捉える。このように、ロボット３００の指部３３０Ａ〜３３０Ｃとコップ３２２の一部を望遠画像４０２で捉えることによって、ロボット３００の指部３３０Ａ〜３３０Ｃとコップ３２２との距離情報を正確に算出することができる。またパンチルト制御部６０は、第１の撮影光学系１２Ｌ又は第２の撮影光学系１２Ｒの望遠光学系１４により測定対象物とロボット３００の特定部位とを捕捉させてもよい。ここで特定部位とは、エンドエフェクタ３０４ではないが距離が既知な部位であって、測定対象物と特定部位との距離がわかることにより、測定対象物とロボット３００、又は測定対象物とエンドエフェクタ３０４との距離がわかる部位である。
【０１７８】
次に、ロボットシステム５００を構成するロボット３００の変形例に関して説明する。
【０１７９】
＜ロボットの変形例１＞
図１８は、変形例１のロボットを示す概念図である。本例ではロボットシステム５００を構成するロボット３００として、自走型ロボット３０１が採用されている。例えば自走型ロボット３０１は、自動操作又は遠隔操作によって、走行し建物の内部の撮影を行う作業を行う。また自走型ロボット３０１は、自己位置の推定を行いつつ走行する場合がある。以下にロボットシステム５００で行われる自己位置推定に関して説明する。また、自走型ロボット３０１では例えば、ステレオカメラ１で取得した距離情報に基づいて車輪である駆動部３０６が制御される。
【０１８０】
＜自己位置検出＞
図１９は自走型ロボット３０１が自己位置推定を行いながら自走している場合を示す概念図であり、図２０は、本発明のロボットシステム５００が備える自己位置推定における機能ブロック図である。
【０１８１】
図１９に示すように、自走型ロボット３０１は自己位置推定を行いつつ走行する場合には、例えば窓Ｗ１〜Ｗ４を測定対象物として測定しつつ自己位置推定を行いながら走行する。
【０１８２】
図２０には、図５で説明を行った第１の画像取得部２２Ｌ、第２の画像取得部２２Ｒ、距離情報算出部７０が示されており、さらに位置検出部３１６、エリア情報取得部３１８、及び自己位置推定部３２０が示されている。
【０１８３】
エリア情報取得部３１８は、自走型ロボット３０１が移動するエリアのエリア情報を取得する。エリア情報取得部３１８は、例えば自走型ロボット３０１が移動するエリアのＣＡＤ情報３２１を取得する。具体的には、自走型ロボット３０１がビル等の建物の内部を移動する場合には、その建物のＣＡＤ情報３２１を取得する。尚、エリア情報取得部３１８に入力されるＣＡＤ情報３２１は、ＣＡＤ情報３２１に限定されるものではなく、ロボットシステム５００が自己位置を推定することができるものであれば、特に限定されるものではない。
【０１８４】
位置検出部３１６は、第１の広角画像又は第２の広角画像に基づいて、エリア情報取得部３１８で取得したエリア情報における測定対象物の位置を検出する。例えば位置検出部３１６は、測定対象物である窓Ｗ１が第１の広角画像において検出された場合に、ＣＡＤ情報３２１における窓Ｗ１の位置を検出する。ここで位置検出部３１６が行うＣＡＤ情報３２１における窓Ｗ１の検出は公知の技術が使用される。尚、測定対象物はロボットシステム５００のユーザが指定をしてもよいし、自動に測定対象物を認識してもよい。
【０１８５】
自己位置推定部３２０は、距離情報算出部７０で算出された相対的な三次元距離情報及び位置検出部３１６の検出結果により、自走型ロボット３０１の自己位置を推定する。すなわち自己位置推定部３２０は、距離情報算出部７０から測定対象物と自走型ロボット３０１の特定部位との相対的な三次元距離情報を取得し、その３次元情報と位置検出部３１６での検出結果に基づいて、エリア情報における自走型ロボット３０１の位置を推定する。具体的には自己位置推定部３２０は、位置検出部３１６の検出結果である窓Ｗ１のＣＡＤ情報３２１における位置を取得し、距離情報算出部７０の窓Ｗ１とロボット３００の特定部位との相対的な３次元距離情報を取得することによって、ロボット３００のＣＡＤ情報３２１における位置を推定する。
【０１８６】
＜ロボットの変形例２＞
図２１は、変形例２のロボットを示す概念図である。本例ではロボットシステム５００を構成するロボットとして、アーム型ロボット３０３が採用されている。例えばアーム型ロボット３０３は装置の組立作業を行う。この場合、測定対象物は組立作業の対象の装置である。またアーム型ロボット３０３では例えば、ステレオカメラ１で取得した距離情報に基づいてアームの駆動部３０６が駆動する。
【０１８７】
アーム型ロボット３０３には、ステレオカメラ１が設けられており、特にエンドエフェクタ３０４が撮影し易い位置にステレオカメラ１は設けられる。例えば、ステレオカメラ１はエンドエフェクタ３０４が接続されている先端部３０５に設けられることにより、測定対象物とエンドエフェクタ３０４とを正確に撮影することができる。
【０１８８】
アーム型ロボット３０３は、ステレオカメラ１の撮影により得られた距離情報に基づいて駆動部３０６及びエンドエフェクタ３０４を駆動させる。この場合、精度のよい距離情報に基づき、駆動部３０６及びエンドエフェクタ３０４は駆動制御されるので、より正確に組立作業を行うことができる。
【０１８９】
＜ロボットの変形例３＞
図２２は、変形例３のロボットを示す概念図である。本例ではロボットシステム５００を構成するロボットとして、図２１で説明したアーム型ロボット３０３が採用されている。本例の場合には、ステレオカメラ１はロボット３０３とは別体に設けられている。このように、別体でステレオカメラ１を設けることによって、エンドエフェクタ３０４と測定対象物とを正確に捕捉して撮影することが可能となる。
【０１９０】
以上で本発明の例に関して説明してきたが、本発明は上述した実施の形態に限定されず、本発明の精神を逸脱しない範囲で種々の変形が可能であることは言うまでもない。
【符号の説明】
【０１９１】
１  ステレオカメラ
２Ｌ  第１のカメラ本体
２Ｒ  第２のカメラ本体
４Ｌ、４Ｒ  台座
８Ａ  ギア
８Ｌ、８Ｒ  保持部
１０Ｌ  第１のパンチルトカメラ
１０Ｒ  第２のパンチルトカメラ
１１Ｌ  第１の撮像部
１１Ｒ  第２の撮像部
１２Ｌ  第１の撮影光学系
１２Ｒ  第２の撮影光学系
１３  広角光学系
１３ａ  第１レンズ
１３ｂ  第２レンズ
１３ｃ  第３レンズ
１３ｄ  第４レンズ
１４  望遠光学系
１４ａ  第１レンズ
１４ｂ  第２レンズ
１４ｃ  第１反射ミラー
１４ｄ  第２反射ミラー
１５  共通レンズ
１６  マイクロレンズアレイ
１６ａ  マイクロレンズ
１７  指向性センサ
１７Ｌ  第１の指向性センサ
１７Ｒ  第２の指向性センサ
１７ａ  中央瞳像
１７ｂ  環状瞳像
１８  イメージセンサ
１８ａ  受光セル
１９  焦点調節機構
２０  Ａ／Ｄ変換器
２２  画像取得部
２２Ｌ  第１の画像取得部
２２Ｒ  第２の画像取得部
２４ｐ  フレーム数
３０Ｌ  第１のパンチルト装置
３０Ｒ  第２のパンチルト装置
３０ｐ  フレーム数
３２  パンチルト機構
３４  パン駆動部
３６  チルト駆動部
３７  保持部材
３８  パン装置
４０  デジタル信号処理部
４２  表示部
４４  記録部
５０Ｌ  第１の被写体検出部
５０Ｒ  第２の被写体検出部
６０  パンチルト制御部
６０Ｌ  第１のパンチルト制御部
６０Ｒ  第２のパンチルト制御部
６２  パン制御部
７０  距離情報算出部
７２  視差量検出部
７２ａ  対応点検出部
７４  距離画像算出部
７６  距離画像記録部
１９０  焦点制御部
１９２  視差量検出部
１９４  距離情報算出部
１９６  合焦位置算出部
３００  ロボット
３０１  自走型ロボット
３０２  頭部
３０３  アーム型ロボット
３０４  エンドエフェクタ
３０５  先端部
３０６  駆動部
３０８  ロボット駆動制御部
３１０  予測部
３１６  位置検出部
３１８  エリア情報取得部
３２０  自己位置推定部
３２１  ＣＡＤ情報
３２２  コップ
３３０Ａ〜Ｃ  指部
５００  ロボットシステム
Ｄ  基線長
Ｌ１  光軸
Ｌ２  光軸
Ｓ１０〜Ｓ２８  ステップ
ｆ  焦点距離
Δｘ１、Δｘ２  視差量
α１、α２  パン角度
β１、β２、γ１、γ２  底角
【図１】
2018134712.tif 000003
【図２】
2018134712.tif 000004
【図３】
2018134712.tif 000005
【図４】
2018134712.tif 000006
【図５】
2018134712.tif 000007
【図６】
2018134712.tif 000008
【図７】
2018134712.tif 000009
【図８】
2018134712.tif 000010
【図９】
2018134712.tif 000011
【図１０】
2018134712.tif 000012
【図１１】
2018134712.tif 000013
【図１２】
2018134712.tif 000014
【図１３】
2018134712.tif 000015
【図１４】
2018134712.tif 000016
【図１５】
2018134712.tif 000017
【図１６】
2018134712.tif 000018
【図１７】
2018134712.tif 000019
【図１８】
2018134712.tif 000020
【図１９】
2018134712.tif 000021
【図２０】
2018134712.tif 000022
【図２１】
2018134712.tif 000023
【図２２】
2018134712.tif 000024