JP3900353B2 - ３次元位置計測システムおよび３次元位置計測方法 - Google Patents

Description

本発明は、２つのカメラを用いて３次元空間内における位置計測を行う３次元位置計測システムおよび３次元位置計測方法に関するものである。

従来より、２つのカメラを用いて３次元空間内で移動する物体の位置計測が行われている。この３次元空間内での位置計測について、図１０に示す３次元位置の計測手法を示した概略図を利用して説明する。

２つのカメラＣ１、Ｃ２により、同期するタイミングで画像が取り込まれると、予め決めておいた３次元座標（Ｘω，Ｙω，Ｚω）上におけるカメラＣ１、Ｃ２の位置と、それぞれのカメラＣ１、Ｃ２が撮影した画像を２次元的に示した場合における物体の座標（ｕ１，ｖ１）、（ｕ２，ｖ２）とを通る光線ｌ１、ｌ２がそれぞれ求められる。そして、ステレオ視により、その２直線の交点Ｐが求められる。この交点Ｐの３次元座標が物体の位置を示しており、このような手法により物体の位置計測を行うことができる。

上記した従来の３次元位置計測では、２つのカメラによる画像の取り込みが同期的に行われる必要がある。この場合、例えば１秒間に３０コマの画像取り込みを行える分解能を有するカメラ（３０ｆｐｓ）を用いているとすると、１秒間に３０回分、物体の３次元位置計測を行うことが可能となる。

しかしながら、物体の３次元位置計測を数多く実行したくても、カメラの単位時間当たりの撮影回数に限界があるという機能限界が存在するため、１秒間に３０回以上の計測が行えない。特に、カメラからの画像に基づいてロボットを制御する場合において、ロボットをより最適に制御するためには、ロボットの３次元位置計測のサイクル、すなわちビジュアルフィードバックのサイクルを高速に行う必要がある。

本発明は上記点に鑑みて、カメラの単位時間当たりの撮影回数を超える回数の３次元位置計測が行える３次元位置計測システムおよび３次元位置計測方法を提供することを目的とする。

上記目的を達成するため、請求項１に記載の３次元位置計測システムでは、同じサイクルかつ互いに異なるタイミングで画像を取り込む第１、第２のカメラ（Ｃ１、Ｃ２）と、第１のカメラ（Ｃ１）が取り込んだ画像を取得し、その画像に存在する物体（２）の第１のカメラ（Ｃ１）の画像上での座標となる画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ））を求める第１画像取得部（３ａ）と、第２のカメラ（Ｃ２）が取り込んだ画像を取得し、その画像に存在する物体（２）の第２のカメラ（Ｃ２）の画像上での座標となる画像座標（（ｕ^２ _ｔ,ｖ^２ _ｔ））を求める第２画像取得部（３ｂ）と、第１、第２画像取得部（３ａ、３ｂ）で求められた画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ）、（ｕ^２ _ｔ,ｖ^２ _ｔ））に基づいて、物体（２）の３次元位置を求める演算部（３ｃ）とを有している。

その演算部（３ｃ）では、第１、第２のカメラ（Ｃ１、Ｃ２）から最新フレームの画像を取り込んだ方のカメラを選択し、選択されたカメラ（Ｃ１）において最新フレームの画像が取り込まれたタイミング（ｔ）の１サイクル前のタイミング（ｔ−１）を２フレーム前のタイミングとして、そのタイミング（ｔ−１）での物体（２）の３次元位置（Ｐ_ｔ−１）が２フレーム前の３次元位置とされる。また、選択されたカメラ（Ｃ１）とは異なる選択されていないカメラ（Ｃ２）により、最新フレームが取り込まれたタイミング（ｔ）とその１サイクル前のタイミング（ｔ−１）との間に画像が取り込まれたときのタイミング（ｔ’−１）を１フレーム前のタイミングとして、そのタイミング（ｔ’−１）での物体（２）の３次元位置（Ｐ_ｔ’−１）が１フレーム前の３次元位置とされる。そして、２フレーム前および１フレーム前の３次元位置（Ｐ_ｔ−１、Ｐ_ｔ’−１）から最新フレームの画像が取り込まれたタイミング（ｔ）での物体（２）の仮の３次元位置となる第１の予測位置（Ｐ^＊ｔ）を求めると共に、選択されたカメラ（Ｃ１）の位置と該選択されたカメラ（Ｃ１）からの画像に基づいて、第１画像取得部（３ａ）で求められた画像座標（（ｕ ^１ _ｔ,ｖ ^１ _ｔ））とを通過する直線（ｌ^１ _ｔ）を求め、直線（ｌ^１ _ｔ）上の点のうち、第１の予測位置（Ｐ^＊ｔ）から最短距離の点を、物体（２）の３次元位置とするようになっていることを特徴としている。

このように、２フレーム前および１フレーム前の物体（２）の３次元位置に基づいて、最新フレームにおける物体（２）の３次元位置を例えば線形予測によって予測し、その予測した位置を第１の予測位置（Ｐ^＊ｔ）としている。さらに、選択されたカメラ（Ｃ１）と画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ））とを通過する直線（ｌ^１ _ｔ）を求めている。つまり、選択されたカメラ（Ｃ１）の位置と物体（２）の画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ））を共に通過する直線（ｌ^１ _ｔ）は、物体（２）が実際に存在している直線を意味している。このため、直線（ｌ^１ _ｔ）上の点のうち、第１の予測位置（Ｐ^＊ｔ）から最短距離に位置する点を求めることにより、物体（２）の３次元位置を正確に求めることができる。

例えば、請求項２に示されるように、２フレーム前の３次元位置は、選択されていないカメラ（Ｃ２）が２フレーム前のタイミング（ｔ−１）で画像の取り込みを行ったと想定した場合に、そのタイミング（ｔ−１）における選択されていないカメラ（Ｃ２）の画像上での画像座標（（ｕ^２＊ _ｔ−１,ｖ^２＊ _ｔ−１））を求め、その画像座標（（ｕ^２＊ _ｔ−１,ｖ^２＊ _ｔ−１））と、このタイミング（ｔ−１）において選択されたカメラ（Ｃ１）の画像に基づいて第１画像取得部（３ａ）で求められた画像座標（（ｕ^１ _ｔ−１,ｖ^１ _ｔ−１））とから求められる。

また、１フレーム前の３次元位置は、選択されたカメラ（Ｃ１）が１フレーム前のタイミング（ｔ’−１）で画像の取り込みを行ったと想定した場合に、そのタイミング（ｔ’−１）における選択されたカメラ（Ｃ１）の画像上での画像座標（（ｕ^１＊ _ｔ’−１,ｖ^１＊ _ｔ’−１））を求め、その画像座標（（ｕ^１＊ _ｔ’−１,ｖ^１＊ _ｔ’−１））と、このタイミング（ｔ’−１）において選択されていないカメラ（Ｃ２）の画像に基づいて第２画像取得部（３ｂ）で求められた画像座標（（ｕ^２ _ｔ’−１,ｖ^２ _ｔ’−１））とから求められる。

さらに、請求項３に示されるように、２フレーム前のタイミング（ｔ−１）における選択されていないカメラ（Ｃ２）の画像上での画像座標（（ｕ^２＊ _ｔ−１,ｖ^２＊ _ｔ−１））は、１フレーム前のタイミング（ｔ’−１）に選択されていないカメラ（Ｃ２）が取り込んだ画像に基づいて第２画像取得部（３ｂ）で求められた画像座標（（ｕ^２ _ｔ’−１,ｖ^２ _ｔ’−１））と、そのタイミング（ｔ’−１）よりも１サイクル前のタイミング（ｔ’−２）に選択されていないカメラ（Ｃ２）が取り込んだ画像に基づいて第２画像取得部（３ｂ）で求められた画像座標（（ｕ^２ _ｔ’−２,ｖ^２ _ｔ’−２））とを、これら各タイミング（ｔ’−１、ｔ’−２）と２フレーム前のタイミング（ｔ−１）との時間ずれ（δ１２、δ２１）に基づいて内挿演算することによって求められる。

また、１フレーム前のタイミング（ｔ’−１）における選択されたカメラ（Ｃ１）の画像上での画像座標（（ｕ^１＊ _ｔ’−１,ｖ^１＊ _ｔ’−１））は、最新フレームの画像が取り込まれたタイミング（ｔ）にその画像に基づいて第１画像取得部（３ａ）で求められた画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ））と、そのタイミング（ｔ）よりも１サイクル前のタイミング（ｔ−１）に選択されたカメラ（Ｃ１）が取り込んだ画像に基づいて第１画像取得部（３ａ）で求められた画像座標（（ｕ^１ _ｔ−１,ｖ^１ _ｔ−１））とを、これら各タイミング（ｔ、ｔ−１）と２フレーム前のタイミング（ｔ’−１）との時間ずれ（δ１２、δ２１）に基づいて内挿演算することによって求められる。

請求項４ないし６に記載の発明は、請求項１ないし３に記載の発明を方法として記載したものである。これら各請求項に示す発明により、請求項１ないし３に記載の発明と同様の効果を得ることができる。

なお、上記各手段の括弧内の符号は、後述する実施形態に記載の具体的手段との対応関係を示すものである。

（第１実施形態）
本発明の一実施形態を適用した３次元位置計測システムの概略構成を図１に示す。以下、この図に基づいて３次元位置計測システムの構成について説明する。

図１に示される３次元位置計測システムは、図中黒丸で示したように、３次元空間１内に存在する物体２の位置を計測するものである。本実施形態では、縦、横、高さがそれぞれ３,０００ｍｍ、２,０００ｍｍ、２,０００ｍｍの３次元空間１を仮想して設定している。この３次元空間１の縦、横、高さは、それぞれこの３次元空間１の任意の点を中心とする世界座標（Ｘｗ，Ｙｗ，Ｚｗ）によって表され、３次元空間１中に存在する移動中の物体２の位置がその世界座標（Ｘｗ，Ｙｗ，Ｚｗ）で示されるようになっている。例えば、図１に示される長方体で区画された３次元空間１のうち、紙面手前側の左下に位置する頂点を中心とする世界座標、すなわち直線ａをＸ座標軸、直線ｂをＹ座標軸、直線ｃをＺ座標軸として表される座標によって物体２の位置が示されるようになっている。

３次元空間１の上部、具体的には、ＸＹ平面から高さ２，８００ｍｍに２つのカメラＣ１、Ｃ２が設置されている。カメラＣ１、Ｃ２は共にＹ座標が１，０００ｍｍ程度の位置、つまり３次元空間１におけるＹ座標の中央ラインの上方に併設されており、カメラＣ１は、ほぼＸＺ平面上に、カメラＣ２は、ほぼＸＺ平面から３，０００ｍｍ離れた位置に、それぞれ配置されている。

カメラＣ１、カメラＣ２は、共に３次元空間１のほぼ全域が視野に入るように、例えば画像の中央が３次元空間１の中央位置とほぼ一致するように傾けられた状態とされている。これら各カメラＣ１、Ｃ２としては、単位時間当たりの画像取り込み回数が等しいもの、例えば１／３０秒のサイクルで１枚の画像が処理可能な分解能を有する３０ｆｐｓのものが用いられている。このため、カメラＣ１、Ｃ２は共に、例えば、１秒間に３０回分、画像を取り込むことができるようになっている。

カメラＣ１、Ｃ２のシャッタータイミングは図示しない同期機からの信号に基づいて設定される。本実施形態では、カメラＣ１、Ｃ２のシャッタータイミングのズレδ１２、δ２１、すなわちカメラＣ１のシャッタータイミングからカメラＣ２のシャッタータイミングまでのズレδ１２と、カメラＣ２のシャッタータイミングからカメラＣ１のシャッタータイミングまでのズレδ２１が共に、各カメラＣ１、Ｃ２の画像取り込み間隔の半分に設定されるようになっている。

カメラＣ１、Ｃ２は、ケーブル４を介してパーソナルコンピュータ（以下、ＰＣという）３に接続されている。ＰＣ３は、ＲＯＭ、ＲＡＭなどのメモリ、入出力部（Ｉ／Ｏ）、ＣＰＵを備えた周知のもので構成され、そのＣＰＵは、カメラＣ１からの画像を取り込みその画像を用いて各種処理を実行する第１画像取得部３ａ、カメラＣ２からの画像を取り込みその画像を用いて各種処理を実行する第２画像取得部３ｂ、および、第１、第２画像取得部３ａ、３ｂからのデータに基づいて物体２の３次元位置計測を行う演算部３ｃを有して構成されている。

第１、第２画像取得部３ａ、３ｂは、ケーブル４を介してカメラＣ１、Ｃ２が取り込んだ画像を取り込み、画像取り込みの終了時刻（キャプチャ終了時刻）を求めると共に、その画像中の色などから観測対象となる物体２を識別する。そして、さらに物体２の中心を求めたのち、キャプチャ終了時刻や物体２の中心に関するデータを演算部３ｃに出力するようになっている。演算部３ｃは、第１、第２画像取得部３ａ、３ｂからのデータに基づいて、後述する各種演算処理を行うことで、物体２の３次元位置を求めるものである。そして、ＰＣ３で求められた物体２の３次元位置が世界座標（Ｘｗ，Ｙｗ，Ｚｗ）で表され、この３次元位置がビジュアルフィードバックされることで、ロボット制御が実行される。

続いて、本実施形態のＰＣ３が実行する３次元位置計測について説明する。

本実施形態では、最新フレームの画像が取り込まれたタイミングにおいて、観測対象物となる物体２が３次元空間１中のどの位置に存在するかという物体２の３次元位置を予測することにより、３次元位置計測を行っている。

具体的には、まず、（１）３次元位置の線形予測として、計算により求めた過去数フレームの３次元位置を用いて最新フレームでの物体２の位置予測を行う。つまり、最新フレームの画像が取り込まれたタイミングｔにおける物体２の世界座標を予測する。そして、（２）光線情報を用いた位置予測として、カメラＣ１、Ｃ２のうち最新フレームの画像を取り込んだ方の世界座標とその画像中に存在する物体２の画像座標（ｕ，ｖ）とを通る３次元空間１中の直線を求め、その直線上の点のうち、位置予測された最新フレームにおける物体２の世界座標から最短距離のものを求める。この点が最新フレームの画像が取り込まれたタイミングにおける物体２の３次元位置となる。

以下、図２に示す３次元位置計測の概略を示した模式図を参照して、（１）３次元位置の線形予測と（２）光線情報を用いた位置予測による本実施形態の３次元位置計測の方法を詳細に説明する。なお、ここでは、最新のフレームでの画像がカメラＣ１で取り込まれた場合を例に挙げて説明する。

（１）３次元位置の線形予測
図２に示されるように、以前に求められた前回および前々回のフレームの画像が取り込まれたタイミングｔ’−１、ｔ−１における物体２の３次元位置がＰ_ｔ’−１とＰ_ｔ−１であったとする。この場合、最新フレームが取り込まれたタイミングｔにおける物体２の３次元位置の第１の予測位置Ｐ^＊ｔ［＝（ｘｗ,ｙｗ,ｚｗ）］は、次式のように求められる。

（数１）
Ｐ^＊ｔ＝Ｐ_ｔ’−１＋δ_２１ｖ_ｔ−１
ただし、ｖ_ｔ−１は単位時間当たりの物体２の３次元位置の移動速度を示したものであり、数式２に示されるように、前々回の物体２の３次元位置から前回の物体２の３次元位置までの距離を、タイミングｔ’−１からタイミングｔ−１までの間隔で割ったものに相当する。

（数２）
ｖ_ｔ−１＝（Ｐ_ｔ’−１−Ｐ_ｔ−１）／δ_１２
つまり、数式１で示された第１の予測位置Ｐ^＊ｔは、過去２フレームのタイミングｔ’−１、ｔ−１における物体２の３次元位置から物体２の線形の移動速度を求め、前回のフレームでの物体２の３次元位置からその移動速度でタイミングｔ’−１からタイミングｔまでの時間移動した想定した場合に物体２が到達する位置である。

（２）光線情報を用いた位置予測
続いて、最新フレームにおけるカメラＣ１の画像座標から求まる３次元空間１の直線を求める。世界座標における原点０からカメラＣ１への平行移動ベクトル、つまりカメラＣ１の世界座標がＴ^１＝［Ｔｘ，Ｔｙ，Ｔｚ］^Ｔとして定義され、カメラＣ１にて取り込まれた画像平面が３次元空間１内に定義されるｕｖ座標平面として表されるとした場合に、物体２の画像座標（ｕ^１ｔ,ｖ^１ｔ）で定義されるとすると、図２中に示されるように、カメラＣ１の世界座標Ｔ^１と物体２の画像座標（ｕ^１ｔ,ｖ^１ｔ）を共に通過する直線がｌ^１ _ｔとして表される。

なお、ここでいうｕｖ座標平面は、カメラＣ１から距離ｆの位置に存在し、カメラＣ１の光軸となるｚ座標軸に対して垂直をなし、かつ、ｚ座標軸がその中心位置を通過している平面を指す。カメラＣ２の画像に対しても、カメラＣ１と同様のｕｖ座標平面が定義され、最新フレームがカメラＣ２で取り込まれたものであった場合には、カメラＣ１と同様の手法によって、カメラＣ２の世界座標Ｔ^２＝［Ｔｘ，Ｔｙ，Ｔｚ］^Ｔと物体２の画像座標（ｕ^２ｔ,ｖ^２ｔ）を共に通過する直線ｌ^２ _ｔが求められる。また、カメラＣ１、Ｃ２の世界座標および各カメラＣ１、Ｃ２に対応するｕｖ座標平面は周知のキャリブレーション技術によって求められる。

そして、世界座標中における直線ｌ^１ _ｔの傾きを表すベクトルがｒ^１ｔ＝［ｘｖ,ｙｖ,ｚｖ）］^Ｔであるとすると、直線ｌ^１ _ｔは次式となる。ただし、ｋは実数である。

（数３）
ｌ^１ _ｔ＝ｋｒ^１ _ｔ＋Ｔ^１
本来ならば、上述の数式１により求めた第１の予測位置Ｐ^＊ｔは、この数式３で定義されるｌ^１ _ｔ上に存在する。しかしながら、第１の予測位置Ｐ^＊ｔは、過去に計算した位置から推定したものであり、予測誤差を含んでいる。例えば、物体２の移動量がタイミングｔ−１〜ｔの間とタイミングｔ’−１〜ｔ−１の間で異なっている場合も有り得るし、物体２が３次元空間１中を必ずしも線形的に移動しているとは限らない。これらの場合には、第１の予測位置Ｐ^＊ｔが直線ｌ^１ _ｔ上に存在しないことになる。

このため、直線ｌ^１ _ｔのうち第１の予測位置Ｐ^＊ｔから最短距離となる点Ｐｔを求める。この点は、第１の予測位置Ｐ^＊ｔが直線ｌ^１ _ｔの傾きを表すベクトルｒ^１ｔ方向への正射影ベクトルであり、次式のように表される。

このようにして、点Ｐｔが求められると、この点Ｐｔが最新フレームでの物体２の３次元位置とされる。このように予測された最新フレームでの物体２の３次元位置は、カメラＣ１の世界座標Ｔ１と物体２の画像座標（ｕ^１ｔ,ｖ^１ｔ）を共に通過する直線ｌ^１ _ｔ上に存在していることから、物体２の位置として正確なものであると言える。

次いで、上述した過去数フレームにおける３次元位置の予測方法について説明する。

上述のように最新フレームにおける物体２の３次元位置をより正確に得るためには、数式１に示した速度ｖｔ−１の演算に用いられる過去２フレームにおける物体２の３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１を精度良く求める必要がある。これら各３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１として適用可能なものとしては、例えば以下に示す２通りが挙げられる。

（ａ）数式３を用い、各フレームそれぞれのタイミングｔ−１、ｔ’−１で物体２の３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１を求め、そのようにして求めた３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１を用いる方法。

（ｂ）最新フレームでのタイミングｔにおいて、過去フレームにおける物体２の３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１を再度計算し直す方法。

これら２つのいずれの場合でも、３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１として適用可能であるが、（ａ）に示した方法の場合、第１回目のフレームでの物体２の３次元位置Ｐ_１を求めるにあたり、物体２の３次元位置の初期値となるＰ_０が必要となる。また、フレーム毎に計算された物体２の３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１は、数式１の線形予測によって求められたものである。このため、その位置精度はすでに推定による予測誤差が生じている場合があり、そのような誤差を有している過去フレームでの３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１に基づいて最新フレームでの物体２の３次元位置Ｐ_ｔを求めれば、その誤差が伝搬されることになる。

したがって、本実施形態では、より好ましい（ｂ）に示した方法を採用している。以下、この（ｂ）に示される３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１の再計算の手法について、図３、図４を参照して説明する。

図３は、カメラＣ１、Ｃ２のシャッタータイミングを示す図であり、図３（ａ）、（ｂ）は、それぞれ本実施形態のシャッタータイミングと従来のシャッタータイミングとを示したものである。

図３（ａ）に示されるように、本実施形態では、カメラＣ１では、ｔ−２、ｔ−１、ｔのタイミングで画像を取り込み、カメラＣ２では、ｔ’−２、ｔ’−１のタイミングで画像を取り込んでいる。ここで、今回カメラＣ１で画像の取り込みを完了したときのタイミングがｔであり、カメラＣ１で１回前と２回前のサイクルの際に画像の取り込みを完了したタイミングがｔ−１、ｔ−２である。また、タイミングｔの直前にカメラＣ２で画像の取り込みを完了したタイミング、およびその前のサイクルの際に画像の取り込みを完了したタイミングがｔ’−１、ｔ’−２である。なお、このときのカメラＣ１、Ｃ２のシャッタータイミングのズレδ１２、δ２１は、同期機にて各カメラＣ１、Ｃ２の画像取り込み間隔の半分となるように設定されている。

このように、本実施形態では、カメラＣ１、Ｃ２でそれぞれ異なるタイミングで画像を取り込むことになり、各カメラＣ１、Ｃ２における単位時間当たりの画像取り込み回数分の２倍の数分、異なるタイミングの画像が存在することになる。

これに対して、図３（ｂ）に示されるように、従来は、カメラＣ１、Ｃ２により同じタイミングで画像を取り込むようにしている。このため、カメラＣ１およびカメラＣ２における単位時間当たりの画像取り込み回数分しか異なるタイミングに取り込んだ画像が存在しない。

図４は、各カメラＣ１、Ｃ２で取り込んだ画像、および、カメラＣ１、Ｃ２がお互いのシャッタータイミングでないときに仮に画像を取り込んでいたとしたら得られたであろう画像を時系列的に示した図である。図中、実線で囲んだフレームが、各カメラＣ1、Ｃ２で取り込んだ画像、点線で囲んだフレームが、カメラＣ１、Ｃ２がお互いのシャッタータイミングでないときに仮に画像を取り込んでいたとしたら得られたであろう画像を示している。

図４に示されるように、カメラＣ１からはｔ−１、ｔのタイミング、カメラＣ２からはｔ’−２、ｔ’−１のタイミングの画像が得られる。しかしながら、カメラＣ１からはｔ’−２、ｔ’−１のタイミング、カメラＣ１からはｔ−１、ｔのタイミングで画像が得られない。

したがって、過去２フレームそれぞれのタイミングｔ−１、ｔ’−１における物体２の３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１を、カメラＣ１、Ｃ２のうち画像が得られた方はその画像を用いて、カメラＣ１、Ｃ２のうち画像が得られない方はその前後の画像を用いて求める。

具体的には、以下のｓｔｅｐ１〜３を実行することで、過去２フレームそれぞれのタイミングｔ−１、ｔ’−１における物体２の３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１を求める。

［ｓｔｅｐ１］
まず、カメラＣ２で仮にタイミングｔ−１で画像を取り込んでいたとしたときの物体２の仮の画像座標（ｕ^２＊ _ｔ−１，ｖ^２＊ _ｔ−１）を、タイミングｔ−１の前後のフレーム、つまりタイミングｔ’−２、ｔ’−１に得られた画像に基づいて推定する。例えば、タイミングｔ’−２、ｔ’−１における物体２の画像座標（ｕ^２ _ｔ’−１，ｖ^２ _ｔ’−１）、（ｕ^２ _ｔ’−２，ｖ^２ _ｔ’−２）を内挿演算することで、仮の画像座標（ｕ^２＊ _ｔ−１，ｖ^２＊ _ｔ−１）が得られる。この仮の画像座標（ｕ^２＊ _ｔ−１，ｖ^２＊ _ｔ−１）の各座標を数式で表すと、それぞれ次式のように表される。

なお、カメラＣ１、Ｃ２のシャッタータイミングのズレδ１２、δ２１が共に、各カメラＣ１、Ｃ２の画像取り込み間隔の半分に設定され、等しい値とされているため、数式５、６は次式のように簡略化される。

（数７）
ｕ^２＊ _ｔ−１＝１／２ｕ^２ _ｔ’−２＋１／２ｕ^２ _ｔ’−１
（数８）
ｖ^２＊ _ｔ−１＝１／２ｖ^２ _ｔ’−２＋１／２ｖ^２ _ｔ’−１
［ｓｔｅｐ２］
タイミングｔ−１におけるカメラＣ１が取り込んだ画像における物体２の画像座標（ｕ^１ _ｔ−１，ｖ^１ _ｔ−１）に基づき、タイミングｔ−１でカメラＣ２が取り込んだと想定した仮の画像内におけるエピポーラ線ｅ^２を求める。ここでいうエピポーラ線ｅ^２とは、カメラＣ１で取り込まれた画像から求めた物体２の画像座標（ｕ^１ _ｔ−１，ｖ^１ _ｔ−１）と対応する線であり、カメラＣ１で取り込まれた画像内の画像座標（ｕ^１ _ｔ−１，ｖ^１ _ｔ−１）の位置に物体２が存在するのであれば、カメラＣ２で取り込んだ画像内においてそのエピポーラ線ｅ^２上に物体２が存在していると考えられる。

そして、エピポーラ線ｅ^２上の点のうち、数式５〜８で定義された仮の画像座標（ｕ^２＊ _ｔ−１，ｖ^２＊ _ｔ−１）から最短距離となるものを求める。この点が、カメラＣ２で仮にタイミングｔ−１で画像を取り込んでいたとしたときの物体２の実際の画像座標（ｕ^２ _ｔ−１，ｖ^２ _ｔ−１）とされる。

［ｓｔｅｐ３］
カメラＣ２で仮にタイミングｔ−１で画像を取り込んでいたとしたときの物体２の実際の画像座標（ｕ^２ _ｔ−１，ｖ^２ _ｔ−１）から、上記した数式２に基づき、世界空間の直線ｌ^２ _ｔ−１を求める。また、カメラＣ１でタイミングｔ−１のときに取り込んだ画像での物体２の画像座標（ｕ^１ _ｔ−１，ｖ^１ _ｔ−１）からも、上記した数式２に基づき、世界空間の直線ｌ^１ _ｔ−１を求める。そして、これら各直線ｌ^１ _ｔ−１、ｌ^２ _ｔ−１の交点を求める。この交点をタイミングｔ−１における物体２の３次元位置Ｐ_ｔ−１とする。

このようなｓｔｅｐ１〜３の処理をタイミングｔ’−１についても行うことで、タイミングｔ’−１における仮の画像座標（ｕ^１＊ _ｔ’−１，ｖ^１＊ _ｔ’−１）が求められ、物体２の３次元位置Ｐ_ｔ−１を求めることができる。これにより、過去２フレームにおける物体２の３次元位置Ｐ_ｔ−１、Ｐ_ｔ’−１を精度良く求めることが可能となる。

以上のような方法に基づき、カメラＣ１、Ｃ２それぞれのシャッタータイミングのたびに、物体２の３次元位置Ｐ_ｔを正確に求めることが可能となる。

続いて、ＰＣ３で行われる物体２の３次元位置Ｐ_ｔの演算処理のフローチャートを図５〜図８に示し、これらの図に基づいて３次元位置Ｐ_ｔの演算処理について説明する。

図５は、ＰＣ３における第１画像取得部３ａで実行されるカメラＣ１の画像データ取得処理を示したフローチャートである。まず、ステップ１１０では、各種データの初期化等を行うための初期化処理が実行される。次に、ステップ１２０にてカメラＣ１からの画像が取得され、ステップ１３０にてそのときのキャプチャ終了時刻を取得される。キャプチャ終了時刻は、例えばＰＣ３内に備えられる図示しないクロック等からの信号から得られる。

ステップ１４０では、取得した画像の色を識別される。続いて、この色の識別に基づいて、ステップ１５０にて対象物となる物体２が画像内のどの位置に存在するかが識別される。そして、さらにステップ１６０にて対象物となる物体２の中心が求められる。この後、ステップ１７０にて対象物となる物体２の中心およびキャプチャ終了時刻、さらには画像を取得したカメラの番号（Ｃ１）が演算部３ｃに送信される。

図６は、ＰＣ３における第２画像取得部３ｂで実行されるカメラＣ２の画像データ取得処理のフローチャートである。この処理は、図５に示した第１画像取得部３ａで実行される処理と同様にして行われるもので、ステップ２２０に示すようにカメラＣ２からの画像を取得すること以外は、図５に示されるフローチャートと同様である。この処理により、カメラＣ２の画像に基づいて物体２の中心が求められ、物体２の中心およびキャプチャ終了時刻、さらには画像を取得したカメラの番号（Ｃ２）が演算部３ｃに送信されるようになっている。

図７は、ＰＣ３における演算部３ｃで実行される３次元位置計測処理を示したフローチャートである。まず、ステップ３１０では、ステップ１１０と同様に初期化処理が実行される。次いで、ステップ２２０では、最新フレームのデータが受信される。この処理で受信されるデータは、図５および図６におけるステップ１７０とステップ２７０から送られるデータを意味している。

そして、データが受信されると、ステップ３３０にて、データの格納処理が実行される。この処理は、例えばＰＣ３に備えられるメモリにデータを記憶しておくことによって実現される。

続く、ステップ３４０では、メモリ内にデータが４つ保持されているか否かが判定される。ここでいう４つのデータとは、例えば、図４に示したタイミングｔ、ｔ’−１、ｔ−１、ｔ’−２の各データというように、最新フレームにおける物体２の３次元位置Ｐ_ｔを求めるのに必要な数のデータということを意味している。

そして、４つのデータが保持されていればステップ３５０以降の処理が実行され、まだ４つのデータが保持されていないようであればステップ３２０に戻って上記処理が繰り返される。

ステップ３５０では、２フレーム前の３次元位置の再計算処理、ステップ３６０では、１フレーム前の３次元位置の再計算処理が実行される。これらの処理は上述した過去数フレームにおける３次元位置の予測方法で示した方法に基づいて実行されるもので、その処理の詳細は図８に示される。以下、図８に基づいて２フレーム前および１フレーム前の３次元位置の再計算処理について説明する。ただし、これら各処理は同様であるため、ここでは説明を簡略化するために、２フレーム前における３次元位置の再計算処理ということで説明する。

まず、図８のステップ４１０では、最新フレームのカメラがどのカメラかが判定される。この処理は、例えば、第１、第２画像取得部３ａ、３ｂから送られてくるキャプチャ終了時刻およびカメラ番号のデータに基づいて判定されるもので、キャプチャ終了時刻が最も遅い（最近）のカメラが最新フレームのカメラと判定される。

そして、カメラＣ１であると判定されれば、ステップ４２０に進んでカメラの番号としてカメラＣ１が選択され、カメラ２であると判定されれば、ステップ４３０に進んでカメラの番頭としてカメラＣ２が選択される。

ステップ４４０では、選択された番号のカメラによる最新フレームよりも１フレーム前の画像と、選択されていない番号のカメラによるその前後のフレーム、つまり過去２フレームの画像が選択される。そして、選択されていない番号のカメラによる２つの画像における物体２の中心のデータから、上述したｓｔｅｐ１に示す数式５〜８に基づいて、選択されていない番号のカメラで仮に最新フレームから１フレーム前の画像を取得したタイミングで画像を取得していたとした場合の仮の画像座標が求められる。

また、ステップ４５０では、選択された番号のカメラによる最新フレームから１サイクル前のフレームでの物体２の中心のデータがメモリから読み出される。そして、上述したｓｔｅｐ２のように、そのフレームと同じタイミングで選択されていない番号のカメラで画像が取得されていた場合を想定して、読み出した物体２の中心のデータから、その仮定した仮の画像内におけるエピポーラ直線が求められる。

そして、ステップ４６０では、ステップ４５０で求められたエピポーラ線上の点のうち、ステップ４４０で求められた仮の画像座標から最短距離のものが求められる。この点が、選択された番号のカメラによる最新フレームから１サイクル前のフレームと同じタイミングで、選択されていない番号のカメラで画像が取得されていたとしたときの物体２の実際の画像座標となる。

この後、ステップ４７０にて、ステップ４６０で求められた実際の画像座標と、選択された番号のカメラによる最新フレームから１サイクル前のフレームでの物体２の中心のデータ、およびカメラＣ１、Ｃ２の世界座標に基づき、上記したｓｔｅｐ３のように、各点を通過する直線が求められ、それらの交点が求められる。この交点が、最新フレームから１サイクル前のフレームにおける物体２の３次元位置となる。

このようにして、２フレーム前の３次元位置の再計算処理が完了する。そして、この処理が完了すると、１フレーム前の３次元位置の再計算処理が実行される。この処理も、図７に示される３次元位置の再計算処理と同様の方法によって実行される。ただし、この場合には、選択されたカメラの最新のフレームとその１サイクル前のフレームとから、選択されていないカメラの最後のフレームの画像が取得されたタイミングにおける物体２の３次元位置を求めることになることが、２フレーム前の処理と異なる。

そして、このようにして２フレーム前と１フレーム前における物体２の３次元位置が求められると、続いて図７のステップ３７０に進む。そして、２フレーム前と１フレーム前における物体２の３次元位置から、線形予測により、最新フレームにおける物体２の第１の予測位置が求められる。

続いて、ステップ３８０にて、選択されたカメラの世界座標と、そのカメラが取得した画像内における物体２の画像座標とを通る光線が求められ、その光線上の点のうち、最もステップ３７０で求められた最新フレームにおける物体２の第１の予測位置から近いものが求められる。この点が最新フレームでの物体２の３次元位置とされる。

以上の処理によって最新フレームでの物体２の３次元位置が求められる。このようにすれば、精度良い過去の複数フレームでの物体２の３次元位置に基づいて最新フレームでの物体２の３次元位置を求めているため、正確な３次元位置を求めることが可能となる。

そして、本実施形態では、カメラＣ１、Ｃ２が異なるタイミングで画像を取り込み、画像が取り込まれた各タイミングで物体２の３次元位置を求めるようにしている。このため、カメラＣ１、Ｃ２のシャッタータイミングを同期させる場合と比べて２倍の数、物体２の３次元位置を取得することが可能となる。このため、カメラＣ１、Ｃ２の単位時間当たりの撮影回数を超える回数の３次元位置計測が行え、より数多くの物体２の３次元位置のデータに基づいてロボットの制御を行うことが可能となり、より適切なロボット制御を実行することが可能となる。

参考として、図１に示した３次元空間１中で移動する物体２の３次元位置を計測する実験を行った。その結果を図９（ａ）、（ｂ）に示す。図９（ａ）は、最新フレームがカメラＣ１である場合、その画像座標（ｕ^１ _ｔ，ｖ^１ _ｔ）と１フレーム前のカメラＣ２の画像座標（ｕ^２ _ｔ’−１，ｖ^２ _ｔ’−１）を用いて、ステレオ視により物体２の３次元位置を求めたものである。また、図９（ｂ）は、上述した本実施形態の手法により、物体２の３次元位置を求めたものである。

これらの実験では、高さ５００ｍｍの箱の上に置かれたターンテーブルとゴルフボールを用いた。長さが１，０００ｍｍのものさしの端にゴルフボールを固定し、ターンテーブルを回転させ、そのときのゴルフボールの軌跡を求めた。このような場合、ゴルフボールが半径５００ｍｍの等速円運動を行う事になる。そして、ターンテーブルの厚みが１５０ｍｍであるとすると、ゴルフボールの高さは６５０ｍｍとなる。また、ターンテーブルの回転速度が４５ｒｐｍであるとすると、ゴルフボールの回転速度は２π・４５／６０＝０．４７８［ｒａｄ／ｓ］となる。

そして、図９（ａ）の実験結果を見てみると、およそ高さ方向において６０ｍｍほど誤差が生じていることが分かる。つまり、この場合には、物体２となるゴルフボールの３次元位置のばらつきが大きく精度が悪いことを表している。これに対し、図９（ｂ）に示した本実施形態の実験結果を見てみると、高さ方向における誤差が約３０ｍｍ以内で収まっていることが分かる。つまり、この場合には物体２となるゴルフボールの３次元位置のばらつきが小さく精度が良いことを表している。この誤差は、上記図１に示される３次元空間１で区画された広い領域内で生じる誤差であるため、非常に小さいといえる。

このように、実験結果からも、本実施形態のような手法によって物体２の３次元位置を求めることにより、より正確な３次元位置と求めることが可能になると言える。

（第２実施形態）
上記第１実施形態では、図２（ｂ）に示すように、カメラＣ１、Ｃ２のシャッタータイミングのずれが各カメラＣ１、Ｃ２の画像取り込み間隔の半分となるようにする場合について説明した。しかしながら、第１実施形態に示したカメラＣ１、Ｃ２のシャッタータイミングは単なる例示であり、その他のシャッタータイミングであっても構わない。

この場合、シャッタータイミングのズレδ１２、δ２１は、ＰＣ３にて、カメラＣ１の画像取り込みの終了タイミング（キャプチャ終了時刻）からカメラＣ２の画像取り込みの終了タイミングまでの時間、および、カメラＣ２の画像取り込みの終了タイミングからカメラＣ１の画像取り込み終了タイミングまでの時間をそれぞれ演算することにより求められる。

そして、上述した数式５、６による内挿演算によって、２次元座標（ｕ^２ _ｔ−１，ｖ^２ _ｔ−１）を求めることが可能である。

このように、カメラＣ１、Ｃ２が画像取り込みできないタイミングに仮に画像取り込みを行ったと想定した場合の物体２の２次元座標を求めることができる。したがって、カメラＣ１、Ｃ２のシャッタータイミングのずれが各カメラＣ１、Ｃ２の画像取り込み間隔の半分以外であったとしても、各カメラＣ１、Ｃ２の画像取り込みの機能限界以上の回数、物体２の３次元位置計測を行うことができる。これにより、第１実施形態と同様の効果を得ることができる。

そして、このようにカメラＣ１、Ｃ２のシャッタータイミングが任意のものであっても良い場合、カメラＣ１、Ｃ２のシャッタータイミングを設定するための同期機が必要とされないため、システム構成の簡略化が可能という効果も得られる。

（他の実施形態）
上記各実施形態では、過去複数フレームでの３次元位置を再計算するために、上述した（ａ）、（ｂ）の手法のうちの（ｂ）の手法を採用している。しかしながら、（ａ）の手法を用いることも可能である。ただし、上述したように、（ｂ）の手法を用いた方が、精度良く３次元位置の再計算を行うことができる。

また、上記各実施形態において、過去複数フレームでの３次元位置からの線形予測によって最新フレームでの３次元位置を求めている。これは、各フレームの画像の取得タイミングの間隔が１／３０秒以下という短い間隔だからであり、例えば、拡張カルマンフィルタを用いたり、３次元曲線を用いたりする等、他の予測法を用いることも可能である。

さらに、上記各実施形態では、第１、第２のカメラＣ１、Ｃ２の２つのカメラを用いる例を示したが、少なくとも２つのカメラがあれば上記効果を得ることが可能であるという意味であり、３つ以上のカメラを用いることも可能である。

なお、各図中に示したステップは、各種処理を実行する手段に対応するものである。

本発明の第１実施形態における３次元位置計測システムの概略構成を示す図である。 ３次元位置計測の概略を示した模式図である。 カメラＣ１、Ｃ２のシャッタータイミングを示したタイミングチャートである。 各カメラＣ１、Ｃ２で取り込んだ画像、および、カメラＣ１、Ｃ２がお互いのシャッタータイミングでないときに仮に画像を取り込んでいたとしたら得られたであろう画像を時系列的に示した図である。 カメラＣ１の画像データ取得処理を示したフローチャートである。 カメラＣ２の画像データ取得処理を示したフローチャートである。 ３次元位置計測処理を示したフローチャートである。 ２フレーム前および１フレーム前の３次元位置の再計算処理のフローチャートである。 ３次元空間中で移動する物体の３次元位置を計測した実験結果を示したグラフである。 従来の３次元位置の計測手法を示した概略図である。

符号の説明

１…３次元空間、２…物体、３…ＰＣ、３ａ…第１画像取得部、３ｂ…第２画像取得部、３ｃ…演算部、４…ケーブル、Ｃ１、Ｃ２…カメラ。

Claims (6)

1. 同じサイクルかつ互いに異なるタイミングで画像を取り込む第１、第２のカメラ（Ｃ１、Ｃ２）と、
   前記第１のカメラ（Ｃ１）が取り込んだ画像を取得し、その画像に存在する物体（２）の前記第１のカメラ（Ｃ１）の画像上での座標となる画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ））を求める第１画像取得部（３ａ）と、
   前記第２のカメラ（Ｃ２）が取り込んだ画像を取得し、その画像に存在する物体（２）の前記第２のカメラ（Ｃ２）の画像上での座標となる画像座標（（ｕ^２ _ｔ,ｖ^２ _ｔ））を求める第２画像取得部（３ｂ）と、
   前記第１、第２画像取得部（３ａ、３ｂ）で求められた画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ）、（ｕ^２ _ｔ,ｖ^２ _ｔ））に基づいて、前記物体（２）の３次元位置を求める演算部（３ｃ）とを有してなる３次元位置計測システムであって、
   前記演算部（３ｃ）は、
   前記第１、第２のカメラ（Ｃ１、Ｃ２）から最新フレームの画像を取り込んだ方のカメラを選択し、選択されたカメラ（Ｃ１）において前記最新フレームの画像が取り込まれたタイミング（ｔ）の１サイクル前のタイミング（ｔ−１）を２フレーム前のタイミングとして、そのタイミング（ｔ−１）での前記物体（２）の３次元位置（Ｐ_ｔ−１）を２フレーム前の３次元位置とし、
   前記選択されたカメラ（Ｃ１）とは異なる選択されていないカメラ（Ｃ２）により、前記最新フレームが取り込まれたタイミング（ｔ）とその１サイクル前のタイミング（ｔ−１）との間に画像が取り込まれたときのタイミング（ｔ’−１）を１フレーム前のタイミングとして、そのタイミング（ｔ’−１）での前記物体（２）の３次元位置（Ｐ_ｔ’−１）を１フレーム前の３次元位置とし、
   前記２フレーム前および前記１フレーム前の３次元位置（Ｐ_ｔ−１、Ｐ_ｔ’−１）から最新フレームの画像が取り込まれたタイミング（ｔ）での前記物体（２）の仮の３次元位置となる第１の予測位置（Ｐ^＊ _ｔ）を求めると共に、
   前記選択されたカメラ（Ｃ１）の位置と該選択されたカメラ（Ｃ１）からの画像に基づいて、前記第１画像取得部（３ａ）で求められた前記画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ））とを通過する直線（ｌ^１ _ｔ）を求め、
   前記直線（ｌ^１ _ｔ）上の点のうち、前記第１の予測位置（Ｐ^＊ _ｔ）から最短距離の点を、前記物体（２）の３次元位置とするようになっていることを特徴とする３次元位置計測システム。
2. 前記２フレーム前の３次元位置は、前記選択されていないカメラ（Ｃ２）が前記２フレーム前のタイミング（ｔ−１）で画像の取り込みを行ったと想定した場合に、そのタイミング（ｔ−１）における前記選択されていないカメラ（Ｃ２）の画像上での前記画像座標（（ｕ^２＊ _ｔ−１,ｖ^２＊ _ｔ−１））を求め、その画像座標（（ｕ^２＊ _ｔ−１,ｖ^２＊ _ｔ−１））と、このタイミング（ｔ−１）において前記選択されたカメラ（Ｃ１）の画像に基づいて前記第１画像取得部（３ａ）で求められた前記画像座標（（ｕ^１ _ｔ−１,ｖ^１ _ｔ−１））とから求められるようになっており、
   前記１フレーム前の３次元位置は、前記選択されたカメラ（Ｃ１）が前記１フレーム前のタイミング（ｔ’−１）で画像の取り込みを行ったと想定した場合に、そのタイミング（ｔ’−１）における前記選択されたカメラ（Ｃ１）の画像上での前記画像座標（（ｕ^１＊ _ｔ’−１,ｖ^１＊ _ｔ’−１））を求め、その画像座標（（ｕ^１＊ _ｔ’−１,ｖ^１＊ _ｔ’−１））と、このタイミング（ｔ’−１）において前記選択されていないカメラ（Ｃ２）の画像に基づいて前記第２画像取得部（３ｂ）で求められた前記画像座標（（ｕ^２ _ｔ’−１,ｖ^２ _ｔ’−１））とから求められるようになっていることを特徴とする請求項１に記載の３次元位置計測システム。
3. 前記２フレーム前のタイミング（ｔ−１）における前記選択されていないカメラ（Ｃ２）の画像上での前記画像座標（（ｕ^２＊ _ｔ−１,ｖ^２＊ _ｔ−１））は、前記１フレーム前のタイミング（ｔ’−１）に前記選択されていないカメラ（Ｃ２）が取り込んだ画像に基づいて前記第２画像取得部（３ｂ）で求められた前記画像座標（（ｕ^２ _ｔ’−１,ｖ^２ _ｔ’−１））と、そのタイミング（ｔ’−１）よりも１サイクル前のタイミング（ｔ’−２）に前記選択されていないカメラ（Ｃ２）が取り込んだ画像に基づいて前記第２画像取得部（３ｂ）で求められた前記画像座標（（ｕ^２ _ｔ’−２,ｖ^２ _ｔ’−２））とを、これら各タイミング（ｔ’−１、ｔ’−２）と前記２フレーム前のタイミング（ｔ−１）との時間ずれ（δ１２、δ２１）に基づいて内挿演算することによって求められ、
   前記１フレーム前のタイミング（ｔ’−１）における前記選択されたカメラ（Ｃ１）の画像上での前記画像座標（（ｕ^１＊ _ｔ’−１,ｖ^１＊ _ｔ’−１））は、前記最新フレームの画像が取り込まれたタイミング（ｔ）にその画像に基づいて前記第１画像取得部（３ａ）で求められた前記画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ））と、そのタイミング（ｔ）よりも１サイクル前のタイミング（ｔ−１）に前記選択されたカメラ（Ｃ１）が取り込んだ画像に基づいて前記第１画像取得部（３ａ）で求められた前記画像座標（（ｕ^１ _ｔ−１,ｖ^１ _ｔ−１））とを、これら各タイミング（ｔ、ｔ−１）と前記２フレーム前のタイミング（ｔ’−１）との時間ずれ（δ１２、δ２１）に基づいて内挿演算することによって求められるようになっていることを特徴とする請求項２に記載の３次元位置計測システム。
4. 第１、第２のカメラ（Ｃ１、Ｃ２）により、同じサイクルかつ互いに異なるタイミングで画像を取り込み、
   前記第１のカメラ（Ｃ１）が取り込んだ画像に基づき、その画像に存在する物体（２）の該画像上での座標となる画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ））を求めると共に、
   前記第２のカメラ（Ｃ２）が取り込んだ画像に基づき、その画像に存在する物体（２）の該画像上での座標となる画像座標（（ｕ^２ _ｔ,ｖ^２ _ｔ））を求め、
   前記第１、第２のカメラ（Ｃ１、Ｃ２）それぞれの画像から求められた画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ）、（ｕ^２ _ｔ,ｖ^２ _ｔ））に基づいて、前記物体（２）の３次元位置を求める３次元位置計測方法であって、
   前記第１、第２のカメラ（Ｃ１、Ｃ２）から最新フレームの画像を取り込んだ方のカメラを選択し、選択されたカメラ（Ｃ１）において前記最新フレームの画像が取り込まれたタイミング（ｔ）の１サイクル前のタイミング（ｔ−１）を２フレーム前のタイミングとして、そのタイミング（ｔ−１）での前記物体（２）の３次元位置（Ｐ_ｔ−１）を２フレーム前の３次元位置とし、
   前記選択されたカメラ（Ｃ１）とは異なる選択されていないカメラ（Ｃ２）により、前記最新フレームが取り込まれたタイミング（ｔ）とその１サイクル前のタイミング（ｔ−１）との間に画像が取り込まれたときのタイミング（ｔ’−１）を１フレーム前のタイミングとして、そのタイミング（ｔ’−１）での前記物体（２）の３次元位置（Ｐ_ｔ’−１）を１フレーム前の３次元位置とし、
   前記２フレーム前および前記１フレーム前の３次元位置（Ｐ_ｔ−１、Ｐ_ｔ’−１）から最新フレームの画像が取り込まれたタイミング（ｔ）での前記物体（２）の仮の３次元位置となる第１の予測位置（Ｐ^＊ _ｔ）を求めると共に、
   前記選択されたカメラ（Ｃ１）の位置と該選択されたカメラ（Ｃ１）からの画像に基づいて、前記第１画像取得部（３ａ）で求められた前記画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ））とを通過する直線（ｌ^１ _ｔ）を求め、
   前記直線（ｌ^１ _ｔ）上の点のうち、前記第１の予測位置（Ｐ^＊ _ｔ）から最短距離の点を、前記物体（２）の３次元位置とすることを特徴とする３次元位置計測方法。
5. 前記選択されていないカメラ（Ｃ２）が前記２フレーム前のタイミング（ｔ−１）で画像の取り込みを行ったと想定した場合に、そのタイミング（ｔ−１）における前記選択されていないカメラ（Ｃ２）の画像上での前記画像座標（（ｕ^２＊ _ｔ−１,ｖ^２＊ _ｔ−１））を求め、その画像座標（（ｕ^２＊ _ｔ−１,ｖ^２＊ _ｔ−１））と、このタイミング（ｔ−１）において前記選択されたカメラ（Ｃ１）の画像に基づいて求められた前記画像座標（（ｕ^１ _ｔ−１,ｖ^１ _ｔ−１））とから前記２フレーム前の３次元位置を求め、
   前記選択されたカメラ（Ｃ１）が前記１フレーム前のタイミング（ｔ’−１）で画像の取り込みを行ったと想定した場合に、そのタイミング（ｔ’−１）における前記選択されたカメラ（Ｃ１）の画像上での前記画像座標（（ｕ^１＊ _ｔ’−１,ｖ^１＊ _ｔ’−１））を求め、その画像座標（（ｕ^１＊ _ｔ’−１,ｖ^１＊ _ｔ’−１））と、このタイミング（ｔ’−１）において前記選択されていないカメラ（Ｃ２）の画像に基づいて求められた前記画像座標（（ｕ^２ _ｔ’−１,ｖ^２ _ｔ’−１））とから前記１フレーム前の３次元位置を求めることを特徴とする請求項３に記載の３次元位置計測方法。
6. 前記１フレーム前のタイミング（ｔ’−１）に前記選択されていないカメラ（Ｃ２）が取り込んだ画像に基づいて前記第２画像取得部（３ｂ）で求められた前記画像座標（（ｕ^２ _ｔ’−１,ｖ^２ _ｔ’−１））と、そのタイミング（ｔ’−１）よりも１サイクル前のタイミング（ｔ’−２）に前記選択されていないカメラ（Ｃ２）が取り込んだ画像に基づいて前記第２画像取得部（３ｂ）で求められた前記画像座標（（ｕ^２ _ｔ’−２,ｖ^２ _ｔ’−２））とを、これら各タイミング（ｔ’−１、ｔ’−２）と前記２フレーム前のタイミング（ｔ−１）との時間ずれ（δ１２、δ２１）に基づいて内挿演算することによって、前記２フレーム前のタイミング（ｔ−１）における前記選択されていないカメラ（Ｃ２）の画像上での前記画像座標（（ｕ^２＊ _ｔ−１,ｖ^２＊ _ｔ−１））を求め、
   前記最新フレームの画像が取り込まれたタイミング（ｔ）にその画像に基づいて前記第１画像取得部（３ａ）で求められた前記画像座標（（ｕ^１ _ｔ,ｖ^１ _ｔ））と、そのタイミング（ｔ）よりも１サイクル前のタイミング（ｔ−１）に前記選択されたカメラ（Ｃ１）が取り込んだ画像に基づいて前記第１画像取得部（３ａ）で求められた前記画像座標（（ｕ^１ _ｔ−１,ｖ^１ _ｔ−１））とを、これら各タイミング（ｔ、ｔ−１）と前記２フレーム前のタイミング（ｔ’−１）との時間ずれ（δ１２、δ２１）に基づいて内挿演算することによって、前記１フレーム前のタイミング（ｔ’−１）における前記選択されたカメラ（Ｃ１）の画像上での前記画像座標（（ｕ^１＊ _ｔ’−１,ｖ^１＊ _ｔ’−１））を求めることを特徴とする請求項５に記載の３次元位置計測方法。

Images