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

Description

本発明は、撮影画像から任意物体の３次元位置を計測する３次元位置計測システム、３次元位置計測装置および３次元位置計測方法に関する。

車両のボディ部に取り付けたカメラで映像を撮影して、車両周辺の物体の３次元位置を計測する装置が開発されている。カメラで撮影した映像から３次元位置を計測する方法としては、２台のカメラを設置した２眼ステレオ視を用いる方法や、１台のカメラを設置して単眼移動ステレオ視を用いる方法などがある。

前者は、空間的に異なる位置に設置した複数台のカメラで撮影した画像群のそれぞれから、物体上の同じ位置を特定することで、映像中の位置の違い（視差）を算出して、三角測量の原理で３次元位置を定める方法である。また後者は、１台のカメラの時系列映像を用いて、複数の画像から物体の特徴的な部分（特徴点）を追跡して、映像に映る物体の３次元位置を算出するものである。

３次元位置を計測する従来技術として、２眼ステレオ視を用いた技術が例えば、特許文献１に提案されている。また、単眼移動ステレオ視を用いた技術が例えば、特許文献２に提案されている。

特開平５−１１４０９９号公報 特開２００７−２５６０２９号公報

２眼ステレオ視方式および単眼移動ステレオ視方式のいずれも、カメラの光軸原点を３次元的に結ぶ直線（基線）と平行な方向をなす対象物については、原理上距離を計測することができない。

ステレオ測距方式は、一方のカメラ映像の着目位置に対して、他方カメラ映像中で、着目位置と同じ映像パターンを持つ箇所を探索によって定めて、２つのカメラ間で対応する位置を使って三角測量の原理で距離を計測する方法を採る。

このとき、２つのカメラで対応する箇所の特徴点同士は、幾何学的な関係から画像中では基線と平行な位置関係に限定される。したがって、測距したい対象の模様が、基線と平行な成分しか持たない場合は、基線と平行な方向に探索しても映像パターンに変化がないため一意に対応する特徴点を求めることができない。その結果、誤った測距結果となるか、一意に対応が求まらないため測距できないことになる。

また、単眼移動ステレオ視では、カメラの移動で定められる２箇所のカメラ位置を２眼ステレオと考えれば、上述の２眼ステレオ視と同様の問題点を有しており、カメラが取り付けられる移動体が、例えば車両のように平面である路面上を移動することを考えれば、路面と平行な方向の対象については距離を計測することができない。

本発明はこのような点に鑑みてなされたものであり、対象物の模様の方向に寄らず、常に安定した距離計測を実現する３次元位置計測システムを提供することを目的とする。
また、本発明の他の目的は、対象物の模様の方向に寄らず、常に安定した距離計測を実現する３次元位置計測装置を提供することである。

さらに、本発明の他の目的は、対象物の模様の方向に寄らず、常に安定した距離計測を実現する３次元位置計測方法を提供することである。

上記課題を解決するために、３次元位置計測システムが提供される。３次元位置計測システムは、移動体の移動に伴い回転する物体に取り付けられたカメラと、前記カメラで撮影された複数の画像と、該複数の画像を各々撮影した時刻と同時刻のカメラ回転位置とを対応づけて記憶する記憶部と、基線方向と平行な成分を持つ対象物の模様に対して、前記複数の画像から、前記平行な成分の方向とは異なる方向に基線方向を与える前記カメラ回転位置の少なくとも２つの画像を選択して、前記対象物の３次元位置を計測する３次元位置計測部とを備える３次元位置計測装置とを有する。

対象物の模様の方向に寄らずに安定した距離計測を行うことが可能になる。

３次元位置計測システムの構成例を示す図である。 ３次元位置計測システムが適用される移動体を示す図である。 移動体に対するカメラの設置位置を示す図である。 カメラの移動軌跡上で単眼移動ステレオ視に使用するカメラ位置を示す図である。 ３次元位置計測装置の適用例を示す図である。 ３次元位置計測装置の構成例を示す図である。 入力映像の回転変換を説明する図である。 測距処理部の動作を示すフローチャートである。 小領域の分割を示す図である。 エピ極線を説明する図である。 ２つの映像間での対応探索箇所を示す図である。 ３次元位置計測装置の適用例を示す図である。 ３次元位置計測装置の構成例を示す図である。 カメラ回転位置算出部の動作を示すフローチャートである。 回転補正を示す図である。 端点の移動軌跡を示す図である。

以下、本発明の実施の形態を図面を参照して説明する。図１は３次元位置計測システムの構成例を示す図であり、図２は３次元位置計測システムが適用される移動体を示す図である。３次元位置計測システム１は、カメラ１ａと３次元位置計測装置１０を備える。３次元位置計測装置１０は、記憶部１４と３次元位置計測部１０ａを備える。

カメラ１ａは、移動体２の移動に伴い回転する物体（回転体）２ａに取り付けられる。記憶部１４は、カメラ１ａで撮影された画像の情報と、撮影時と同時刻のカメラ回転位置の情報とを対応づけて記憶する。

３次元位置計測部１０ａは、対象物の模様に対して、模様の方向とは異なる方向に基線方向を与えるカメラ回転位置の画像群から、少なくとも２つの画像を選択して、対象物の３次元位置を計測する。

次に３次元位置計測について説明する。なお、以降の説明では、画像を映像と表記する。本技術で述べる映像は、動画（複数の画像からなる一連のイメージ）ではなく、光線の屈折または反射によって作られた像、または画面に映し出された画像に該当する。

図３は移動体２に対するカメラ１ａの設置位置を示す図である。移動体２が移動に伴って回転する回転体２ａを持ち、回転体２ａにカメラ１ａを外側に向けて取り付ける。回転体２ａとしては例えば、車輪が該当するので、以降では車輪２ａとして説明する。

カメラ位置は、車輪２ａの回転にしたがって、上下変動を伴い横方向へ進みながら変化する。この場合、測距したい対象物の模様の方向に対して、上下方向に揺動しながら進むカメラ軌跡から、模様の向きとは異なる方向に基線方向を取るような適切な２つのカメラ１ａの位置を選択する単眼移動ステレオ視を行う。これにより、対象物の模様の方向に寄らず、常に安定した距離計測を実現するものである。

図４はカメラ１ａの移動軌跡上で単眼移動ステレオ視に使用するカメラ位置を示す図である。例として、直径１ｍの車輪２ａの外枠位置にカメラ１ａを取り付けたとする。この場合に、移動体２が路面のような水平面を直線状に移動した際のカメラ１ａの位置として、水平方向の位置を横軸にとり、垂直方向の位置を縦軸にとる。

カメラ１ａは、垂直方向（上下方向）を揺動しながら、水平方向に移動する軌跡を描く。ここで、カメラ１ａは移動しながら、過去一定時間内の映像データを蓄積していくものとする。

いま、ある時刻ｔで着目時点ｐ０にカメラが位置し、このとき、距離の計測対象の模様の向きが水平方向となっており、移動体２の水平移動に伴ってカメラ１ａも水平に移動するものとする。

この場合、通常行われる移動体２に取り付けたカメラ１ａによる単眼移動ステレオ視では、カメラ間を結ぶ基線の向きと計測対象の模様の向きとが同じ方向となってしまうので、距離計測を行うことができない。

これに対して、本技術では、例えば、参照時点ｐ１の位置のカメラ映像を、単眼移動ステレオ視のもう１つのカメラ位置に選ぶ。これにより、着目時点ｐ０と参照時点ｐ１とのカメラ位置で決まる基線は、基線ｂ１となる。基線ｂ１の方向は、水平でないため、水平方向の模様に対して、２つのカメラ映像によって距離を決定することが可能になる。

ただし、参照時点ｐ１を選ぶことで、基線ｂ１と平行な成分しか持たない対象物については、距離計測は不可となる。したがって、このような対象物については、参照時点ｐ１とは異なるタイミングのカメラ映像をさらに選択して、基線の方向を対象物の模様の向きとは異なる方向に設定する。

例えば、参照時点ｐ２を選んで、着目時点ｐ０と参照時点ｐ２とで決まる基線ｂ２を生成する。これにより、基線ｂ１と平行な模様の対象物についても距離を計測することが可能になる。

このように、距離測定対象の模様方向によって、揺動移動するカメラ軌跡の状態から、対象物の模様と平行にならない基線を与えるカメラ位置を適切に選択する。これにより、対象物の模様の方向に寄らず、２つのカメラ映像間で対象部分の映像パターンによる対応を確実に行えるようにして、安定した距離の計測を実現する。

次に対象物の模様の方向に対する最適な参照時刻の映像決定の方法について説明する。過去のカメラ移動で撮影した映像列の集合をＰ＝｛ｐｋ｜ｋ∈｛０，１，２，３，・・・，Ｎ｝｝（Ｎは最大蓄積数でｋ＝０は現時刻での撮影時点を表す）とする。

また、各位置で撮影したときのカメラ１ａの水平位置の集合をＭｈ＝｛ｈｋ｜ｋ∈｛０，１，２，３，・・・，Ｎ｝｝とし、垂直位置の集合をＭｖ＝｛ｖｋ｜ｋ∈｛０，１，２，３，・・・，Ｎ｝｝とする。

このとき、あるｋ^*∈｛０，１，２，３，・・・，Ｎ｝に着目すると、基線長さｂ_k*は、式（１）で与えられる。

また、基線方向の水平面からの傾き角度は、式（２）で与えられる。
θ_k*＝arctan（（ｖ_k*−ｖ₀）／（ｈ_k*−ｈ₀）） ・・・（２）
これらは集合のインデックスｋの各々について定義されるので、これらも集合となり、以下のように定義する。

基線長の集合Ｂを、Ｂ＝｛ｂｋ｜ｋ∈｛０，１，２，３，・・・，Ｎ｝｝とし、基線の傾きの集合Ｔを、Ｔ＝｛θｋ｜ｋ∈｛０，１，２，３，・・・，Ｎ｝｝とする。
いま、現時点で着目対象の模様の方向をθｍとするとき、過去の撮影位置ｋ∈｛１，２，３，・・・，Ｎ｝の各々について、集合Ｂ、集合ＴからスコアＳｃ^k（Ｂ，Ｔ，θｍ）を算出し、

式（３）から最大のＳｃ^kを与えるｋｍａｘを定めることで、着目対象に対する最適な撮影位置ｐ_kmax、ｖ_kmax、ｈ_kmaxを決定する（なお、（argmax（f（x））は、f（x）を最大にするxを意味する）。

ここで、Ｓｃ^k（Ｂ，Ｔ，θｍ）は、基線長に関する評価値Ｓ₁ ^k（Ｂ）および基線方向に関する評価値Ｓ₂ ^k（Ｔ，θｍ）によって、式（４）で与えられる。
Ｓｃ^k（Ｂ，Ｔ，θｍ）＝Ｓ₁ ^k（Ｂ）×Ｓ₂ ^k（Ｔ、θｍ） ・・・（４）
次に基線長に関する評価値Ｓ₁ ^k（Ｂ）について説明する。ステレオ視による距離計測では、距離計測の基礎式は以下の式（５）で与えられる。

Ｚ＝ｆ・ｂ／ｄ ・・・（５）
ｆはカメラの焦点距離［画素］、ｂは基線長［ｍ］、ｄは画像上での対応点のずれ量（視差）［画素］、Ｚは対象物までの距離［ｍ］を表す。このとき、視差が対応誤差によって１だけずれたときの距離をＺ₂とすると、Ｚ₂は以下の式（６）で与えられる。

Ｚ₂＝ｆ・ｂ／（ｄ＋１） ・・・（６）
したがって、視差値で１の誤差による計測距離の誤差ΔＺは、式（７）で与えられる。
ΔＺ（ｂ）＝Ｚ−Ｚ₂＝（ｆ・ｂ／ｄ）−（ｆ・ｂ／（ｄ＋１））
＝ｆ・ｂ・（（１／ｄ）−（１／（ｄ＋１）））
＝ｆ・ｂ・（１／ｄ・（ｄ＋１））＝Ｚ・（１／（ｄ＋１））
＝Ｚ・（１／（（ｆ・ｂ／Ｚ）＋１））
＝Ｚ²／（ｆ・ｂ＋Ｚ） ・・・（７）
ΔＺ（ｂ）は、距離Ｚにある対象を基線長ｂでステレオ測距した場合の計測誤差を与えるもので、ΔＺ（ｂ）が大きいほど計測誤差が大きく、精度が低いことを表す。

式（７）から、基線長ｂを大きくすれば、それだけ計測誤差が減少する性質を持つことがわかる。ここで、基線長ｂを変化させた場合の測距精度の向上度合いｓ₁（ｂ）を以下のように定義する。

ｓ₁（ｂ）＝ΔＺ（ｂ₀）／ΔＺ（ｂ）＝（ｆ・ｂ＋Ｚ）／（ｆ・ｂ₀＋Ｚ）
・・・（８）
式（８）において、ｂ₀は基準となる基線長を表し、ここではｂ₀＝１（ｍ）とする。また、対象距離Ｚは、基準距離を設定し、ここではＺ＝１（ｍ）とする。ｓ₁（ｂ）は、基線長をｂとした場合の基準基線長ｂ₀に対する精度の改善比（無次元量）を表し、ｓ₁（ｂ）が大きいほど測距精度が高くなることを意味する。

このｓ₁（ｂ）を用いて、集合Ｂの要素｛ｂ_k｝に対して、以下の式（９）を定める。
Ｓ₁ ^k（Ｂ）＝ｓ₁（ｂ_k） ・・・（９）
次に基線方向に関する評価値Ｓ₂ ^k（Ｔ，θｍ）について説明する。集合Ｔの各要素｛θ_k｝と着目点の模様の方向θｍを用いて、模様方向θｍと直交する方向となるにつれ大きな値を取る関係式ｓ₂（θ_k，θ_m）を以下の式（１０）のように導入する。

ｓ₂（θ_k，θ_m）＝｜sin（θ_k−θ_m）｜ ・・・（１０）
これを用いて、集合Ｔの要素｛θ_k｝に対するＳ₂ ^k（Ｔ，θｍ）を以下の式（１１）で定義する。

Ｓ₂ ^k（Ｔ，θｍ）＝ｓ₂（θ_k，θ_m） ・・・（１１）
次に３次元位置計測装置１０の構成および動作について詳しく説明する。図５は３次元位置計測装置の適用例を示す図である。移動体２が車輪２ａの移動に伴い回転する部分を持ち、その回転面で少なくとも移動体２の外側を視野内に持つようにカメラ１ａを取り付ける。

そして、移動体２の移動に伴い、カメラ１ａ自体が回転と上下動を行いながら移動体２と共に移動する。また、車輪２ａの回転位置は、移動体２に取り付けた車輪回転位置計測センサ２１によって計測される。

図６は３次元位置計測装置の構成例を示す図である。３次元位置計測装置１０−１は、映像入力部１１、車輪回転位置計測部１２、映像・カメラ回転位置蓄積部１３、映像・カメラ回転位置保存ＤＢ（データベース）１４ａおよび測距処理部１５を備える。

なお、映像入力部１１、車輪回転位置計測部１２、映像・カメラ回転位置蓄積部１３および測距処理部１５の各機能は、図１に示した３次元位置計測部１０ａに含まれる。また、映像・カメラ回転位置保存ＤＢ１４ａは、図１に示した記憶部１４に該当する。

映像入力部１１は、カメラ１ａで撮影された入力映像に対し、入力映像がアナログ情報であるならば、配列要素に映像の明るさや色情報を持つデジタルデータに変換する。また、入力映像がデジタル情報ならば、必要に応じて処理に適した形態のデジタルデータに変換する。変換後の映像デジタル情報は、映像・カメラ回転位置蓄積部１３へ送信される。

車輪回転位置計測センサ２１は、車輪の回転位置情報を計測し、車輪回転位置をデジタルデータに変換する。変換後のデジタルデータは、映像・カメラ回転位置蓄積部１３へ送信される。

映像・カメラ回転位置蓄積部１３は、入力された車輪回転位置からカメラ回転位置を算出する。そして、映像・カメラ回転位置蓄積部１３は、入力された映像デジタル情報と、同時刻のカメラ回転位置とを対応付けて、映像・カメラ回転位置保存ＤＢ１４ａに新たに保存すると共に、システムで定めた最大蓄積フレーム数Ｎより過去の情報を、蓄積情報から削除する。なお、同時刻とは、必ずしも一致する必要はない。例えば、映像・カメラ回転位置蓄積部１３は、カメラで画像を取得したタイミングと最も近い時刻で車輪回転位置計測センサ２１が取得した車輪回転位置を、同時刻のカメラ回転位置とみなす。また、現時刻に撮影した映像デジタル情報と、現時刻のカメラ回転位置とは、測距処理部１５へ送信される。

測距処理部１５は、入力された現時刻に撮影した映像デジタル情報と、現時刻のカメラ回転位置とに加えて、映像・カメラ回転位置保存ＤＢ１４ａから取得した最大Ｎフレームまでの過去の映像デジタル情報およびカメラ回転位置を用いて、過去の映像群から、画像内の各位置で測距に最適な映像を決定する。そして、現時刻の映像と、決定した過去映像との組を用いて距離を算出する。算出された距離データは、システム出力として外部へ出力される。

次に映像・カメラ回転位置蓄積部１３について説明する。映像・カメラ回転位置蓄積部１３では、映像が入力されたタイミングで取得した車輪回転位置φに対して、車輪に取り付けたカメラ位置によってあらかじめシステムで決まっているカメラ１ａの基準位置（回転位置）φ_cを用いて、式（１２）の演算を行い、カメラ回転位置γを算出する。

γ＝φ−φ_c ・・・（１２）
ただし、γは以下の式（１３ａ）、（１３ｂ）に示す範囲とする。
γ＝γ＋３６０°（γ＜０°） ・・・（１３ａ）
γ＝γ−３６０°（３６０°≦γ） ・・・（１３ｂ）
なお、カメラ１ａの基準位置は、車輪２ａの回転のどの位置でも良いが、例えば、カメラ１ａが車輪２ａの最下端に位置するときの位置とする。上記のような演算処理を行うことで、カメラ回転位置を効率よく算出することができる。

図７は入力映像の回転変換を説明する図である。図中、斜線部は、回転変換後に映像が抜ける箇所を表している。入力映像ｑ₀に対して、カメラ１ａの回転位置γを用いて、映像の向きをカメラ１ａの基準方向φ_cと一致させた傾きのない映像ｐ₀に変換する。

回転変換後の映像ｐ₀と、カメラ回転量γとは、現フレームでの映像とカメラ回転位置の情報として、映像・カメラ回転位置保存ＤＢ１４ａに蓄積されると共に、後段の測距処理部１５へ出力される。

次に測距処理部１５について説明する。図８は測距処理部１５の動作を示すフローチャートである。
〔Ｓ１〕測距処理部１５は、回転補正映像に微分処理を施す。回転補正後の映像ｐ₀に対して、Sobelフィルタ等の微分処理を施し、各画素について微分値が一定値以下の箇所を削除する閾値処理を施す。その後、残った画素について、画像の水平方向の微分値ｄｘと垂直方向の微分値ｄｙとで定義されるエッジ方向Ｅ_d（ｘ，ｙ）を以下のように算出する。

Ｅ_d（ｘ，ｙ）＝arctan（ｄｙ／ｄｘ） ・・・（１４）
〔Ｓ２〕測距処理部１５は、微分映像を小領域に区分する。図９は小領域の分割を示す図である。微分映像を横ｎｘ個、縦ｎｙ個の小領域に分割する。この結果、全体でＭ＝ｎｘ×ｎｙ個の小領域が形成される。得られたｉ番目の小領域をｗｉとして、小領域の集合Ｗを、Ｗ＝｛ｗｉ｜ｉ∈｛１，２，・・・，Ｍ｝｝とする。

〔Ｓ３〕測距処理部１５は、未処理の小領域から１つを選ぶ。小領域の集合Ｗから、未選択の小領域ｗｉ（ｔ∈｛１，２，・・・，Ｍｒ｝）を選ぶ。ただし、Ｍｒは、小領域の集合Ｗの中で未処理の小領域の個数を表す。

〔Ｓ４〕測距処理部１５は、小領域があるか否かを判断する。未選択の小領域の有無を判定し、既に全ての小領域について処理済みならば測距処理を完了し、未処理の小領域 があれば、次の処理に移る。

〔Ｓ５〕測距処理部１５は、小領域内の微分値から主方向θｍを算出する。小領域ｗｉ内のエッジ方向Ｅｄ（ｘ，ｙ）の平均を式（１５）のように求め、これを主方向θｍと定義する。Ｇは、小領域の画素面積を表す。また、（ｘ，ｙ）∈｛ｐｉｘｅｌ｝は、（ｘ，ｙ）∈｛ｗｉ内の画素位置｝を意味する。

〔Ｓ６〕測距処理部１５は、主方向θｍと、カメラ回転位置集合｛Γ｝を用いて、映像集合｛Ｐ｝から最適映像ｐ_k*とインデックス値ｋ^*を決定する。まず、映像・カメラ回転位置保存ＤＢ１４ａから与えられる、過去のカメラ移動で撮影した映像列の集合をＰ＝｛ｐｋ｜ｋ∈｛１，２，３，・・・，Ｎ｝｝、各々位置で撮影したときのカメラの回転位置の集合をΓ＝｛γｋ｜ｋ∈｛１，２，３，・・・，Ｎ｝｝とする。

また、現フレームでのカメラの回転位置をγ₀とする。このとき、現フレームを起点に、過去のｋフレーム目でのカメラ１ａの相対位置は、カメラ１ａの車輪上での設置位置の車輪半径方向の距離Ｒを用いて、水平位置ｈ_kおよび垂直位置ｖ_kは、以下の式（１６ａ）、（１６ｂ）で与えられる。

ｈ_k＝Ｒ・（sin（γ_k）−sin（γ₀））＋Ｌ・ｃｋ ・・・（１６ａ）
ｖ_k＝Ｒ・（cos（γ_k）−cos（γ₀）） ・・・（１６ｂ）
ただし、ｃｋは現フレームでのカメラ回転位置γ₀から過去ｋフレーム目でのカメラ回転位置γ_kに至るまでの回転の周期数を表す。これは、カメラ回転位置集合Γから、添え字ｋを０，１，２，・・・，ｋまでたどりながら、添え字が隣り合うカメラ回転位置γの値が例えば３００度以上に急激に変化する回数で与えられる。また、Ｌは、カメラ１ａを取り付けた車輪の円周長を表し、Ｌ＝２・π・Ｒで与えられる。

このとき、水平位置の集合をＭｈ＝｛ｈｋ｜ｋ∈｛１，２，３，・・・，Ｎ｝｝、垂直位置の集合をＭｖ＝｛ｖｋ｜ｋ∈｛１，２，３，・・・，Ｎ｝｝とすると、式（１７）、式（１８）で与えられる基線長ｂ_kと基線方向の水平面からの傾き角度θ_kを算出する。

θ_k＝arctan（（ｖ_k−ｖ₀）／（ｈ_k−ｈ₀）） ・・・（１８）
これらの集合として、基線長の集合Ｂ＝｛ｂｋ｜ｋ∈｛１，２，３，・・・，Ｎ｝｝および基線の傾きの集合Ｔ＝｛θｋ｜ｋ∈｛１，２，３，・・・，Ｎ｝｝を算出する。

次に現時点で着目対象の模様の方向をθｍとするとき、過去の撮影位置ｋ∈｛１，２，３，・・・，Ｎ｝の各々について、集合Ｂ、集合ＴからスコアＳｃ^k（Ｂ，Ｔ，θｍ）を算出し、以下の式（１９）として、

最大のＳｃ^kを与えるｋ^*を定めることで、着目対象に対する最適な撮影位置ｐ_k*、ｖ_k*、ｈ_k*を決定する。
ここで、Ｓｃ^k（Ｂ，Ｔ，θ_m）は、基線長に関する評価値Ｓ₁ ^k（Ｂ）および基線方向に関する評価値Ｓ₂ ^k（Ｔ，θ_m）によって、上述の式（４）で与えられる。なお、基線長に関する評価値Ｓ₁ ^k（Ｂ）および基線方向に関する評価値Ｓ₂ ^k（Ｔ，θ_m）の定義については、上述したので説明は省略する。

〔Ｓ７〕測距処理部１５は、小領域内のエッジ点について、現在フレームの画像ｐ₀と最適映像 ｐ_k*とで、映像照合を実施し対応位置を算出する。ここで、現フレーム画像ｐ₀内の小領域ｗｉに含まれるエッジ点ｅ_u（ｕ∈｛小領域ｗｉ内のエッジ点｝）に対して、現在フレームでの回転補正画像ｐ₀、ステップＳ６の処理で求まったインデックス値ｋ^*で与えられる過去映像ｐ_k*と、カメラ位置ｖ_k*、ｈ_k*を用いて、エピ極線Ｌを定める。

図１０はエピ極線を説明する図である。エッジ点ｅ_uの画像内での座標を（ｘ⁰ _u，ｙ⁰ _u）とする。ただし、座標原点は画像中央を取る。このとき、最適映像ｐ_k*におけるエピ極線Ｌは、式（２０）で表される直線となる。

ｈ_k*・（ｙ−ｙ⁰ _u）−ｖ_k*・（ｘ−ｘ⁰ _u）＝０ ・・・（２０）
図１１は２つの映像間での対応探索箇所を示す図である。エッジ点ｅ_u（ｕ∈｛小領域ｗｉ内のエッジ点｝）に対して、そのエッジ点を中心に縦横Ｈ画素の矩形領域を定め、画像ｐ₀での矩形領域内の映像パターンと最も相関の高い映像パターンを有する箇所を、過去映像ｐ_k*内でエピ極線Ｌ上を探索して定める。

この結果、小領域ｗｉに含まれるエッジ点ｅ_uの各々について、２つの映像間での対応位置が求まる。すなわち、エッジ点ｅ_uの画像中での位置を（ｘ⁰ _u，ｙ⁰ _u）とし、映像ｐ_k*で対応する箇所を（ｘ^k* _u，ｙ^k* _u）が定まることになる。

〔Ｓ８〕対応位置を用いて距離を算出する。エッジ点ｅ_uでの２つの映像での対応位置を（ｘ⁰ _u，ｙ⁰ _u）、（ｘ^k* _u，ｙ^k* _u）とするとき、視差ｄ_uは式（２１）で定まる。

また、視差ｄ_uを用いて、エッジ点ｅ_uの距離Ｚ_uは式（２２）で求まる。
Ｚ_u＝ｆ・ｂ^k*／ｄ_u ・・・（２２）
ｆは、カメラの焦点距離（画素）、ｂ^k*は、最適基準で求めた基線長（ｍ）である。

次に変形例の３次元位置計測装置の構成および動作について詳しく説明する。図１２は３次元位置計測装置の適用例を示す図である。移動体２の移動に伴い回転する車輪２ａを持ち、車輪２ａの回転面で少なくとも移動体２の外側を視野内に持つようにカメラ１ａを取り付ける。

変形例では、車輪回転位置計測センサ２１を備えていない構成である。また、カメラ１ａの取り付け時、カメラ１ａが車輪２ａの最下端に位置するときに、外界の水平線がカメラ映像で水平あるいは垂直に映るように調整して取り付けるものとする。すなわち、カメラ１ａが車輪２ａの最下端に位置するときに、カメラの向きは０度、９０度、１８０度、２７０度のいずれかの方向を持って取り付けられる。

図１３は３次元位置計測装置の構成例を示す図である。３次元位置計測装置１０−２は、映像入力部１１、映像・カメラ回転位置蓄積部１３、映像・カメラ回転位置保存ＤＢ１４ｂ、測距処理部１５、カメラ回転位置算出部１６および処理流れ制御部１７を備える。

なお、映像入力部１１、映像・カメラ回転位置蓄積部１３および測距処理部１５の動作は、３次元位置計測装置１０−１の場合と同じである。また、映像入力部１１、映像・カメラ回転位置蓄積部１３、測距処理部１５、カメラ回転位置算出部１６および処理流れ制御部１７の各機能は、図１に示した３次元位置計測部１０ａに含まれる。また、映像・カメラ回転位置保存ＤＢ１４ｂは、図１に示した記憶部１４に該当する。

カメラ１ａで撮影された映像は、映像入力部１１に入力される。映像がアナログ情報であるならば配列要素に映像の明るさや色情報を持つデジタルデータに変換され、映像がデジタル情報ならば必要に応じて処理に適した形態のデジタルデータに変換される。変換後のデータは、映像・カメラ回転位置蓄積部１３とカメラ回転位置算出部１６に送信される。

カメラ回転位置算出部１６では、入力された現フレームでの映像情報と、映像・カメラ回転位置保存ＤＢ１４ｂに保存された過去の映像情報を用いて、前時刻から現時刻までの画像の回転量、すなわちカメラの回転量の増分を算出する。

そして、この増分を前時刻で定まっているカメラ回転量に加えることで、基準位置からのカメラ回転位置を算出する。カメラ回転位置は、映像・カメラ回転位置蓄積部１３に送信される。

なお、基準位置は、車輪２ａの回転において基準となる回転位置を表し、例えばカメラ１ａが最下端に位置する回転位置とするが、処理開始時は不定のため０に初期化しておくものとする。

すなわち、処理開始時は、前時刻の回転量は０である。カメラ回転位置算出部１６では、回転基準位置が定まっているか否かを調べ、定まっていない場合は、入力された現フレームでの映像情報と、映像・カメラ回転位置保存ＤＢ１４ｂに保存された過去の映像情報を用いて、回転基準位置を設定する。

この操作において、蓄積される映像数が少ない等の理由で回転基準位置を定めることができない場合は、回転基準位置が未定である状態を処理流れ制御部１７へ送信する。また、既に回転基準位置が定まっている場合は、回転基準位置が既定である状態を、処理流れ制御部１７に通知する。

映像・カメラ回転位置蓄積部１３は、入力された映像デジタル情報と、同時刻に生じた映像デジタル情報のカメラ回転位置とを対応付けて、映像・カメラ回転位置保存ＤＢ１４ｂに新たに保存すると共に、システムで定めた最大蓄積フレーム数Ｎより過去の情報を、蓄積情報から削除する。また、現時刻に撮影した映像デジタル情報と現時刻のカメラ回転位置は、処理流れ制御部１７へ送信される。

処理流れ制御部１７は、カメラ回転位置算出部１６からの入力によって、次フレーム処理への移行か後段処理への移行かを切り替える。すなわち、入力された状態が、回転基準位置未定の場合は、後段処理を行わず次フレームの映像入力処理に戻る。また、回転基準位置既定の場合は、後段処理である測距処理部１５へ制御を移す。

測距処理部１５は、入力された現時刻に撮影した映像デジタル情報と現時刻のカメラ回転位置に加えて、映像・カメラ回転位置保存ＤＢ１４ｂより取得した最大Ｎフレームまでの過去の映像・カメラ回転位置から、画像内の各位置で測距に最適な映像を各々決定する。そして、現時刻の映像と決定された映像との組を用いて、距離を算出する。算出された距離データは、システム出力として外部へ出力される。

次にカメラ回転位置算出部１６について説明する。図１４はカメラ回転位置算出部１６の動作を示すフローチャートである。
〔Ｓ１１〕カメラ回転位置算出部１６は、前フレームでの蓄積映像と現フレームの入力映像を比較し、映像間の回転量を算出する。具体的には、前フレームと現フレームの映像をSobelフィルタ等の微分操作を行った後、非極大値抑制による細線化とエッジ追跡による線分検出を行い、直線的な要素を抽出する。

次に、前フレームと現フレームのそれぞれで得られた直線要素を対応付ける。このとき、前フレームの映像は、回転基準位置の角度に補正された映像となっているので、現フレームでの映像とはおおよそカメラ回転位置γだけ画像の傾きが異なっている。

そのため、直線要素の対応付けでは、現フレームの映像から得られた直線要素の位置を回転量γで回転し、前フレームの映像と同じ傾きに補正した後、前フレームから得られた直線要素の位置と最も近いものとを対応付ける。

ここで、“最も近い”とは、直線要素のそれぞれの端点間の距離差の合計が最も小さいものと定義できる。対応の求まった直線要素の傾きの差を定め、これを得られた線分の対応組全ての平均を求めることで、前フレームと現フレームの画像間の回転量Δφとする。

〔Ｓ１２〕カメラ回転位置算出部１６は、前フレームの処理で求まっている画像回転量φlastと、ステップＳ１１で求めた前フレームから現フレームまでの差分回転量Δφを用いて、現フレームでの画像回転量φcurを式（２３）で算出する。

φcur＝φlast＋Δφ ・・・（２３）
なお、ここでは前フレームと現フレームとを区別するために、添え字last、curで区別した。また、処理の開始時はφ＝０とする。

〔Ｓ１３〕カメラ回転位置算出部１６は、回転基準位置からのカメラ回転位置を算出するために、回転基準位置をφ_cとするとき、以下の式（２４）でカメラ回転位置γを算出する。

γ＝φcur−φ_c ・・・（２４）
回転基準位置φ_cは、車輪の回転において基準となる回転位置を表し、例えばカメラが最下端に位置する回転位置とするが、処理開始時は不定のため０に初期化しておくものとする。

したがって、処理開始時からしばらくは処理開始時のカメラ位置を原点とした回転位置が算出されることになり、最下端の位置からみると、一定量ずれた回転位置となる。上記のような演算処理を行うことで、カメラ回転位置を効率よく算出することができる。また、このような演算処理でカメラ回転位置を算出することにより、車輪回転位置計測センサが不要となり、装置規模の削減を図ることができる。

〔Ｓ１４〕カメラ回転位置算出部１６は、ステップＳ１３で求めたカメラ回転位置γを外部に出力する。
〔Ｓ１５〕カメラ回転位置算出部１６は、カメラ回転位置算出部１６が管理する回転基準位置φ_cの算出完了フラグを参照し、回転基準位置が決定されているか否かを調べる。既に回転基準位置が決定されている場合は、ステップＳ１９に移る。まだ回転基準位置が決定されていない場合は、ステップＳ１６に移る。

〔Ｓ１６〕カメラ回転位置算出部１６は、最大過去Ｎフレームまでの蓄積映像と現フレームの映像とを用いて、回転基準位置φ_cの決定を試みる。
図１５は回転補正を示す図である。カメラ（映像）が常に水平面に対してφ_c傾いて、車輪２ａ上の回転している状態を示している。ステップＳ１１において現フレーム映像から取得した直線要素の端点位置を、ステップＳ１３で求めたγを用いて回転補正して暫定的に定めた回転基準位置φ_cと同じ傾きを持つ座標系に変換する。さらに、この座標列を映像・カメラ回転位置保存ＤＢ１４ｂに登録する。

この処理を時系列で繰り返すため、過去Ｎフレームまでの蓄積映像についても、同様に直線線素の端点位置が暫定的に定めた回転基準位置φ_cと同じ傾きを持つ座標系に変換されて登録されていることになる。これは図１５に示すように、カメラが路面に対して一定の傾きφ_cをもって回転しているときに同じ端点を観測していることに相当する。

図１６は端点の移動軌跡を示す図である。上記の変換を行った後の直線要素の端点の軌跡を考えると、図１６に示すような軌跡となる。図では、３回転分の端点の軌跡を描いた例である。

これは、図３で示したカメラの揺動を上下反転し、角度φ_cだけ傾けた形状となる。したがって、過去フレームからの端点の軌跡から、節点を結ぶ直線を求め、この直線の水平線からの傾きを算出することで、回転基準位置φ_cを定めることができる。実際には、１つの端点だけだと誤差を含むため、追跡できている端点の軌跡それぞれで傾きを求め、それらの平均値として回転基準位置を定める。

なお、この処理で、追跡フレーム数が少ない場合は、節点を定めることができないので、回転基準位置を定めることはできない。
〔Ｓ１７〕ステップＳ１６によって、回転基準位置φ_cが定められた否かを判定する。定められなかった場合は、ステップＳ２０に移行し、定められた場合は、ステップＳ１８に移る。

〔Ｓ１８〕ステップＳ１６で定めた回転基準位置φ_cを確定するために、カメラ回転位置算出部１６が管理する回転基準位置φ_cの算出完了フラグを立てる。また、これまでの観測では暫定的に定めた回転基準位置に方向を合わせて映像を蓄積しているので、回転基準位置φ_cが定まった後では映像がずれてしまうので、映像・カメラ回転位置保存ＤＢ１４ｂに蓄積している映像とカメラ回転位置のデータを全て破棄する。

〔Ｓ１９〕回転基準位置として既定を示すフラグを外部に出力する。
〔Ｓ２０〕回転基準位置として未定を示すフラグを外部に出力する。
以上説明したように、３次元位置計測システム１によれば、距離の測定対象の模様方向によって、揺動移動するカメラ軌跡の状態から、模様と平行にならない基線を与えるカメラ位置を適切に選択して、対象物との距離を測定する構成とした。これにより、対象物の模様の方向に寄らない２つのカメラ映像間で対象部分の映像パターンによる対応を確実に行うことができ、安定した距離の計測を行うことが可能になる。

以上、実施の形態を例示したが、実施の形態で示した各部の構成は同様の機能を有する他のものに置換することができる。また、他の任意の構成物や工程が付加されてもよい。

１ ３次元位置計測システム
１ａ カメラ
２ 移動体
２ａ 回転体
１０ ３次元位置計測装置
１０ａ ３次元位置計測部
１４ 記憶部

Claims (6)

1. 移動体の移動に伴い回転する物体に取り付けられたカメラと、
   前記カメラで撮影された複数の画像と、該複数の画像を各々撮影した時刻と同時刻のカメラ回転位置とを対応づけて記憶する記憶部と、基線方向と平行な成分を持つ対象物の模様に対して、前記複数の画像から、前記平行な成分の方向とは異なる方向に基線方向を与える前記カメラ回転位置の少なくとも２つの画像を選択して、前記対象物の３次元位置を計測する３次元位置計測部とを備える３次元位置計測装置と、
   を有することを特徴とする３次元位置計測システム。
2. 前記３次元位置計測部は、現フレーム画像と、前記現フレーム画像の前記カメラ回転位置を起点にして過去フレーム画像の前記カメラ回転位置で与えられる基線の長さが所定回転周期中で最も大きく、かつ基線方向が前記平行な成分の方向に対して直交方向に最も近くなる過去フレーム画像と、を選択することを特徴とする請求項１記載の３次元位置計測システム。
3. 前記移動体に回転体位置計測センサが設置し、前記３次元位置計測部は、前記回転体位置計測センサで計測された回転体の回転位置を取得して、前記カメラの基準回転位置と、取得した前記回転位置との差分から前記カメラ回転位置を算出することを特徴とする請求項１記載の３次元位置計測システム。
4. 前記３次元位置計測部は、撮影された画像にもとづき、前フレーム画像と現フレーム画像間の差分画像回転量を求め、前記前フレーム画像で定まっている画像回転量に前記差分画像回転量の増分を加えて前記現フレーム画像の画像回転量を求め、前記カメラの基準回転位置と、前記現フレーム画像の画像回転量との差分から前記カメラ回転位置を算出することを特徴とする請求項１記載の３次元位置計測システム。
5. 移動体の移動に伴い回転する物体に取り付けられたカメラで撮影された画像と、複数の画像を各々撮影した時刻と同時刻のカメラ回転位置とを対応づけて記憶する記憶部と、
   基線方向と平行な成分を持つ対象物の模様に対して、前記複数の画像から、前記平行な成分の方向とは異なる方向に基線方向を与える前記カメラ回転位置の少なくとも２つの画像を選択して、前記対象物の３次元位置を計測する３次元位置計測部と、
   を有することを特徴とする３次元位置計測装置。
6. ３次元位置計測方法において、
   移動体の移動に伴い回転する物体に取り付けられたカメラで撮影された画像と、複数の画像を各々撮影した時刻と同時刻のカメラ回転位置とを対応づけて記憶し、
   基線方向と平行な成分を持つ対象物の模様に対して、前記複数の画像から、前記平行な成分の方向とは異なる方向に基線方向を与える前記カメラ回転位置の少なくとも２つの画像を選択して、前記対象物の３次元位置を計測する、
   ことを特徴とする３次元位置計測方法。
   
   

Images