JP5011528B2 - ３次元距離計測センサおよび３次元距離計測方法 - Google Patents

Description

本発明は、測定点の３次元距離を計測する３次元距離計測センサおよび３次元距離計測方法に関するものである。

移動ロボットや車両等の移動体をナビゲーションするためには、移動体周囲３６０度（全天周）に位置する物体に対する詳細な３次元位置情報が必要となる。このような３次元位置情報の取得方法として、ステレオ視技術が知られている。

ステレオ視技術は、撮影範囲が重複するように複数の異なる視点に配置されたカメラによって撮影を行い、得られた画像間で重複範囲の対応点を検出して（マッチング処理）、対応点に対する視差からカメラと対応点との３次元距離を計測する技術である。特に、ステレオ視技術の中でも、異なる３つの視点の画像を用いたステレオ視（３眼ステレオ視）は、２眼ステレオ視に比べてより正確なマッチング処理を行う技術として、有用視されている。

このような３眼ステレオ視を用いた技術として、特許文献１には、直角に配置された平面画像面の２組のエピポーラ線に基づいて画像のエッジの深度をより正確に決定する３眼ステレオ視方式が示されている。
Y. Ohta, M. Watanabe and K. Ikeda, "Improving Depth map byrightangled Trinocular stereo", Proc. of ICPR, 1986, pp.519-521.

上記の３眼ステレオ視方式はいわゆるピンホールカメラモデルなどで取得される平面画像上によるステレオ視処理であるため、一度に取得可能な視野が狭くナビゲーションや周辺監視などに求められる広範囲な視野内の3次元距離をより早く且つ正確に取得することが困難であった。

本発明は、上記した事情に鑑みて為されたものであり、より広い範囲内の測定点の３次元距離を早く且つより正確に測定可能な３次元距離計測センサおよび３次元距離計測方法を提供することを課題とする。

本発明に係る３次元距離計測センサは、測定点までの３次元距離を計測する３次元距離計測センサであって、魚眼レンズ特性を有すると共に、第１の視点から測定点を撮像する第１のカメラ部と、魚眼レンズ特性を有すると共に、第１の視点と異なる第２の視点から測定点を撮像する第２のカメラ部と、魚眼レンズ特性を有すると共に、第１および第２の視点と異なる第３の視点から測定点を撮像する第３のカメラ部と、第１のカメラ部、第２のカメラ部および第３のカメラ部の各々によって取得され出力される第１の平面画像、第２の平面画像および第３の平面画像から第１の球面画像、第２の球面画像および第３の球面画像をそれぞれ形成する球面画像形成手段と、第１および第２の球面画像を対応付けて第１の視差を算出すると共に、第１および第３の球面画像を対応付けて第２の視差を算出する視差算出手段と、第１の視点から測定点までの３次元距離を算出する距離算出手段と、を備え、第１および第２の視点を通る直線と第１および第３の視点を通る直線とは、交差しており、距離算出手段は、第１および第２の視差の各々に基づいて第１の視点から測定点までの第１および第２の距離パラメータを算出し、第１および第２の距離パラメータを統合することによって３次元距離を算出することを特徴とする。

本発明の３次元距離計測センサによれば、魚眼レンズ特性を有する第１〜第３のカメラ部を利用しているため、第１〜第３のカメラ部で取得された第１〜第３の平面画像には、第１〜第３のカメラ部の周囲のより広い範囲内（視野内）の情景が含まれることになる。この情景内の任意の点が測定点であるため、情景は測定点の集合とみなすことができる。そして、３次元距離計測センサでは、取得された第１〜第３の平面画像から第１〜第３の球面画像が形成された後、第１の視差が、第１および第２の球面画像を対応付けて算出され、第２の視差が、第１および第３の球面画像を対応付けて算出される。そして、測定点までの３次元距離は、第１および第２の視差にもとづく第１および第２の距離パラメータを統合して算出される。よって、一度の撮影によって広範囲の測定点に対する３次元距離を算出可能である。また、例えば、測定点が第１および第２の視点を結ぶ直線上にある場合（球面画像上ではエピポール付近に測定点の対応点が位置する場合）、第１の視差がほとんど得られないおそれがあるが、第１、第２および第３の視点は同一直線上にないので、第２の視差を確実に得ることができる。したがって、第１〜第３のカメラ部の撮像範囲の測定点に対しては、第１または第２の視差をより確実に得られるので、広範囲の測定点に対する３次元距離計測を行うことができる。また、第１及び第２の視点を通る直線上付近の測定点に対して第１及び第２の球面画像からのみ３次元距離を算出しようとすると、第１の視差の計測誤差が大きくなるが、その場合、第２の視差の計測誤差が小さくなるため、第１及び第２の視差に基づく第１及び第２の距離パラメータを統合することによって、より正確な３次元距離を算出可能である。

ここで、上記の距離算出手段は、第１および第２の距離パラメータの信頼度に応じて第１および第２の距離パラメータを重み付ける第１および第２の統合パラメータを算出する統合パラメータ算出部を有することが好ましい。

上記の距離算出手段によれば、第１および第２の距離パラメータは、その計測における信頼度に応じて重み付けされて統合されるので、計測誤差を含んで信頼度の低い距離パラメータの影響を小さくすることができる。

上記の距離算出手段は、第１および第２の視差の各々に基づいて算出される第１の視点から測定点までの第１および第２の推定距離の逆数として第１および第２の距離パラメータを算出する距離パラメータ算出部を有することが好ましい。

上記の距離算出手段によれば、第１および第２の距離パラメータは、第１および第２の視差の各々に基づいて算出される第１の視点から測定点までの３次元距離の逆数であらわされるので、測定点が非常に遠方に位置する場合においても、距離パラメータがオーバーフローすることなく算出される。

上記距離算出手段としては、第１の距離パラメータをα_１２、第２の距離パラメータをα_１３、第１の視差をｄ_Ｓ２、第２の視差をｄ_Ｓ３、ｄ_Ｓ２によるα_１２の微分をα’_１２、ｄ_Ｓ３によるα_１３の微分をα’_１３、第１の距離パラメータの信頼度に応じて第１の距離パラメータα_１２を重み付ける第１の統合パラメータをｗ_１２、第２の距離パラメータの信頼度に応じて第２の距離パラメータα_１３を重み付ける統合パラメータｗ_１３とするとき、３次元距離ρ_１を以下の式（１）によって算出することが好ましい。

ただし、

上記の距離算出手段によれば、３次元距離ρ_１が（１）式〜（３）式によって算出される。例えば、第１の距離パラメータの変化に対して第１の視差の変化が０に近い場合、すなわち第１の視差による計測で第１の距離パラメータが大きな誤差を含む可能性がある場合には、（２）式および（３）式における１／｜α´_１２｜が０に近くなり、第１の統合パラメータに比べて、第２の統合パラメータが大きくなる。逆に、第２の距離パラメータの変化に対して第２の視差の変化が０に近い場合、すなわち第２の推定距離が誤差を含みやすい場合には、（１）式および（２）式における１／｜α´_１３｜が０に近くなり、第２の統合パラメータに比べて、第１の統合パラメータが大きくなる。従って、第１および第２の距離パラメータに含まれる計測誤差の度合いに応じて、第１および第２の統合パラメータが増減されるので、距離パラメータに適度な重み付けがされて測定点の３次元距離が算出される。そのため、広範囲の測定点に対して適切に３次元距離を測定することが可能である。

また、本発明に係る３次元計測センサにおいて、第１〜第３のカメラ部の配置としては、第１および第２の視点を通る直線と、第１および第３の視点を通る直線とが略直交していることが好ましい。

また、本発明に係る３次元計測センサが有する第１〜第３のカメラ部としては、具体的には略３６０度の画角を有する全天周カメラであり、全天周カメラは、１８０度以上の画角を有する魚眼レンズを２つ有し、２つの魚眼レンズの入射面が互いに反対向き配置されている。この構成では、第１〜第３のカメラ部によって第１〜第３のカメラ部の周囲の情景を一度に取得することができ、結果として、全天周の第１〜第３の球面画像を形成できる。そして、第１〜第３の球面画像に基づいて、全天周に位置する測定点に対して３次元距離を測定することができるので、例えば、画角のより小さい平面画像を複数枚組み合わせて全天周内の複数の測定点に対する３次元距離を算出する場合に比べて、全天周内の複数の測定点に対する３次元距離をより早く且つより正確に算出することができる。

また、本発明に係る３次元計測センサでは、第１〜第３のカメラ部としては、１８０度以上の画角を有する魚眼カメラであってもよい。

また、本発明に係る３次元距離計測方法は、測定点までの３次元距離を計測する３次元距離計測方法であって、魚眼レンズ特性を有する第１のカメラ部によって第１の視点から測定点を撮像して第１の平面画像を取得し、魚眼レンズ特性を有する第２のカメラ部によって第１の視点と異なる第２の視点から測定点を撮像して第２の平面画像を取得し、魚眼レンズ特性を有する第３のカメラ部によって第１および第２の視点と異なる第３の視点から測定点を撮像して第３の平面画像を取得する平面画像取得工程と、第１の平面画像、第２の平面画像および第３の平面画像から第１の球面画像、第２の球面画像および第３の球面画像をそれぞれ形成する球面画像形成工程と、第１および第２の球面画像を対応付けて第１の視差を算出すると共に、第１および第３の球面画像を対応付けて第２の視差を算出する視差算出工程と、第１の視点から測定点までの３次元距離を算出する距離算出工程と、を備え、第１および第２の視点を通る直線と第１および第３の視点を通る直線とは交差しており、距離算出工程は、第１および第２の視差の各々に基づいて第１の視点から測定点までの第１および第２の距離パラメータを算出し、第１および第２の距離パラメータを統合することによって３次元距離を算出することを特徴とするであることを特徴とする。

本発明の３次元距離計測方法によれば、魚眼レンズ特性を有する第１〜第３のカメラ部を利用しているため、平面画像取得工程において第１〜第３のカメラ部で取得された第１〜第３の平面画像には、第１〜第３のカメラ部の周囲のより広い範囲内（視野内）の情景が含まれることになる。この情景内の任意の点が測定点であるため、情景は測定点の集合とみなすことができる。そして、上記３次元距離計測方法では、球面画像形成工程において、取得された第１〜第３の平面画像から第１〜第３の球面画像が形成された後、視差算出工程において、第１の視差が、第１および第２の球面画像を対応付けて算出され、第２の視差が、第１および第３の球面画像を対応付けて算出される。そして、測定点までの３次元距離が、距離算出工程において、第１および第２の視差にもとづく第１および第２の距離パラメータを統合して算出される。よって、一度の撮影によって広範囲の測定点に対する３次元距離を算出可能である。また、例えば、測定点が第１および第２の視点を結ぶ直線上にある場合（球面画像上ではエピポール付近に測定点の対応点が位置する場合）、第１の視差がほとんど得られないが、第１、第２および第３の視点は同一直線上にないので、第２の視差を確実に得ることができる。したがって、第１〜第３のカメラ部の撮像範囲の測定点に対しては、第１または第２の視差をより確実に得られるので、広範囲の測定点に対する３次元距離計測を行うことができる。また、第１及び第２の視点を通る直線上付近の測定点に対して第１及び第２の球面画像からのみ３次元距離を算出しようとすると、第１の視差の計測誤差が大きくなるが、その場合、第２の視差の計測誤差が小さくなるため、第１及び第２の視差に基づく第１及び第２の距離パラメータを統合することによって、より正確な３次元距離を算出可能である。

また、上記の距離算出工程は、第１および第２の距離パラメータの信頼度に応じて第１および第２の距離パラメータを重み付ける第１および第２の統合パラメータを算出することが好ましい。

上記の距離算出工程によれば、第１および第２の距離パラメータは、その計測における信頼度に応じて重み付けされて統合されるので、計測誤差を含んで信頼度の低い距離パラメータの影響を小さくすることができる。

また、上記の距離算出工程は、第１および第２の視差の各々に基づいて算出される第１の視点から測定点までの第１および第２の推定距離の逆数として第１および第２の距離パラメータを算出することが好ましい。

上記の距離算出工程によれば、第１および第２の距離パラメータは、第１および第２の視差の各々に基づいて算出される第１の視点から測定点までの３次元距離の逆数であらわされるので、測定点が非常に遠方に位置する場合においても、距離パラメータがオーバーフローすることなく算出される。

また、上記作用を好適に奏する距離算出工程は、具体的には、第１の距離パラメータをα_１２、第２の距離パラメータをα_１３、第１の視差をｄ_Ｓ２、第２の視差をｄ_Ｓ３、ｄ_Ｓ２によるα_１２の微分をα’_１２、ｄ_Ｓ３によるα_１３の微分をα’_１３、第１の距離パラメータの信頼度に応じて第１の距離パラメータα_１２を重み付ける第１の統合パラメータをｗ_１２、第２の距離パラメータの信頼度に応じて第２の距離パラメータα_１３を重み付ける統合パラメータｗ_１３とするとき、３次元距離ρ_１を上記（１）式、（２）式および（３）式によって算出する。

上記の距離算出工程によれば、第１および第２の統合パラメータが（１）式〜（３）式によって算出される。例えば、第１の距離パラメータの変化に対して第１の視差の変化が０に近い場合、すなわち第１の視差による計測で第１の距離パラメータが大きな誤差を含む可能性がある場合には、（２）式および（３）式における１／｜α´_１２｜が０に近くなり、第１の統合パラメータｗ_１２に比べて、第２の統合パラメータｗ_１３が大きくなる。逆に、第２の距離パラメータの変化に対して第２の視差の変化が０に近い場合、すなわち第２の推定距離が誤差を含みやすい場合には、（１）式および（２）式における１／｜α´_１３｜が０に近くなり、第２の統合パラメータｗ_１３に比べて、第１の統合パラメータｗ_１２が大きくなる。従って、第１および第２の距離パラメータに含まれる計測誤差の度合いに応じて、第１および第２の統合パラメータが増減されるので、距離パラメータに適度な重み付けがされて測定点の３次元距離が算出される。そのため、広範囲の測定点に対して適切に３次元距離を測定することが可能である。

本発明の３次元距離計測センサおよび３次元距離計測方法によれば、より広い範囲内の測定点の３次元距離を早く且つより正確に測定可能である。

以下、添付図面を参照して本発明の実施形態について説明する。なお、図面の説明において同一の要素には同一の符号を付し、重複する説明を省略する。また、図示の便宜上、図面の寸法比率は説明のものと必ずしも一致しない。

本実施形態では、本発明に係る３次元距離計測センサの一実施形態を、車両や移動ロボットなどの移動体に搭載されるナビゲーションシステムに適用した場合について説明する。

まず、図１〜図１１を参照して、第一実施形態にかかるナビゲーションシステムの構成について説明する。ここで、図１は本発明に係る３次元距離計測センサの一実施形態を適用したナビゲーションシステムの構成を示すブロック図、図２（ａ）はステレオカメラ部を示す斜視図である。図２（ｂ）は、第１のカメラ部の構成の一例を示す模式図であり、第１のカメラ部を側方からみた場合の図に対応する。

図１を参照すると、ナビゲーションシステム１は、３次元距離計測センサ１００、表示部２００、および、入力部３００を備えて構成されている。表示部２００は、例えばディスプレイであって、３次元距離計測センサ１００で抽出した被写体の３次元構造を操作者に表示する。また、入力部３００は、例えば、表示部２００に設けられたタッチパネルやボタンなどであって、操作者が指示コマンドを入力するためのものである。入力部３００は、入力された指示を３次元距離計測センサ１００に入力する。

３次元距離計測センサ１００は、ステレオカメラ部１０、解析装置２０を有する。ステレオカメラ部１０は、移動体において周囲の情景を取得できる位置（例えば、自動車等の車両ではボンネット上）に取り付けられている。３次元距離計測センサ１００は、後述する第１〜第３のカメラ部としてのカメラ１１ｌ，１１ｒ，１１ｄにより取得された平面画像に対応する球面画像を利用して、３次元距離計測センサ１００から測定点Ｐまでの３次元距離を算出する機能を有する。３次元距離計測センサ１００は、各カメラ１１ｌ，１１ｒ，１１ｄの視点から測定点Ｐまでの距離ρ_１，ρ_２，ρ_３までをそれぞれ測定可能である。ただし、本実施形態では、カメラ１１ｌを距離計測のための基準カメラとし、３次元距離計測センサ１００から測定点Ｐまでの３次元距離とは、カメラ１１ｌの視点から測定点Ｐまでの距離ρ_１とする。なお、図１では距離ρ_１，ρ_２，ρ_３を模式的に示している。

ステレオカメラ部１０は、カメラ（第１のカメラ部）１１ｌ、カメラ（第２のカメラ部）１１ｒおよびカメラ（第３のカメラ部）１１ｄを含んで構成されている。カメラ１１ｌの構成について図１及び図２を利用して説明する。

カメラ１１ｌは、図２に示すように、１８０度以上の画角を有する２つの魚眼レンズ１２ｌ１，１２ｌ２を有し、魚眼レンズ１２ｌ１，１２ｌ２は、それらの光軸が同一直線上であって光の入射面１４ｌ１，１４ｌ２が互いに反対向きになるように配置されている。これにより、カメラ１１ｌは、魚眼レンズ特性を有すると共に、実質的に３６０度の画角を有することになる。以下の説明では、一対の魚眼レンズ１２ｌ１，１２ｌ２を単に魚眼レンズ対１２ｌとも称す。図１では、魚眼レンズ対１２ｌを模式的に１つのレンズで示している。また、魚眼レンズ１２ｌ１，１２ｌ２の射影方式は、例えば、等距離射影方式である。魚眼レンズ１２ｌ１，１２ｌ２は必ずしも１枚のレンズでなくてもよく、魚眼レンズ特性を有する複数枚のレンズからなるレンズ系であってもよい。

図１に示すように、カメラ１１ｌは、魚眼レンズ対１２ｌで結像される像を撮像するための撮像手段１３ｌを有しており、撮像手段１３ｌは、取得した像に対応する画像データを解析装置２０に入力する。このようにカメラ１１ｌから解析装置２０に入力される画像データに対応する画像を第１の平面画像２ｌと称し、本実施形態では、第１の平面画像２ｌは、各魚眼レンズ１２ｌ１，１２ｌ２で結像されて取得される一対の魚眼画像（平面画像）を含んでいる。

撮像手段１３ｌとしては、各魚眼レンズ１２ｌ１，１２ｌ２によって結像される像を取得できれば特に限定されないが、図２に示すように、魚眼レンズ１２ｌ１，１２ｌ２の結像面にそれぞれ配置された一対のＣＣＤ撮像素子１３ｌ１，１３ｌ２が例示される。この場合、魚眼レンズ１２ｌ１とＣＣＤ撮像素子１３ｌ１は魚眼カメラ１５ｌ１を構成しており、魚眼レンズ１２ｌ２とＣＣＤ撮像素子１３ｌ２とは魚眼カメラ１５ｌ２を構成していることになる。なお、第１のカメラ部としてのカメラ１１ｌに含まれる魚眼レンズ対１２ｌ１および撮像手段１３ｌは一つの筐体内に収容されていてもよいし、図２（ｂ）に示すように別々の筐体に含まれていても良い。

上記構成のカメラ１１ｌでは、魚眼レンズ１２ｌ１，１２ｌ２を備えることで、実質的に３６０度の画角を有することから、カメラ１１ｌの全周囲（全天周）を一度に撮影可能である。すなわち、カメラ１１ｌは、全周囲を撮影可能な全天周カメラである。

カメラ部１１ｒの構成はカメラ１１ｌの構成と同様である。すなわち、図１及び図２に示すように、カメラ１１ｒは、１８０度以上の画角を有する一対の魚眼レンズ１２ｒ１，１２ｒ２及び撮像手段１３ｒを有している。一対の魚眼レンズ１２ｒ１，１２ｒ２の配置関係は、魚眼レンズ１２１１，１２ｌ２の配置関係と同じである。また、撮像手段１３ｒの構成も撮像手段１３ｌの構成と同様であり、魚眼レンズ１２ｒ１，１２ｒ２に対する撮像手段１３ｒの配置関係もカメラ１１ｌの場合と同様である。以下の説明においては、一対の魚眼レンズ１２ｒ１，１２ｒ２を魚眼レンズ対１２ｒとも称し、図１では、魚眼レンズ対１２ｒを１つのレンズとして模式的に示している。なお、カメラ１１ｒにおいても、撮像手段１３ｒが有する一対のＣＣＤ撮像素子１３ｒ１，１３ｒ２と魚眼レンズ１２ｒ１，１２ｒ２とは、それぞれ魚眼カメラ１５ｒ１，１５ｒ２を構成していることになり、その構成の模式図は、図２（ｂ）において「ｌ」を「ｒ」に変えたものに対応する。

上記構成のカメラ１１ｒでは、撮像手段１３ｒは、魚眼レンズ対１２ｒで結像された像を撮像しその像に対応する画像データを解析装置２０に入力する。このようにカメラ１１ｒから解析装置２０に入力される画像データに対応する画像を第２の平面画像２ｒを称し、本実施形態では、第２の平面画像２ｒは、各魚眼レンズ１２ｒ１，１２ｒ２によって結像された像に対応する一対の魚眼画像（平面画像）を含んでいることになる。そして、カメラ部１１ｒも魚眼レンズ１２ｌ１，１２ｌ２を備えることで、実質的に３６０度の画角を有していることから、カメラ部１１ｒの全周囲（全天周）を一度に撮影可能な全天周カメラである。

更に、カメラ部１１ｄの構成もカメラ１１ｌの構成と同様である。すなわち、図１及び図２に示すように、カメラ１１ｄは、１８０度以上の画角を有する一対の魚眼レンズ１２ｄ１，１２ｄ２及び撮像手段１３ｄを有している。一対の魚眼レンズ１２ｄ１，１２ｄ２の配置関係は、魚眼レンズ１２１１，１２ｌ２の配置関係と同じである。また、撮像手段１３ｄの構成も撮像手段１３ｌの構成と同様であり、魚眼レンズ１２ｄ１，１２ｄ２に対する撮像手段１３ｄの配置関係もカメラ１１ｌの場合と同様である。以下の説明においては、一対の魚眼レンズ１２ｄ１，１２ｄ２を魚眼レンズ１２ｄとも称し、図１では、魚眼レンズ対１２ｄをレンズとして模式的に示している。なお、カメラ１１ｄにおいても、撮像手段１３ｄが有する一対のＣＣＤ撮像素子１３ｄ１，１３ｄ２と魚眼レンズ１２ｒｄ，１２ｒｄとは、それぞれ魚眼カメラ１５ｄ１，１５ｄ２を構成していることになり、カメラ１１ｄの模式図は、図２（ｂ）において添え字の「ｌ」を「ｄ」に変えたものに対応する。

上記構成のカメラ１１ｄでは、撮像手段１３ｄは、魚眼レンズ対１２ｄで結像され撮像手段１３ｄで取得された像に対応する画像データを解析装置２０に入力する。このようにカメラ１１ｄから解析装置２０に入力される画像データに対応する画像を第３の平面画像２ｄと称し、本実施形態では、第３の平面画像２ｄは、魚眼レンズ１２ｄ１，１２ｄ２で結像された像に対応する一対の魚眼画像（平面画像）を含んでいることになる。そして、カメラ１１ｄも魚眼レンズ１２ｄ１，１２ｄ２を備えることで、実質的に３６０度の画角を有していることから、カメラ１１ｄの全周囲（全天周）を一度に撮影可能な全天周カメラである。

ステレオカメラ部１０では、図２に示すように、カメラ１１ｌ〜１１ｄは、カメラ１１ｌとカメラ１１ｒとが水平方向に離して配置され、カメラ１１ｌとカメラ１１ｄとが鉛直方向に離して配置されている。この場合、カメラ１１ｌ〜１１ｄにより３つの異なる第１〜第３の視点Ｖ１〜Ｖ３から測定点Ｐが撮影されることになる。換言すれば、第１〜第３の視点Ｖ１〜Ｖ３は、第１及び第２の視点Ｖ１，Ｖ２を通る直線としてのベースラインＲｒと、第１及び第３の視点Ｖ１，Ｖ３を通る直線としてのベースラインＲｄとが略直交するような位置関係を有しており、カメラ１１ｌ〜１１ｄは、第１〜第３の視点Ｖ１〜Ｖ３から測定点Ｐが撮影されるように配置されていることになる。なお、本実施形態では、第１及び第２の視点Ｖ１，Ｖ２間の距離と、第１及び第３の視点Ｖ１，Ｖ３間の距離とは略一致しているものとして説明する。

図３はカメラ１１ｌ，１１ｒ，１１ｄを利用して撮影して取得された平面画像の一例に対応する図である。図３（ａ）は、カメラ１１ｌの魚眼レンズ１２ｌ１によって結像された図を示し、図３（ｂ）は、カメラ１１ｒの魚眼レンズ１２ｒ１によって結像された図を示し、図３（ｃ）は、カメラ１１ｄの魚眼レンズ１２ｄ１によって結像された図を示す。図３に示されるように、カメラ１１ｌ，１１ｒ，１１ｄが魚眼レンズ特性を有するため、カメラ１１ｌ，１１ｒ，１１ｄで取得される画像では、画像の視野範囲が略円形になり、ピンホールカメラに比べてカメラ周囲のより広い視野内の環境情報を一度に取得できる。

解析装置２０は、演算を行うマイクロプロセッサ、マイクロプロセッサに各処理を実行させるためのプログラム等を記憶するＲＯＭ、演算結果などの各種データを記憶するＲＡＭ、および入出力インターフェース等を備え、球面画像形成部２１と、メモリ部２２と、パラメータ算出部２３と、視差算出手段２４と、経緯度画像形成部２５と、対応点探索部２６と、距離算出手段２７と、構造解析部２８、制御部２９とを有する。解析装置２０の各構成要素は、ハードウエア的に個別に構成されてもよいし、あるいは、共通のハードウエア上で稼働する各ソフトウエアによって構成されてもよい。これらのソフトウェアは別々のプログラム、サブルーチン等によって構成される場合もあるが、一部またはその大部分のルーチンを共有していてもよい。

球面画像形成部２１は、第１の平面画像２ｌ、第２の平面画像２ｒおよび第３の平面画像２ｄに基づいて第１の球面画像３ｌ、第２の球面画像３ｒおよび第３の球面画像３ｄをそれぞれ形成する機能を有する。

ここで、３次元距離計測センサ１００において利用する球面画像について説明する。この球面画像とは、カメラで取得された平面画像を仮想球面に投影して得られる画像である。より具体的に図４を利用して説明する。図４は球面画像の形成を説明するための図である。ここでは、カメラは魚眼レンズを有しているものとし、その魚眼レンズの焦点距離は、魚眼レンズ１２ｌ１等と同様にｆとする。図４において、Ｏ_ｃ−Ｘ_ｃＹ_ｃＺ_ｃはカメラ座標系、ｆはカメラの焦点距離、Ｏ_ｃはカメラの焦点、Ｏ_ｉ−Ｘ_ｉＹ_ｉは画像平面座標系、Ｏ_ｉは画像中心、Ｐは測定点、Ｒ１はＰに対する投影線、ｐは仮想球面３における測定点Ｐの投影点、ｑは平面画像における測定点Ｐに対する撮像点、ｒは画像中心Ｏ_ｉと撮像点ｑとの距離、θは投影線Ｒ１の天頂角、φはＸ_ｃ軸に対する方位角、をそれぞれ示す。

球面画像は、カメラの焦点Ｏ_ｃを中心としてカメラの焦点距離ｆを半径とする仮想球面３に平面画像の撮像点ｑを投影することによって得られる画像である。以下、平面画像が投影された球面３を、球面画像３とも表記する。このような球面画像３では、周りの全ての視野をカバーでき全天周を持つことが可能である。なお、本明細書では、球面射影に従って得られた部分的な球面も球面画像、例えば半球視野に対応する半球画像も特に断らない限り球面画像と称す。

ここで、球面画像３における投影点ｐの位置ベクトルｍと、平面画像における撮像点ｑの座標ｑ（ｘ_ｉ，ｙ_ｉ）との関係は以下の式によって表される。

次に、球面画像形成部２１により、第１の平面画像２ｌから第１の球面画像３ｌを形成する方法について説明する。図５は、球面画像形成部２１により形成される第１〜第３の球面画像３ｌ，３ｒ，３ｄの斜視図である。前述したように第１の平面画像２ｌは、魚眼レンズ１２ｌ１，１２ｌ２により結像された像に対応する一対の魚眼画像を含んでいる。よって、球面画像形成部２１は、先ず、第１の平面画像２ｌに含まれる一対の魚眼画像に対応する球面画像をそれぞれ形成する。すなわち、各魚眼画像における全撮像点を（４）式〜（６）式を利用して射影し、球面画像を得る。この際、各魚眼画像に対応する球面画像は、魚眼カメラ１５ｌ１，１５ｌ２のカメラ座標系で表されている。各魚眼画像に対応する球面画像は、半球視野をカバーした半球面画像に対応するため、それらを所定の画像変換によって統合することによってカメラ１１ｌの全周囲の情景が投影された第１の球面画像３ｌが形成されることになる。図５では、この第１の球面画像３ｌを表示するための座標系をカメラ１１ｌのカメラ座標系Ｓ_ｌと称し、Ｏ_１−Ｘ_１Ｙ_１Ｚ_１で表す。そして、カメラ座標系Ｓ_ｌの原点Ｏ_１がカメラ１１ｌの視点（第１の視点）Ｖ_１となる。

第２及び第３の球面画像３ｒ，３ｄの形成方法も同様である。そして、第２の球面画像３ｒを表示するための座標系をカメラ１１ｒのカメラ座標系Ｓ_ｒと称し、Ｏ_２−Ｘ_２Ｙ_２Ｚ_２で表す。また、第３の球面画像３ｄを表示するための座標系をカメラ１１ｄのカメラ座標系Ｓ_ｄと称し、Ｏ_３−Ｘ_２Ｙ_３Ｚ_３で表す。また、カメラ座標系Ｓ_ｒの原点Ｏ_２がカメラ１１ｒの視点（第２の視点）Ｖ２となり、カメラ座標系Ｓ_ｄの原点Ｏ_３がカメラ１１ｄの視点（第３の視点）Ｖ３となる。

上記視点Ｖ１〜Ｖ３のカメラ１１ｌ，１１ｒ，１１ｄ内における位置は、カメラ１１ｌ，１１ｒ，１１ｄにおいて同様に決定されていれば特に限定されず、第１〜第３の球面画像３ｌ，３ｒ，３ｄの各々を形成するために、２つの半球面画像を統合する際の画像変換により調整可能である。カメラ１１ｌの場合（すなわち、第１の球面画像３ｌの場合）を例にして説明すると、魚眼カメラ１５ｌ１のカメラ座標系に、他方の魚眼カメラ１５ｌ２のカメラ座標系で表される半球面画像を投影することで、第１の球面画像３ｌを形成した場合には、魚眼レンズ１２ｌ１の焦点が視点Ｖ１に対応することになる。また、カメラ１１ｌに含まれる２つの魚眼カメラ１５ｌ１，１５ｌ２のカメラ座標系で表される半球面画像を、カメラ１１ｌの中心を原点として設定された所定のカメラ座標系に投影することで第１の球面画像３ｌを形成した場合、カメラ１１ｌの中心が視点Ｖ１となる。カメラ１１ｒ，１１ｄについても同様である。なお、全天周に位置する測定点Ｐに対する距離計測の観点からは、カメラ１１ｌ，１１ｒ，１１ｄの中心に設定されていることが好ましい。

また、球面画像形成部２１は、図２に示された各平面画像２ｌ，２ｒ，２ｄの画素位置としての点の座標と、球面画像３ｌ，３ｒ，３ｄ上の点の座標とを関連付けたテーブル（第１のテーブル）を作成しメモリ部２２に登録する。

メモリ部２２は、各部の処理結果を格納する機能を有し、例えば、ＦＲＯＭ（Flash Read Only Memory）やＨＤＤ(Hard Disk Drive)などの不揮発性メモリで構成される。 各部の処理結果とは、具体的には、カメラ１１ｌ，１１ｒ，１１ｄの内部パラメータおよび外部パラメータ、各平面画像２ｌ，２ｒ，２ｄの画素位置としての点座標と、球面画像３ｌ，３ｒ，３ｄ上の点座標とを関連付けた第１のテーブル等である。

パラメータ算出部２３は、カメラ１１ｌ，１１ｒ，１１ｄの内部パラメータおよび外部パラメータを算出し、メモリ部２２に格納する。

カメラ１１ｌ，１１ｒ，１１ｄの内部パラメータとは、具体的には、カメラ１１ｌ，１１ｒ，１１ｄが有する魚眼レンズ１２ｌ１，１２ｌ２，１２ｒ１，１２ｒ２，１２ｄ１，１２ｄ２の焦点距離及び焦点が例示される。この焦点距離及び焦点の校正に伴い、特願２００５−２０６０３４号明細書に示されているように、画像平面上における径方向の放射歪みや接線歪みも校正していることが好ましい。また、カメラ１１ｌ，１１ｒ，１１ｄの外部パラメータとは、具体的には、所定の世界座標系Ｓ_ｗに対する各カメラ座標系Ｓ_ｌ，Ｓ_ｒ，Ｓ_ｄの姿勢を示す回転行列および並進ベクトルが例示される。内部パラメータおよび外部パラメータの算出には、カメラ１１ｌ，１１ｒ，１１ｄの画角が１８０度を越えることから、例えば、図６（ａ）に示されるような、ハーフボックスが使用され、例えば、特願２００５−２０６０３４号明細書に記載の手法によりパラメータが算出される。すなわち、図６（ａ）に示すように３面にカラーマーキング（図６（ａ）では格子点）が施されたハーフボックスにおける３面の接点（角部）を原点とした世界座標系を設定する。ハーフボックスをカメラ１１ｌ、１１ｒ，１１ｄでそれぞれ撮影し、予め初期値として設定されている各種パラメータを利用して形成される球面画像３ｌ，３ｒ，３ｄ上の上記格子点の位置と、世界座標系で表される格子点を球面画像３ｌ，３ｒ，３ｄに投影した際の格子点との位置関係に基づいて内部及び外部パラメータを校正することが考えられる。図６（ａ）は、カメラ校正のために用いるハーフボックスに対応する図であり、図６（ｂ）は、図６（ａ）をカメラの魚眼レンズで撮影した画像に対応する図である。

経緯度画像形成部２５は、球面画像に対応する経緯度画像を形成する機能を図７及び図８を利用して説明する。図７はカメラ１１ｌ，１１ｒで構成される横ペアの経緯度画像に対する対応点探索を示す図である。図８はカメラ１１ｌ，１１ｄで構成される縦ペアの経緯度画像に対する対応点探索を示す図である。

経緯度画像とは、図７および図８に示されるような、球面画像での経度および緯度が直交するように展開された画像である。ここで、６ｌ、６ｒ、６ｄはそれぞれ球面画像３ｌ、３ｒ、３ｄに対応する経緯度画像、７ｒは経線に沿ったエピポーラ線、７ｄは緯線に沿ったエピポーラ線、を示す。また、経緯度画像形成部２５は、図２に示された球面画像３ｌ，３ｒ，３ｄ上の点の座標と、経緯度画像上の点６ｌ，６ｒ，６ｄの座標とを関連付けたテーブル（第２のテーブル）を作成しメモリ部２２に登録する。

図９は経緯度画像の一例を示す図である。なお、図９に示した経緯度画像６ｌは球面画像３ｌを展開して形成されるものである。球面画像３ｌにおける投影点と経緯度画像６ｌにおける点との関係は、以下の式によって求められる。ここで、ＸeYe平面は経緯度画像６ｌを含む平面、Ｌ（ｉ，ｊ）（ｉ＝０，１，・・・，ｍ；ｊ＝０，１，・・・，ｎ）は経緯度画像６ｌに含まれておりエピポーラ線７ｌ上の点を示す。球面画像３ｌにおける点Ｌに対応する点ｐの座標ｐ（ｕ，ｖ，ｓ）は、それぞれ以下の式で表される。

なお、（７）式〜（９）式では球面座標３ｌの半径ｆは便宜的に１としているが、ｆが１でない場合には、（７）式〜（９）式で表されるｕ、ｖ、ｓをそれぞれｆ倍すればよい。なお、図９では、球面画像３ｌから形成される経緯度画像６ｌを示しているが、球面画像３ｒ，３ｄから形成される経緯度画像は、図９において添え字「ｌ」をそれぞれ「ｒ」，「ｄ」に変えたものに相当する。この際、図９に示した点Ｌは、経緯度画像６ｒ，６ｄにおけるＬ（ｉ，ｊ）に含まれておりエピポーラ線７ｒ，７ｄ上の点を示すことになる。球面画像３ｒ，３ｄにおける経緯度画像６ｒ，６ｆ上の点Ｌに対応する点の座標も（７）式〜（９）式と同様の関係で対応付けられる。

対応点探索部２６は、経緯度画像形成部２５で形成された一対の経緯度画像６ｌ，６ｒ，６ｄにおいて測定点Pに対応する対応点ｐ_ｌ，ｐ_ｒ，ｐ_ｄを特定する機能を有する。前述したようにエピポーラ線７ｌとそれに対応するエピポーラ線７ｒとが平行になっているので、相関法を適用することによって対応点ｐ_ｌ，ｐ_ｒ，ｐ_ｄを容易に探索することができる。

視差算出手段２４は、第１の球面画像３ｌと第２の球面画像３ｒとを対応付けてそれぞれの対応点ｐ_ｌ，ｐ_ｒ間の第１の視差ｄ_１ｒを算出し、第１の球面画像３ｌと第３の球面画像３ｄとを対応付けてそれぞれの対応点ｐ_ｌ，ｐ_ｄ間の第２の視差ｄ_１ｄを算出する機能を有する。

ここで、図１０および図１１を参照して、第１の視差ｄ_１ｒおよび第２の視差ｄ_１ｄの算出方法について説明する。図１０は一対の球面画像における視差を説明するための図である。図１１は図１０のエピポーラ平面の模式図である。なお、図１０および図１１は、第１の視差ｄ_１ｒの算出方法に対応しており、第２の視差ｄ_１ｄを算出する場合には、図１０および図１１に記載の添え字「ｒ」を「ｄ」に置き換えるものとする。第１の視差ｄ_１ｒは、エピポーラ平面５ｒにおける円弧Ｃ_ｌと円弧Ｃ_ｒとの差によって定義され、第２の視差ｄ_１ｄは、エピポーラ平面５ｄにおける円弧Ｃ_ｌと円弧Ｃ_ｄとの差によって定義される。したがって、第１の視差ｄ_１ｒおよび第２の視差ｄ_１ｄは、それぞれ以下の式で算出される。

ここで、θ_ｌ、θ_ｒおよびθ_ｄは、対応点ｐ_ｌ，ｐ_ｒの座標をｐ_ｌ（ｕ_ｌ，ｖ_ｌ，ｓ_ｌ）、ｐ_ｒ（ｕ_ｒ，ｖ_ｒ，ｓ_ｒ）とすると、それぞれ以下の式で表される。

また、視差算出手段２４は、平面画像２ｌ，２ｒ，２ｄに対して（又は平面画像に対応する経緯度画像に対して）視差ｄをマッピングすることで視差図を作成し、視差に応じて濃淡を付けて表示部２００に表示させることも好適である。表示部２００で表示される画像の濃淡でカメラ１０ｌ，１０ｒに対する被写体の配置関係が分かる。

距離算出手段２７は、第１の視差ｄ_１ｒおよび第２の視差ｄ_１ｄの各々に基づいて第１の視点Ｖｌから測定点Ｐまでの第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄを算出し、第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄを統合することによって３次元距離ρ_１を算出する機能を有する。距離算出手段２７は、距離パラメータ算出部２７A、統合パラメータ算出部２７B、距離算出部２７Cによって構成されている。

距離パラメータ算出部２７Aは、第１の視差ｄ_１ｒおよび第２の視差ｄ_１ｄの各々に基づいて第１の視点Ｖ_ｌから測定点Ｐまでの第１および第２の推定距離を算出し、第１及び第２の推定距離の逆数として第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄを算出する機能を有する。

ここで、図１１を参照して、第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄの具体的な算出方法について説明する。第１の距離パラメータα_１ｒ（α_１２）および第２の距離パラメータα_１ｄ（α_１３）は、第１の視点Ｖ１から測定点Pまでの第１および第２の推定距離をρ_１ｒ、ρ_１ｄとすると、それぞれ以下の式で算出される。

ここで、ｂ_ｓは、本実施形態では、第１の視点Ｖｌと第２の視点Ｖ２との間の距離及び第１の視点Ｖｌと第３の視点Ｖ３との間の距離である。

統合パラメータ算出部２７Bは、第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄの信頼度に応じて第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄを重み付ける第１の統合パラメータｗ_１ｒ（ｗ_１２）および第２の統合パラメータｗ_１ｄ（ｗ_１３）を算出する機能を有する。

ここで、第１の統合パラメータｗ_１ｒおよび第２の統合パラメータｗ_１ｄの具体的な算出方法について説明する。第１の統合パラメータｗ_１ｒおよび第２の統合パラメータｗ_１ｄは、それぞれ以下の式で算出される。

ここで、α’_１ｒはｄ_１ｒによるα_１ｒの微分、α’_１ｄはｄ_１ｄによるα_１ｄの微分で定義され、それぞれ以下の式で表される。

距離算出部２７Cは、第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄを統合することによって３次元距離ρ_１を算出する機能を有する。具体的には、３次元距離ρ_１は、以下の式で算出される。

構造解析部２８は、距離算出手段２７によって算出された距離ρ_ｌを利用してカメラ１１ｌ，１１ｒ，１１ｄの周囲の情景内の三次元形状を復元する。また、カメラ１１ｌ，１１ｒ，１１ｄで取得された平面画像２ｌ，２ｒ，２ｄ（又はそれに対応する経緯度画像６ｌ，６ｒ，６ｄ）を利用して各測定点Ｐに対応する対応点ｐ_ｌ，ｐ_ｒ，ｐ_ｄでの色情報や領域分割、パターン認識などの画像情報と、復元された三次元形状とにより三次元構造を抽出する。構造解析部２８は、抽出した三次元構造の情報を制御部２９に入力する。

制御部２９は、入力部３００から入力された指示コマンドに基づいて撮像手段１３ｌ，１３ｒ，１３ｄおよび解析装置２０を制御する。例えば、第１〜第３の平面画像２ｌ，２ｒ，２ｄを取得する旨の指示が入力部３００を通して入力されると、制御部２９は、撮像手段１３ｌ，１３ｒ，１３ｄを駆動して第１〜第３の平面画像２ｌ〜２３を球面画像形成部２１に入力せしめる。また、制御部２９は、第１〜第３の平面画像２ｌ，２ｒ，２ｄの取得を停止する（又はナビゲーションを停止する）旨の指示コマンドが入力されると、撮像手段１３ｌ，１３ｒ，１３ｄから球面画像形成部２１への第１〜第３の平面画像２ｌ，２ｒ，２ｄの画像データの入力を停止せしめる。

更に、制御部２９は、入力部３００から内部パラメータおよび外部パラメータを算出する旨の指示コマンドが入力されると、パラメータ算出部２３によって内部パラメータおよび外部パラメータを算出し、メモリ部２２に入力せしめる。また、制御部２９は、構造解析部２８で抽出した三次元構造および距離算出手段２７に基づいて、カメラ１１ｌの周囲の三次元構造までの３次元距離ρ_ｌが所定の距離よりも短くなれば、ナビゲーションシステム１が搭載される車両等の移動体のブレーキなどを作動させる制御信号を移動体の制御装置（不図示）に向けて発信する。制御信号としては、前述のようにブレーキなどを作動させるものでもよいし、アラーム信号を発するものなども有効である。

次に、図１２〜図１４を参照して、ナビゲーションシステム１の動作を説明し、併せて本発明による３次元距離計測方法の一実施形態を説明する。ここで、図１２はナビゲーションシステムの動作を示すフローチャート、図１３は図１２のステップＳ１０のサブルーチンを示すフローチャート、図１４は図１３のステップＳ１５のサブルーチンを示すフローチャートである。

図１２を参照すると、先ず、ステップS１０において、前処理としてカメラ１１ｌ，１１ｒ，１１ｄの内部パラメータおよび外部パラメータを校正する。この内部パラメータおよび外部パラメータの校正方法を、図１３を参照して詳細に説明する。

先ず、ステップS１０Ａにおいて、操作者は、図６（a）に示したハーフボックスをカメラ１１ｌ，１１ｒ，１１ｄの視野内に配置する。その後、操作者によって入力部３００を介して制御部２９に撮影を指示する指示が入力されると、制御部２９は、ＣＣＤ撮像素子１３ｌ，１３ｒ，１３ｄを制御して、被写体としての校正パターンの像（すなわち、第１〜第３の平面画像２ｌ，２ｒ，２ｄ）を取得する。

ステップS１０Ｂにおいて、パラメータ算出部２３は、撮像手段１３ｌ，１３ｒ，１３ｄからの第１〜第３の平面画像２ｌ，２ｒ，２ｄに対応する画像データを受けとり、特願２００５−２６０３４号明細書に記載の手法と同様にしてカメラ１１ｌ，１１ｒ，１１ｄの内部パラメータ及び外部パラメータを算出する。

図１２を参照すると、ステップS１０での内部パラメータおよび外部パラメータの算出後、ステップS１１において、カメラ１１ｌ，１１ｒ，１１ｄにステレオカメラ部１０の周囲の情景を被写体として撮影せしめる。次に、ステップS１２において、経緯度画像形成部２５は、メモリ部２２に記録されている第２のテーブルを参照して、取得された平面画像２ｌ，２ｒ，２ｄから経緯度画像６ｌ，６ｒ，６ｄを形成する。

続いて、ステップS１３において、対応点探索部２６は、経緯度画像６ｌ，６ｒ，６ｄに相関法を適用することで、ステレオカメラ部１０の周囲の情景内の点Ｐ（実空間上の測定点Ｐ）に対応する対応点ｐ_ｌ，ｐ_ｒ，ｐ_ｄを特定する。次に、ステップS１４において、視差算出手段２４は、第１の球面画像３ｌと第１の球面画像３ｒとを対応付けてそれぞれの対応点ｐ_ｌ，ｐ_ｒ間の第１の視差ｄ_１ｒを算出し、第１の球面画像３ｌと第３の球面画像３ｄとを対応付けてそれぞれの対応点ｐ_ｌ，ｐ_ｄ間の第２の視差ｄ_１ｄを算出する。より具体的には、（１０）式〜（１４）式によって算出される。

ステップS１５において、距離算出手段２７は、第１の視差ｄ_１ｒおよび第２の視差ｄ_１ｄの各々に基づいて第１の視点Ｖ１から測定点Pまでの第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄを算出し、第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄを統合することによって３次元距離ρ_１を算出する。この３次元距離ρ_１算出方法を、図１４を参照して、詳細に説明する。

ステップＳ１５Aにおいて、距離パラメータ算出部２７Aは、第１の視差ｄ_１ｒおよび第２の視差ｄ_１ｄの各々に基づいて算出される第１の視点Ｖ１から測定点Ｐまでの第１および第２の推定距離ρ_１ｒ，ρ_１ｄの逆数として第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄを算出する。より具体的には、（１５）式および（１６）式によって算出する。

ステップＳ１５Bにおいて、統合パラメータ算出部２７Bは、第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄの信頼度に応じて第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄを重み付ける第１の統合パラメータｗ_１ｒおよび第２の統合パラメータｗ_１ｄを算出する。より具体的には、第１の統合パラメータｗ_１ｒおよび第２の統合パラメータｗ_１ｄは、（１７）式〜（２０）式によって算出される。

ステップＳ１５Cにおいて、距離算出部２７Cは、第１の距離パラメータα_１ｒおよび第２の距離パラメータα_１ｄを統合することによって３次元距離ρ_１を算出する。より具体的には、３次元距離ρ_１を、（２１）式によって算出する。

このように距離ρ_ｌが算出された後、図１２を参照すると、ステップS１６において、構造解析部２８は、原画像である平面画像２ｌ，２ｒ，２ｄと距離ρ_ｌとに基づいてステレオカメラ部１０の周囲の情景の３次元形状を復元し、平面画像２ｌ，２ｒ，２ｄが有する画像情報に基づいて被写体の３次元構造を抽出する。

ステップS１７において、表示部２００は、構造解析部２８で解析された３次元構造を操作者に表示し、ステップS１８において、制御部２９は、構造解析部２８によって抽出されたステレオカメラ部１０の周囲の３次元構造および距離ρ_ｌに基づいてナビゲーションシステム１が搭載されている車両などの移動体を制御する。移動体の制御の例としては、例えば、ステレオカメラ部１０の周囲の３次元構造物までの距離ρ_ｌが、予め設定してある所定の距離よりも短い場合には、移動体の移動を停止せしめる制御信号（例えば、ブレーキを作動させる信号）を発信する。

次に、ステップS１９において、制御部２９は、入力部３００から撮影を停止する旨（言い換えれば、ナビゲーションを停止する旨）の指示が入力されたか否かを判断する。停止する旨の指示に対応する停止コマンドが入力されていない場合には（ステップS１９で「Ｎｏ」）、ステップS１２に戻る。また、停止コマンドが入力された場合には（ステップS１９で「Ｙｅｓ」）、ナビゲーションを終了する。

なお、上記方法では、カメラ１１ｌ，１１ｒ，１１ｄの内部パラメータ、外部パラメータの校正を実施しているが、これらは、例えば、３次元距離計測センサ１００を組み立てた際に一度実施しておけばよく、次にステレオカメラ部１０の周囲の３次元構造を抽出する場合には、ステップS１２から実施すればよい。

以上のように、３次元距離計測センサ１００によれば、全天周カメラであるカメラ１１ｌ，１１ｒ，１１ｄを利用して各カメラ１１ｌ，１１ｒ，１１ｄの全周囲を一度に撮影している。その結果、各カメラ１１ｌ，１１ｒ，１１ｄで取得される第１〜第３の平面画像２ｌ，２ｒ，２ｄに基づいて全天周の第１〜第３の球面画像３ｌ，３ｒ，３ｄを取得することができる。そして、第１〜第３の球面画像３ｌ，３ｒ，３ｄを利用して測定点Ｐまでの距離ρ_１を算出しているため全天周内の各測定点Ｐまでの距離ρ_１をより早く且つより正確に計測することが可能である。

また、距離ρ_１を算出する際に、第１及び第２の球面画像３ｌ，３ｒと、第１及び第３の球面画像３ｌ，３ｄとを利用してそれぞれ算出されるれ第１及び第２の視差ｄ_１ｒ，ｄ_１ｄに基づいて、第１及び第２の推定距離ρ_１ｒ，ρ_１ｄを算出し、その逆数を第１及び第２の距離パラメータα_１ｒ，α_１ｄとし、それらを所定の重み付け、すなわち、第１及び第２の統合パラメータｗ_１ｒ，ｗ_１ｄにより重み付けして統合していることも重要である。

仮に、第１及び第２の推定距離ρ_１ｒ，ρ_１ｄを直接統合して３次元距離ρ_１を算出しようとすると、測定点Pが無限遠にある場合にオーバーフローが生じ易い。一方、距離計測において無限遠の点を予め排除しておくことは困難である。これに対して、第１及び第２の推定距離ρ_１ｒ，ρ_１ｄの逆数を第１及び第２の距離パラメータα_１ｒ，α_１ｄとして利用する場合、（１５）式及び（１６）式から理解されるようにオーバーフローは主にθｒが０又はπのときである。よって、距離ρ_１を算出する際に、θｒが０又はπである点を予め除いておくことで容易にオーバーフローを回避でき、結果として、より早く距離ρ_１を算出できる。また、第１及び第２の視差ｄ_１ｒ，ｄ_１ｄが画像上の対応点探索の結果として算出されるため、それらには誤差が含まれることになる。このような視差の計測誤差のよる第１及び第２の距離パラメータα_１ｒ，α_１ｄの変動幅Δα_Ｎ（Ｎは、１ｒ又は１ｄ）は、次の式で表される。

この式から理解されるように、第１及び第２の距離パラメータα_１ｒ，α_１ｄの変動幅、言い換えれば誤差の範囲は、視差ｄ_１ｌ，ｄ_１ｒのみならず、対応点の位置にも依存する。すなわち、第１及び第２の球面画像３ｌ，３ｒの組又は第１及び第３の球面画像３ｌ，３ｄの組の何れかにおいて対応点がエピポール近傍になるにつれて、対応する変動幅Δα_１ｒ又はΔα_１ｄが大きくなることになる。従って、仮に、一組の第１及び第２の球面画像３ｌ，３ｒから算出される第１の視差ｄ_１ｌのみから距離ρ_１を算出していると、エピポール近傍の対応点に対して算出される距離の誤差が大きくなる。

これに対して、３次元計測センサ１では、第１〜第３の球面画像３ｌ，３ｒ，３ｄのうちの２組の球面画像対、すなわち、第１及び第２の球面画像３ｌ，３ｒの組と、第１及び第３の球面画像３ｌ，３ｄの組とを利用して第１及び第２の視差ｄ_１ｒ，ｄ_１ｄを算出し、それらから第１及び第２の距離パラメータを算出している。そして、第１及び第２の距離パラメータα_１ｒ，α_１ｄを（２２）式のｄ_Ｎの係数を利用して規定されている第１及び第２の統合パラメータｗ_１ｒ，ｗ_１ｄによって重み付けて統合しているため、より正確に距離ρ_１を算出可能である。

すなわち、第１〜第３の視点Ｖ１〜Ｖ３が同一直線上にないため、例えば第１及び第２の球面画像３ｌ，３ｒの組から算出される対応点がエピポーラに近づいた場合には、第１及び第３の球面画像３ｌ，３ｄの組から算出される対応点はエピポーラから離れた位置にある傾向がある。そして、（１７）式及び（１８）式によれば、第１及び第２の統合パラメータｗ_１ｌ，ｗ_１ｒは、一方の距離パラメータの信頼度が低い場合には、その低い方の距離パラメータの重み付けを小さくし、他方の距離パラメータの重み付けを大きくするように決定されている。従って、（２１）式では、第１及び第２の距離パラメータα_１ｒ，α_１ｄの信頼度に応じて３次元距離ρ_１が決定されることになるので、３次元距離計測センサ１００ではより正確な距離ρ_１を算出することが可能である。

なお、上記作用・効果を奏するためには、前述したように、カメラ１１ｌ，１１ｒが水平方向に離れ、カメラ１１ｌ，１１ｄが鉛直方向に離れることで、第１及び第２の視点Ｖ１，Ｖ２を通る直線としてのベースラインＲｒ、第１及び第３の視点Ｖ１，Ｖ３を通る直線としてのベースラインＲｄとが略直交していることが好ましい。この場合、例えば球面画像３ｌ，３ｒにおいて対応点が前述したように、エピポールに近づいたとしても、他方の球面画像の組である球面画像３ｌ，３ｄ側での対応点はエピポールからより離れた所に位置しやすく、結果として、第１及び第２の視差ｄ_１ｒ，ｄ_１ｄの何れか一方をより高い精度で算出できることになる。

図１に示したナビゲーションシステム１では、カメラ１１ｌ，１１ｒ，１１ｄは、魚眼レンズ対１２ｌ，１２ｒ，１２ｄを有する全天周カメラとしているため、３次元距離計測センサ１００は、単に３つのカメラ１１ｌ，１１ｒ，１１ｄで３次元距離計測センサ１００の周囲の位置する測定点Ｐまでの３次元距離ρ_１を容易に測定できることになる。すなわち、より少ないカメラ数で周囲の位置する測定点Ｐまでの３次元距離ρ_１を容易に測定できる構成となっている。前述したように、カメラ１１ｌ，１１ｒ，１１ｄの各々は、一対の魚眼カメラ１５ｌ１，１５ｌ２，１５ｒ１，１５ｒ２，１５ｄ１，１５ｄ２が、互いに反対方向を撮影しているともみなせる。この場合には、３次元距離計測センサ１００は、６個の魚眼カメラ１５ｌ１，１５ｌ２，１５ｒ１，１５ｒ２，１５ｄ１，１５ｄ２により、３次元距離計測センサ１００の周囲に位置する測定点Ｐまでの３次元距離ρ_１を測定できることになる。この場合、魚眼カメラを１つのカメラの単位とした場合、より少ないカメラ数で周囲に位置する測定点Ｐまでの３次元距離ρ_１を測定可能な構成となっている。

なお、カメラ１１ｌ，１１ｒ，１１ｄは、図１５に示すように、１つの魚眼レンズ１２ｌ１，１２ｒ１，１２ｄ１を有する魚眼カメラ１５ｌ１，１５ｒ１、１５ｄ１から構成されているとすることもできる。この場合には、魚眼レンズ１２ｌ１，１２ｒ１，１２ｄ１の画角に応じた半球以上の視野内の画像が投影された第１〜第３の球面画像３ｌ，３ｒ，３ｄを一度に取得できることになるため、広範囲の情景内の測定点Ｐに対して距離ρ１をより早く且つより高精度に算出できることになる。よって、リアルタイム性が求められるナビゲーションシステムなどに好適に利用できる。

図１６〜図１９を利用して３次元計測センサ１００及びその３次元計測センサ１００を利用した３次元距離計測方法の効果について説明する。ここでは、３眼ステレオ（本実施形態）の場合と２眼ステレオの場合に対する、３次元距離マップを元に構成された復元画像を比較しながら、より具体的に説明する。

３次元距離計測センサ１００が有するステレオカメラ部１０を図１６に示す構成とした。すなわち、カメラ１１ｌ，１１ｒ，１１ｄは、ソニー製のビデオカメラ（ＤＣＲ−ＨＣ３０）にオリンパス製の魚眼レンズ（ＦＣＯＮ−０２）を取り付けた魚眼カメラとした。魚眼レンズの画角は、約１８５度である。そして、カメラ１１ｌ，１１ｒ，１１ｄは、カメラ１１ｌ，１１ｒを水平方向に離し、カメラ１１ｌ，１１ｄを鉛直方向に離して配置した。なお、ここでは、カメラ１１ｌ，１１ｒ，１１ｄの内部・外部パラメータは校正されているものとして説明する。

図１７（a）は、カメラ１１ｌ，１１ｒで構成される横ペアによって作成された３次元距離マップ、図１７（ｂ）は図１７（a）に基づいて復元された復元画像を示し、図１８（a）はカメラ１１ｌ，１１ｄで構成される縦ペアによって作成された３次元距離マップ、図１８（ｂ）は図１８（a）に基づいて復元された復元画像を示す、図１９は縦ペア、横ペアの画像を統合して復元された復元画像を示す。

上記図１７（ａ）及び図１８（ａ）は、第１及び第２の推定距離ρ_１ｒ，ρ_１ｄがマッピングされて形成されたものに相当し、それを利用して復元された図１７（ｂ）及び図１８（ｂ）に示す復元画像では、どちらもエピポール付近において大きな歪みが観測されていることがわかる。

一方、図１９は本実施形態で示したように、３つのカメラ１１ｌ，１１ｒ，１１ｄで取得された画像を全て利用すると共に、（２１）式を利用して第１及び第２の距離パラメータα_１ｒ，α_１ｄが統合されて算出される距離ρ_１に基づいて復元されたものである。図１９では、図１７（ｂ）及び図１８（ｂ）に現れていた歪みが解消されていることが確認できる。

なお、上述した実施形態は本発明に係る３次元距離計測センサ及び３次元距離計測方法の一例を示すものである。本発明に係る３次元距離計測センサ及び３次元距離計測方法は、上記実施形態に限られるものではなく、本発明の要旨を変更しないように上記実施形態を変形したものであってもよい。例えば、カメラ１１ｌ，１１ｒ，１１ｄが有する撮像手段１３ｌ，１３ｒ，１３ｄは、それぞれ一対のＣＣＤ撮像素子を有するとしたが、これに限定されない。例えば、１つの撮像手段１３ｌが一つのＣＣＤ撮像素子から構成されていてもよい。この場合には、一つのＣＣＤ撮像素子の撮像面を２つの領域に分けてその２つの領域でそれぞれ魚眼レンズ１２ｌ１，１２ｌ２によって結像される像を取得すればよい。撮像手段１３ｒ，１３ｄについても同様である。

本発明に係る３次元距離計測センサおよび３次元距離計測方法は、車両や移動ロボットなどのナビゲーションシステムや周辺監視システムに利用することが可能である。

本発明に係る３次元距離計測センサの一実施形態を適用したナビゲーションシステムの構成を示すブロック図である。 図２（ａ）は、ステレオカメラ部を示す斜視図である。図２（ｂ）は、第１のカメラ部の構成の一例を示す模式図であり、第１のカメラ部を側方からみた場合の図に対応する図である。 図３（ａ）は、第１のカメラ部を利用して撮影して取得された平面画像の一例に対応する図である。図３（ｂ）は、第２のカメラ部を利用して撮影され取得された平面画像の一例に対応する図である。図３（ｃ）は、第３のカメラ部を利用して撮影され取得された平面画像の一例に対応する図である。 球面画像の模式図である。 ３次元距離計測センサで形成される球面画像の模式図である。 図６（ａ）は、カメラ校正のために用いるハーフボックスを示す図である。図６（ｂ）は、図６（ａ）を魚眼レンズで撮影した画像の一例に対応する図である。 第１及び第２のカメラ部で構成される横ペアの経緯度画像に対する対応点探索を示す図である。 第１及び第３のカメラ部構成される縦ペアの経緯度画像に対する対応点探索を示す図である。 経緯度画像の形成を説明する図である。 球面ステレオにおける視差を説明するための図である。 図１０のエピポーラ平面の模式図である。 ナビゲーションシステムの動作を示すフローチャートである。 図１２のステップＳ１０のサブルーチンを示すフローチャートである。 図１２のステップＳ１５のサブルーチンを示すフローチャートである。 ステレオカメラ部の他の例を模式的に示す斜視図である。 実験に用いたステレオカメラ部を示す斜視図である。 図１７（a）は、第１及び第２のカメラ部で構成される横ペアによって作成された３次元距離マップである。図１７（ｂ）は、図１７（a）に基づいて復元された復元画像である。 図１８（a）は、第１及び第３のカメラ部で構成される横ペアによって作成された３次元距離マップである。図１８（ｂ）は、図１８（a）に基づいて復元された復元画像である。 横ペアと縦ペアの画像を統合することによって復元された復元画像である。

符号の説明

１…３次元距離計測センサ、３ｌ…球面画像（第１の球面画像）、３ｒ…球面画像（第２の球面画像）、３ｄ…球面画像（第３の球面画像）、５…エピポーラ平面、６ｌ…経緯度画像、７ｌ，７ｒ，７ｄ…エピポーラ線、１０…ステレオカメラ部、１１ｌ…カメラ（第１のカメラ部）、１１ｒ…カメラ（第２のカメラ部）、１１ｄ…カメラ（第３のカメラ部）、２１…球面画像形成部、２２…メモリ部、２３…パラメータ算出部、２４…視差算出手段、２５…経緯度画像形成部、２６…対応点探索部、２７…距離算出手段、２７Ａ…距離パラメータ算出部、２７Ｂ…統合パラメータ算出部、２７Ｃ…距離算出部、２８…構造解析部、１００…３次元距離計測センサ、１１０…距離算出装置、Ｃ_ｌ…円弧、Ｃ_ｒ…円弧、ｅ_１ｌ，ｅ_２ｌ…第１の球面画像の一対のエピポール、ｅ_１ｒ，ｅ_２ｒ…第２の球面画像の一対のエピポール、Ｖ１…第１の視点、Ｖ２…第２の視点、Ｖ３…第３の視点、ｐ_ｌ…測定点の第１の球面画像上の対応点、ｐ_ｒ…測定点の第２の球面画像上の対応点、ｐ_ｄ…測定点の第３の球面画像上の対応点、Ｓ_ｌ…カメラ座標系、Rｒ…ベースライン（第１及び第２の視点を通る直線）、Rｄ…ベースライン（第１及び第３の視点を通る直線）。

Claims (9)

1. 測定点までの３次元距離を計測する３次元距離計測センサであって、
   魚眼レンズ特性を有すると共に、第１の視点から前記測定点を撮像する第１のカメラ部と、
   魚眼レンズ特性を有すると共に、前記第１の視点と異なる第２の視点から前記測定点を撮像する第２のカメラ部と、
   魚眼レンズ特性を有すると共に、前記第１および第２の視点と異なる第３の視点から前記測定点を撮像する第３のカメラ部と、
   前記第１のカメラ部、前記第２のカメラ部および前記第３のカメラ部の各々によって取得され出力される第１の平面画像、第２の平面画像および第３の平面画像から第１の球面画像、第２の球面画像および第３の球面画像をそれぞれ形成する球面画像形成手段と、
   前記第１および第２の球面画像を対応付けて第１の視差を算出すると共に、前記第１および第３の球面画像を対応付けて第２の視差を算出する視差算出手段と、
   前記第１の視点から前記測定点までの３次元距離を算出する距離算出手段と、
   を備え、
   前記第１および第２の視点を通る直線と前記第１および第３の視点を通る直線とは、交差しており、前記距離算出手段は、前記第１および第２の視差の各々に基づいて前記第１の視点から前記測定点までの前記第１および第２の距離パラメータを算出し、前記第１および第２の距離パラメータを統合することによって前記３次元距離を算出し、
   前記距離算出手段は、
   前記第１の距離パラメータをα ₁₂ 、前記第２の距離パラメータをα ₁₃ 、前記第１の視差をｄ _S2 、前記第２の視差をｄ _S3 、ｄ _S2 によるα ₁₂ の微分をα' ₁₂ 、ｄ _S3 によるα ₁₃ の微分をα' ₁₃ 、第１の距離パラメータの信頼度に応じて第１の距離パラメータα ₁₂ を重み付ける第１の統合パラメータをｗ ₁₂ 、第２の距離パラメータの信頼度に応じて第２の距離パラメータα ₁₃ を重み付ける第２の統合パラメータｗ ₁₃ とするとき、３次元距離ρ ₁ を
   

   

   

   

   によって算出することを特徴とする３次元距離計測センサ。
2. 前記距離算出手段は、
   前記第１および第２の距離パラメータの信頼度に応じて前記第１および第２の距離パラメータを重み付ける前記第１および第２の統合パラメータを算出し、前記第１及び第２の距離パラメータを前記第１及び第２の統合パラメータによって重み付けして統合することを特徴とする請求項１に記載の３次元距離計測センサ。
3. 前記距離算出手段は、
   前記第１および第２の視差の各々に基づいて算出される前記第１の視点から前記測定点までの第１および第２の推定距離の逆数として前記第１および第２の距離パラメータを算出することを特徴とする請求項１または２に記載の３次元距離計測センサ。
4. 前記第１および第２の視点を通る直線と、前記第１および第３の視点を通る直線とは略直交していることを特徴とする請求項１〜３のいずれか一項に記載の３次元距離計測センサ。
5. 前記第１〜第３のカメラ部の各々は、略３６０度の画角を有する全天周カメラであり、前記全天周カメラは、１８０度以上の画角を有する魚眼レンズを２つ有し、２つの前記魚眼レンズがそれらの入射面が互いに反対向きになるように配置されていることを特徴とする請求項１〜４のいずれか一項に記載の３次元距離計測センサ。
6. 前記第１〜第３のカメラ部の各々は、１８０度以上の画角を有する魚眼カメラであることを特徴とする請求項１〜４のいずれか一項に記載の３次元距離計測センサ。
7. 測定点までの３次元距離を計測する３次元距離計測方法であって、
   魚眼レンズ特性を有する第１のカメラ部によって第１の視点から前記測定点を撮像して第１の平面画像を取得し、魚眼レンズ特性を有する第２のカメラ部によって前記第１の視点と異なる第２の視点から前記測定点を撮像して第２の平面画像を取得し、魚眼レンズ特性を有する第３のカメラ部によって前記第１および第２の視点と異なる第３の視点から前記測定点を撮像して第３の平面画像を取得する平面画像取得工程と、
   前記第１の平面画像、前記第２の平面画像および前記第３の平面画像から第１の球面画像、第２の球面画像および第３の球面画像をそれぞれ形成する球面画像形成工程と、
   前記第１および第２の球面画像を対応付けて第１の視差を算出すると共に、前記第１および第３の球面画像を対応付けて第２の視差を算出する視差算出工程と、
   前記第１の視点から前記測定点までの３次元距離を算出する距離算出工程と、
   を備え、
   前記第１および第２の視点を通る直線と前記第１および第３の視点を通る直線とは交差しており、
   前記距離算出工程では、前記第１および第２の視差の各々に基づいて前記第１の視点から前記測定点までの前記第１および第２の距離パラメータを算出し、前記第１および第２の距離パラメータを統合することによって前記３次元距離を算出し、
   前記距離算出工程は、
   前記第１の距離パラメータをα ₁₂ 、前記第２の距離パラメータをα ₁₃ 、前記第１の視差をｄ _S2 、前記第２の視差をｄ _S3 、ｄ _S2 によるα ₁₂ の微分をα' ₁₂ 、ｄ _S3 によるα ₁₃ の微分をα' ₁₃ 、第１の距離パラメータの信頼度に応じて第１の距離パラメータα ₁₂ を重み付ける第１の統合パラメータをｗ ₁₂ 、第２の距離パラメータの信頼度に応じて第２の距離パラメータα ₁₃ を重み付ける第２の統合パラメータｗ ₁₃ とするとき、３次元距離ρ ₁ を
   

   

   

   

   によって算出することを特徴とする３次元距離計測方法。
8. 前記距離算出工程では、
   前記第１および第２の距離パラメータの信頼度に応じて前記第１および第２の距離パラメータを重み付ける前記第１および第２の統合パラメータを算出し、前記第１および第２の距離パラメータを前記第１及び第２の統合パラメータによって重み付けして統合することによって前記３次元距離を算出することを特徴とする請求項７に記載の３次元距離計測方法。
9. 前記距離算出工程では、
   前記第１および第２の視差の各々に基づいて算出される前記第１の視点から前記測定点までの第１および第２の推定距離の逆数として前記第１および第２の距離パラメータを算出することを特徴とする請求項７または８に記載の３次元距離計測方法。

Images