JP2007078409A - 物体測位装置 - Google Patents

Abstract

【課題】 測位誤差を低減して精度良く対象物を測位する。
【解決手段】 観測部１０２は画像から対象物の情報の尤度関数を求め、推定部１０４は対象物の情報の尤度関数と予測された事前分布に基づき対象物の情報の事後分布を推定し、予測部１０６はカメラの位置情報及び姿勢情報と推定された対象物の情報の事後分布に基づき対象物の情報の事前分布を予測し、検出部１０７は対象物の情報の事後分布に基づき対象物の情報の検出値を算出し、測位部１０８は対象物の情報の検出値とカメラの位置情報及び姿勢情報に基づき対象物の地理座標を算出する。
【選択図】 図１

Description

この発明は映像から特定の物体を検出して測位する物体測位装置に関するものである。

地理情報を収集する従来の物体測位装置として、特許文献１に記載の道路環境情報通知装置があり、地理情報のデータベースを連携させた物体認識手法が開示されている。この装置は、映像から道路標識等の対象物を認識し、車載したステレオカメラ等を用いてカメラから対象物までの相対距離を求め、この相対距離と車載したＧＰＳの測位結果を加算して対象物の地理座標を求める。この求めた対象物の地理座標と、データベースに保存された対象物の地理座標とを統合して、最終的な対象物の認識結果を算出する。このため、日照変化により対象物の見かけが変化したり、遮蔽物等で対象物が隠れてしまうということが原因で、対象物の検出が困難な場合でも、データベースの情報で対象物の検出を補助することが可能である。

また、特許文献１に記載の装置において、対象物を映像から認識する際に認識確信度（認識の結果の確かさを示す指標）を算出し、過去の映像における認識確信度と現在の映像における認識確信度を統合することで検出精度を高める方法についても開示されている。

特開２００４−１７１１５９号公報（段落００１７，００２１）

従来の物体測位装置は以上のように構成されているので、ステレオ立体視やＧＰＳの測位結果に伴う外乱を除去して、正確に対象物を測位する方法については開示されていなかった。

また、従来の物体測位装置において、対象物の種類や大きさ等を推定する方法は開示されていなかった。例えば、映像情報からある道路標識を認識した結果、それが“駐車禁止”の道路標識である確率が３３％、“速度制限”の道路標識である確率が３３％、“進入禁止”の道路標識である確率が３３％、という複数の認識確信度が得られることがある。しかし、従来の物体測位装置では、一つの対象物は一つの認識確信度しか持たないことを前提としており、複数の認識確信度が得られた場合に対象物の種類を決定する方法について開示されていなかった。

このため、ＧＰＳの測位誤差や、ステレオ立体視に伴う測位誤差を低減して対象物を測位する手法が必要とされ、また、例えば、道路標識の種類等を効率良く特定する方法も必要とされている。

この発明は上記のような課題を解決するためになされたもので、ＧＰＳの測位誤差やステレオ立体視に伴う測位誤差を低減して精度良く対象物を測位することができ、対象物の種類や大きさ等の対象物の情報も効率的に求めることができる物体測位装置を得ることを目的とする。

この発明に係る物体測位装置は、映像情報並びにその映像情報に同期したカメラの位置情報及び姿勢情報を記憶するデータベースと、該データベースに記憶されている映像情報から画像を取得する画像取得部と、該画像取得部により取得された画像から対象物の情報の尤度関数を求める観測部と、該観測部により求められた対象物の情報の尤度関数と予測された対象物の情報の確率分布である事前分布に基づき、対象物の情報の確率分布を更新して対象物の情報の事後分布を推定する推定部と、上記データベースに記憶されているカメラの位置情報及び姿勢情報を取得する位置姿勢取得部と、該位置姿勢取得部により取得されたカメラの位置情報及び姿勢情報と上記推定部により推定された対象物の情報の事後分布に基づき、対象物の情報の事前分布を予測する予測部と、上記推定部により推定された対象物の情報の事後分布に基づき対象物の情報の検出値を算出する検出部と、該検出部により算出された対象物の情報の検出値と上記位置姿勢取得部により取得されたカメラの位置情報及び姿勢情報に基づき、対象物の地理座標を算出する測位部とを備えたものである。

この発明により、ＧＰＳの測位誤差やステレオ立体視に伴う測位誤差を低減して精度良く対象物を測位することができ、対象物の種類や大きさ等の対象物の情報も効率的に求めることができるという効果が得られる。

実施の形態１．
以下、この実施の形態１において、「相対座標」とはカメラを中心とした３次元の座標系における座標を指し、「地理座標」とは地理座標系における座標（緯度、経度、標高）を指し、「画像座標」とは２次元の画像上の座標（正規化カメラ座標）を指し、「対象物」とは検出の対象となっている物体を指すものとする。

図１はこの発明の実施の形態１による物体測位装置の構成を示すブロック図である。この物体測位装置は、映像から特定の対象物を検出してその地理座標を算出するもので、データベース１００、画像取得部１０１、観測部１０２、事前分布計算部１０３、推定部１０４、位置姿勢取得部１０５、予測部１０６、検出部１０７、測位部１０８及び表示部１０９を備えている。

図１において、データベース１００は映像情報並びにその映像情報に同期したカメラの位置情報及び姿勢情報を記憶し、画像取得部１０１はデータベース１００の映像情報から画像を取得する。観測部１０２は画像取得部１０１により取得された画像から対象物の情報の尤度関数を求め、事前分布計算部１０３は対象物の事前情報を反映した確率分布を対象物の情報の事前分布として算出する。

推定部１０４は、観測部１０２により求められた対象物の情報の尤度関数と事前分布計算部１０３に算出された対象物の情報の事前分布に基づき、対象物の情報の確率分布を更新して対象物の情報の事後分布を推定すると共に、観測部１０２により求められた対象物の情報の尤度関数と予測部１０６により予測された対象物の情報の確率分布である事前分布に基づき、対象物の情報の確率分布を更新して対象物の情報の事後分布を推定する。

位置姿勢取得部１０５はデータベース１００に記憶されているカメラの位置情報及びカメラの姿勢情報を取得する。予測部１０６は、位置姿勢取得部１０５により取得されたカメラの位置情報及びカメラの姿勢情報と推定部１０４により推定された対象物の情報の事後分布に基づき、対象物の情報の事前分布を予測する。

検出部１０７は推定部１０４により推定された対象物の情報の事後分布に基づき対象物の情報の検出値を算出する。測位部１０８は、検出部１０７により算出された対象物の情報の検出値と位置姿勢取得部１０５により取得されたカメラの位置情報及びカメラの姿勢情報に基づき、対象物の地理座標を算出する。表示部１０９は、検出部１０７及び測位部１０８により算出された対象物の情報を表示する。

ここで、データベース１００には、映像情報と、映像情報の各フレームに同期して計測されたカメラの位置情報（地理座標の情報）と、カメラの姿勢情報（三軸の角度、角加速度等の情報）とが記憶されているものとする。

図２はカメラの位置情報及び姿勢情報を収集する装置の一例を示す図である。ここでは、例えば、カメラの位置情報を計測するＧＰＳ等の高精度測位装置２０１、映像情報を取得するカメラ２０２、カメラの姿勢情報を計測する方位センサ２０３及び収集した各情報を記録するデータベース１００が移動体（車両）２０４に搭載されている。この車両２０４を用いて情報収集対象地域の路上を走行して、路上の映像情報と、路上の映像情報の各フレームに同期したカメラの位置情報（地理座標情報）及びカメラの姿勢情報の収集を行い、この収集された各情報をデータベース１００に記録することができる。

このデータベース１００に記憶されている映像情報は、一眼カメラで撮影された一つの映像でも良いし、複数のカメラで同じ対象物を撮影した複数の映像でも良い。また、映像情報は、可視カメラで撮影した可視画像に限らず、例えば、ラインスキャンレーザで得られる奥行き映像情報や、赤外線カメラによって得られる熱映像情報等、カメラ以外のセンサから得られる情報でも良い。

次に動作について説明する。
図３はこの発明の実施の形態１による物体測位装置の初期化フェイズの処理手順を示すフローチャートである。この初期化フェイズは映像中に対象物を初めて検出して対象物の事前分布を予測する段階である。

ステップＳＴ１０１において、画像取得部１０１はデータベース１００に記憶されている映像情報から画像を取得する。以下では、この画像が撮影された時刻をｔとし、画像取得部１０１が取得した画像をｙ_tとする。ここで、画像取得部１０１による画像の取得は、映像情報の収集中にリアルタイムに行っても良いし、あるいは、予め収集しておいた映像情報から画像を取得しても良い。

ステップＳＴ１０２において、観測部１０２は画像取得部１０１により取得された画像ｙ_tから対象物の情報の尤度関数を求める。ここで、対象物の情報とは、対象物の地理座標や相対座標、種類、大きさ、向き、経年変化、対象物の周辺情報等の対象物の情報全般のことを指す。すなわち、観測部１０２は対象物の地理座標や相対座標、種類、大きさ、向き、経年変化、対象物の周辺情報等の対象物の情報全般の尤度関数を求める。

ここで、尤度関数の定義について述べる。以下、この実施の形態１では、時刻ｔにおける対象物の情報を確率変数とみなし、これをＸ_tとし、また、時刻ｔにおける画像取得部１０１が取得する画像を確率変数とみなし、これをＹ_tとする。また、Ｘ_t＝ｘ_tが与えられたときのＹ_tの条件付確率密度関数をＰ（ｙ_t｜Ｘ_t＝ｘ_t）とする。このとき、対象物の情報の尤度関数とは、与えられた画像Ｙ_t＝ｙ_tに対する対象物の情報ｘ_tの関数として、次の式（１）で定義される。
Ｌ（ｘ_t｜ｙ_t）＝Ｐ（Ｙ_t＝ｙ_t｜ｘ_t） （１）
上記尤度関数Ｌは、画像ｙ_tを観測したときの対象物の情報ｘ_tの尤もらしさを表す指標と考えることができる。

図４は、観測部１０２が取得された画像から、例えば色のヒストグラムを利用して対象物の情報の尤度関数を求める方法を説明する図である。ここでは、観測部１０２は、画像上に射影された対象物を囲む矩形領域に含まれる色のヒストグラムを利用して、対象物の情報の尤度関数を求める。
以下では、道路標識の大きさと、種類、カメラからの相対座標の情報を収集する状況を想定する。また、時刻ｔにおける対象物の大きさをｓ_tとし、時刻ｔにおける対象物の種類をａ_tとし、時刻ｔにおける対象物の相対座標をｚ_tとし、時刻ｔにおける対象物の情報をｘ_t＝｛ｚ_t，ａ_t，ｓ_t｝とする。

初めに、観測部１０２は、認識のために必要な多数の種類のテンプレート画像を用意し、各テンプレート画像の色のヒストグラムを求めておく。例えば、テンプレート画像がＲＧＢ画像の場合、観測部１０２は、図４（ａ）に示すように、Ｒプレーン（赤成分）における画素値の和と、Ｇプレーン（緑成分）における画素値の和と、Ｂプレーン（青成分）における画素値の和を求めておく。以下、種類ａのテンプレート画像の色のヒストグラムをｔＨｉ（ａ）と定義する。

次に、観測部１０２は、図４（ｂ）に示すように、相対座標がｚ_tで大きさがｓ_tである対象物が、画像ｙ_t上に射影されたときの矩形領域ｒ_tを求める。矩形領域ｒ_tの中心座標ｃ_tと、矩形領域ｒ_tの大きさｉｓ_tは、例えば、次の式（２）により求めることができる。

ここで、ｆはカメラの焦点距離である。また、ｚ_t（１）、ｚ_t（２）、ｚ_t（３）はそれぞれｚ_tの垂直成分、水平成分、奥行き成分とする。また、ｃ_t（１）、ｃ_t（２）はｃ_tの垂直成分と水平成分とする。

次に、観測部１０２は、種類ａのテンプレート画像の色のヒストグラムｔＨｉ（ａ）を求めたときと同じ方法により、矩形領域ｒ_tにおける色のヒストグラムＨｉ（ｒ_t）を求める。

最後に、観測部１０２は、上記で求めたヒストグラムｔＨｉ（ａ）とＨｉ（ｒ_t）を用いて、次の式（３）により対象物の情報の尤度関数を求める。

ここで、Ｂは正の定数であり、Ａは規格化定数である。また、Ｈｉ（ｒ_t）＊ｔＨｉ（ａ_t）は、各ヒストグラムの要素対の積和（各ヒストグラムをベクトルとみなして内積した値）である。また、矩形領域ｒ_tはｚ_tとｓ_tの関数である。

また、観測部１０２は、例えば、下記の参考文献１及び参考文献２に記載のアルゴリズムを利用して、対象物の情報の尤度関数を求めても良い。参考文献１に記載のＡｄａｂｏｏｓｔあるいはＬｏｇｉｔｂｏｏｓｔと呼ばれる学習アルゴリズム、及び参考文献２に記載の画像検出アルゴリズムを用いると、多数の判別関数の出力を組み合わせて、“矩形領域ｒ_tの画像が種類ａ_tの対象物である確信度”を算出する関数Ｋ（ｒ_t，ａ_t）を得ることができる。ここで、確信度とは、矩形領域の画像が、目的の種類の対象物の画像と類似している度合いを数値的に表現したものであり、類似性が高いほど確信度は大きな値をとり、類似性が低いほど確信度は小さな値を取る。このとき、尤度関数Ｌ（ｘ_t｜ｙ_t）は次の式（４）で与えられる。
Ｌ（ｘ_t｜ｙ_t）＝ｅｘｐ（Ｋ（ｒ_t，ａ_t））／（ｅｘｐ（Ｋ（ｒ_t，ａ_t））
＋ｅｘｐ（−Ｋ（ｒ_t，ａ_t））） （４）

［参考文献１］
Jerome Friedman,Trevor Hastie and Robert Tibshirani,“Additive Logistic Regression: a Statistical View of Boosting”,Ann.Statist. 28,no.2(2000),pp.337-407
［参考文献２］
Viola,P.;Jones,M.,“Rapid Object Detection Using a Boosted Cascade of Simple Features”,IEEE Computer Society Conference on Computer Vision and Pattern Recognition(CVPR),ISSN:1063-6919,Vol.1,pp.511-518,December 2001

なお、観測部１０２は、上記で述べた二つの方法に限らず、パターンマッチングやニューラルネット等、その他の方法を用いて尤度関数を求めても良い。

次に、図２のステップＳＴ１０３において、事前分布計算部１０３は、対象物の事前情報を反映した確率分布（以下、これを対象物の情報の事前分布と呼ぶ）を算出する。

ここで、対象物の情報の事前分布とは、例えば、時刻１からｔ−１までの画像ｙ_1:t-1を与えたときの、対象物の情報ｘ_tの条件付確率分布Ｐ（ｘ_t｜ｙ_1:t-1）のことを指す。

例えば、事前分布計算部１０３が、道路標識の大きさと、種類、カメラからの相対座標の情報を収集する状況を想定して、事前分布Ｐ（ｘ_t｜ｙ_1:t-1）を求める方法について説明する。
ここで、事前分布は過去の画像ｙ_1:t-1に依存せず、また、対象物の位置と大きさと種類はそれぞれ独立であると仮定すると、次の式（５）のように分解できる。
Ｐ（ｘ_t｜ｙ_1:t-1）＝Ｐｚ（ｚ_t）Ｐａ（ａ_t）Ｐｓ（ｓ_t） （５）
ここで、ｘ_t＝｛ｚ_t，ａ_t，ｓ_t｝であり、Ｐｚ（ｚ_t）は対象物の相対座標の事前情報を反映した確率分布とし、Ｐａ（ａ_t）は対象物の種類の事前情報を反映した確率分布とし、Ｐｓ（ｓ_t）は対象物の大きさの事前情報を反映した確率分布とする。

例えば、予め寸法が定められている対象物（例えば、道路標識や信号機等）の場合、これらの定められた値をｓ₀として、次の式（６）のように設定することができる。
Ｐｓ（ｓ_t）＝Ａｓｅｘｐ（−Ｂｓ（ｓ_t−ｓ₀）²） （６）
ここで、Ａｓは規格化定数であり、Ｂｓは分布の広がりを調節する定数である。

また、例えば、対象物の種類に対して予め情報がない場合には、次の式（７）のように設定することができる。
Ｐａ（ａ_t）＝１／Ｎ （全てのａ_tに対して） （７）
ここで、Ｎは対象物の種類の総数である。

また、例えば、移動体２０４がステレオカメラを搭載している場合には、ステレオ立体視を用いて計測した対象物の相対座標をｚ₀として、次の式（８）のように設定することができる。
Ｐｚ（ｚ_t）＝Ａｚｅｘｐ（−Ｂｚ（ｚ_t−ｚ₀）²） （８）
ここで、Ａｚは規格化定数であり、Ｂｚは分布の広がりを調節する定数である。

さらに、例えば、対象物の位置がカメラから半径Ｒメートル付近に存在することが予め判明している場合には、次の式（９）のように設定しても良い。

ここで、Ａｚは規格化定数であり、Ｂｚは分布の広がりを調節する定数である。

次に、ステップＳＴ１０４において、推定部１０４は、事前分布計算部１０３により算出された対象物の情報の事前分布と、観測部１０２で求めた対象物の情報の尤度関数とを用いて、対象物の情報の確率分布を更新して対象物の情報の事後分布を推定する。

推定部１０４は、例えば、ベイズの定理を用いて、次の式（１０）に示すように事後分布Ｐ（ｘ_t｜ｙ_1:t-1）を求めても良い。このとき、推定部１０４は、ベイズの定理を用いて、対象物の３次元座標、対象物の種類、対象物の大きさ等の対象物の情報の確率分布を同時に更新して対象物の情報の事後分布を推定する。
Ｐ（ｘ_t｜ｙ_1:t-1）＝Ｃ×Ｌ（ｘ_t｜ｙ_t）×Ｐ（ｘ_t｜ｙ_1:t-1） （１０）
ここで、Ｃは規格化定数である。また、Ｌ（ｘ_t｜ｙ_t）は観測部１０２が求めた尤度関数で、Ｐ（ｘ_t｜ｙ_1:t-1）は事前分布計算部１０３が算出した事前分布である。また、事後分布Ｐ（ｘ_t｜ｙ_1:t-1）は、時刻１からｔ−１までの画像ｙ_1:t-1を与えたときの対象物の情報ｘ_tの条件付確率分布である。

このように、対象物の情報を確率分布として表現することにより、対象物の情報を効率的かつ、総合的に判断することが可能となる。例えば、推定部１０４が道路標識の種類の事後分布を求めて、“駐車禁止”の道路標識である確率が１％、“速度制限”の道路標識である確率が４９％、“進入禁止”の道路標識である確率が５０％という結果が得られたとする。このような場合に、この実施の形態１では、最も確率の大きな確率を持つ“進入禁止”だけを選択するのではなく、どの結果も次の時刻における確率分布の推定に使用する。このことにより、二番目に大きな確率を持つ“速度制限”も、検出結果の候補に入れることができる。

また、推定部１０４は、このように、対象物の情報の確率分布を更新して事後分布を推定することにより、推定した対象物の情報の事後分布の分布状況に基づき、対象物の情報の精度も同時に推定することが可能である。
図５は推定部１０４が求めたｚ_t（１）の事後分布の例を示す図である。図５（ａ）と図５（ｂ）において、事後分布を最大化するような値はどちらもｍである。しかし、図５（ａ）に示す事後分布は分布の広がりが大きいため、出力した値ｍの精度が低い（信頼度が低い）と解釈することができる。一方、図５（ｂ）に示す事後分布は分布の広がりが小さいため、出力した値ｍの精度が高い（信頼度が高い）と解釈することができる。

ステップＳＴ１０５において、位置姿勢情報取得部１０５は、データベース１００に記憶されている映像情報に同期したカメラの位置情報及び姿勢情報を取得する。ここで、位置姿勢情報取得部１０５は、時刻ｔにおけるカメラの位置情報及び姿勢情報を取得しても良いし、時刻１からｔまでのカメラの位置情報及び姿勢情報を取得しても良い。また、すでに収集した映像情報をオフラインで処理している場合には、時刻１から一時刻先（時刻ｔ＋１）のカメラの位置情報及び姿勢情報を取得しても良い。
以下では、時刻ｔにおけるカメラの位置情報及び姿勢情報をＧ_tとする。また、時刻１からｔまでのカメラの位置情報及び姿勢情報をＧ_1:tとする。

ステップＳＴ１０６において、予測部１０６は、位置姿勢取得部１０５により取得されたカメラの位置情報及び姿勢情報と推定部１０４により推定された事後分布に基づき、次の時刻における対象物の情報の確率分布を対象物の情報の事前分布として予測する。

ここで、例えば、位置姿勢取得部１０５が時刻１からｔ+1までのカメラの位置情報及び姿勢情報Ｇ_1:t+1を取得したとする。このとき、予測部１０６は、事前分布として、画像ｙ_1:tとＧ_1:t+1を与えたときの次の時刻における対象物の情報ｘ_t+1の条件付確率分布Ｐ（ｘ_t+1｜ｙ_1:t，Ｇ_1:t+1）を予測する。
例えば、予測部１０６は、次の式（１１）に従って事前分布を予測しても良い。

ここで、Ｐ（ｘ_t｜ｙ_1:t）は推定部１０４が推定した事後分布である。また、Ｐ（ｘ_t+1｜ｘ_t，Ｇ_1:t+1）は、ｘ_tとＧ_1:t+1を与えたときのｘ_t+1の条件付確率分布（以下、これを状態遷移確率分布と呼ぶ）である。この状態遷移確率分布は時刻ｔからｔ＋１に進むにつれて対象物の情報がどのように変化するのかを確率的に表現したものである。

ここで、状態遷移確率分布について説明する。例えば、道路標識の大きさと、種類と、カメラからの相対座標の情報を収集する状況を想定し、時刻ｔにおける対象物の情報をｘ_t＝｛ｚ_t，ａ_t，ｓ_t｝とする。このとき、状態遷移確率分布は以下の式（１２）のように分解することができる。
Ｐ（ｘ_t+1｜ｘ_t，Ｇ_1:t+1）
＝Ｐ（ｚ_t+1｜ｚ_t，Ｇ_1:t+1）Ｐ（ａ_t+1｜ａ_t）Ｐ（ｓ_t+1｜ｓ_t） （１２）
ここで、Ｐ（ｚ_t+1｜ｚ_t，Ｇ_1:t+1）は対象物の相対座標の状態遷移確率分布であり、Ｐ（ａ_t+1｜ａ_t）は対象物の種類の状態遷移確率分布であり、Ｐ（ｓ_t+1｜ｓ_t）は対象物の大きさの状態遷移確率分布である。

ここで、対象物の相対座標の状態遷移確率分布は、例えば、次の式（１３）のように設定することができる。
Ｐ（ｚ_t+1｜ｚ_t，Ｇ_1:t+1）
＝Ａｅｘｐ（−Ｂ（ｚ_t+1−ｚ_t−Ｔ_t）²） （１３）
ここで、Ｔ_tはカメラの位置情報とカメラの姿勢情報から算出した対象物の相対座標の移動ベクトルである。また、ｚ_t＋Ｔ_tは次の時刻における対象物の相対座標の予測値である。このように、予測部１０６は、位置情報取得部１０５により取得されたカメラ位置情報及び姿勢情報を用いて、次の時刻における対象物の相対位置の確率分布を求めて、時々刻々と対象物の相対座標及び画像座標を追跡（トラッキング）する。

また、Ａは規格化定数であり、Ｂはカメラの位置情報及び姿勢情報の測位精度により決定される定数である。例えば、カメラの位置情報及び姿勢情報の測位精度が悪い場合にはＢの値は小さく設定し、カメラの位置情報及び姿勢情報の測位精度が良い場合にはＢの値は大きく設定する。このように、カメラ位置情報及び姿勢情報の計測精度を状態遷移確率分布によりモデル化することが可能であり、例えば、ＧＰＳの精度に対応した対象物の情報を求めることができる。

また、対象物の種類の状態遷移確率分布は、例えば、次の式（１４）のように設定することができる。
Ｐ（ａ_t+1｜ａ_t）＝０．６δ（ａ_t+1−ａ_t）＋０．２δ（ａ_t+1−α）
＋０．２δ（ａ_t+1−β） （１４）
ここで、α、βは対象物の種類のインデックスである。これは、時刻ｔ＋１における対象物の種類が、確率０．６で種類ａ_tに、確率０．２で種類αに、確率０．２で種類βに遷移することを表している。

また、対象物の大きさの状態遷移確率分布は、例えば、次の式（１５）のように設定することができる。
Ｐ（ｓ_t+1｜ｓ_t）
＝Ａｓｅｘｐ（−Ｂｓ（ｓ_t+1−ｓ_t）²） （１５）
ここで、Ａｓは規格化定数であり、Ｂｓは対象物の大きさの測定精度により決定される定数である。

図６はこの発明の実施の形態１による物体測位装置の追跡フェイズの処理手順を示すフローチャートである。この追跡フェイズは図３に示す初期化フェイズの後に対象物を追跡して対象物の情報の確率分布を求める段階である。

ステップＳＴ２０１において、画像取得部１０１はデータベース１００に記憶されている映像情報から次の時刻の画像を取得する。以下では、この画像が撮影された時刻をｔ＋１とし、画像取得部１０１が取得した画像をｙ_t+1とする。

ステップＳＴ２０２において、観測部１０２は画像取得部１０１により取得された画像ｙ_t+1から対象物の情報の尤度関数Ｌ（ｘ_t+1｜ｙ_t+1）を求める。観測部１０２の処理は前述したステップＳＴ１０２と同一であるのでここでは記述を省略する。

ステップＳＴ２０３において、推定部１０４は、観測部１０２により求められた対象物の情報の尤度関数と前の時刻において予測部１０６により予測された対象物の情報の事前分布に基づき、対象物の情報の確率分布を更新して対象物の情報の事後分布を推定する。
例えば、推定部１０４は、次の式（１６）に示すベイズの定理を用いて、対象物の情報の事後分布Ｐ（ｘ_t+1｜ｙ_1:t+1，Ｇ_1:t+1）を推定しても良い。
Ｐ（ｘ_t+1｜ｙ_1:t+1，Ｇ_1:t+1）
＝Ｃ×Ｌ（ｘ_t+1｜ｙ_t+1）×Ｐ（ｘ_t+1｜ｙ_1:t，Ｇ_1:t+1） （１６）
ここで、Ｃは規格化定数である。また、Ｐ（ｘ_t+1｜ｙ_1:t，Ｇ_1:t+1）は前の時刻において予測部１０６が予測した事前分布であり、Ｌ（ｘ_t+1｜ｙ_t+1）は観測部１０２が求めた尤度関数である。
その他の処理の方法は前述したステップＳＴ１０４の方法と同一であるので、ここでは記述を省略する。

ステップＳＴ２０４において、位置姿勢情報取得部１０５はデータベース１００に記憶されている映像情報に同期したカメラの位置情報及び姿勢情報を取得する。その他の処理の方法は、前述したステップＳＴ１０５の方法と同一であるので、ここでは記述を省略する。

ステップＳＴ２０５において、予測部１０６は、位置姿勢取得部１０５により取得されたカメラの位置情報及び姿勢情報と推定部１０４により推定された事後分布に基づき、次の時刻における対象物の情報の事前分布を予測し、推定部１０４は、観測部１０２により求められた対象物の情報の尤度関数と予測部１０６により予測された対象物の情報の事前分布に基づき、次の時刻において、対象物の情報の確率分布を更新して対象物の情報の事後分布を推定する。

例えば、位置姿勢取得部１０５がＧ_1:t+2を取得した場合、予測部１０６は次の式（１７）に従って事前分布Ｐ（ｘ_t+2｜ｙ_1:t+1，Ｇ_1:t+2）を予測しても良い。

ここで、Ｐ（ｘ_t+1｜ｙ_1:t+1，Ｇ_1:t+1）は推定部１０４により推定された事後分布である。また、Ｐ（ｘ_t+2｜ｘ_t+1，Ｇ_1:t+2）は、上記ステップＳＴ１０６で述べた状態遷移確率分布である。その他の処理の方法は、前述したステップＳＴ１０６の方法と同一であるので、ここでは記述を省略する。
予測された事前分布は、次の時刻（時刻ｔ＋２）において、推定部１０４が事後分布を推定するために用いる。

ステップＳＴ２０６において、検出部１０７は推定部１０４により推定された事後分布に基づき対象物の情報の検出値を算出する。

ここで、検出部１０７の処理例について説明する。例えば、道路標識の相対座標と、大きさと、種類の情報を収集する状況を想定し、時刻ｔ＋１における対象物の情報をｘ_t+1＝｛ｚ_t+1，ａ_t+1，ｓ_t+1｝とする。
このとき、検出部１０７は、例えば、推定部１０４により推定された事後分布Ｐ（ｘ_t+1｜ｙ_1:t+1，Ｇ_1:t+1）＝Ｐ（ｚ_t+1，ａ_t+1，ｓ_t+1｜ｙ_1:t+1，Ｇ_1:t+1）を最大とするような値を対象物の情報の検出値としても良い。すなわち、検出部１０７は、次の式（１８）に従って、時刻ｔ＋１における対象物の相対座標の検出値

と、時刻ｔ＋１における対象物の大きさの検出値

と、時刻ｔ＋１における対象物の種類の検出値

とを求めても良い。

また、検出部１０７は、推定部１０４により推定された事後分布の分布状況に基づき、すなわち事後分布の広がりに基づき、対象物の情報の検出値の精度（あるいは対象物の情報の検出値の信頼度）を判定することができる。

また、検出部１０７は、画像に複数の対象物が同時に写っている場合、対象物の情報をグルーピングすることにより複数の対象物の情報の検出値を算出しても良い。
例えば、まず、検出部１０７は下記の式（１９）の条件を満たすｚ_t+1を抽出する。

ここで、θは検出率を調節するための閾値である。
次に、検出部１０７は、抽出したｚ_t+1のうち、距離が近いデータをまとめてグループ化する。ここで、グループ化の方法には、例えばｋ−ｍｅａｎｓアルゴリズム等の既存のクラスタリングアルゴリズムを用いても良い。

次に、検出部１０７は、次の式（２０）に従って、各グループにおける対象物の大きさと対象物の種類を決定する。

ここで、

はｉ番目のグループＭｉに属するｚ_t+1に対して和をとることを意味する。また、

はｉ番目のグループにおける対象物の大きさの検出値であり、

はｉ番目のグループにおける対象物の種類の検出値である。

ステップＳＴ２０７において、測位部１０８は、検出部１０７により算出された対象物の情報の検出値である対象物の相対座標の検出値と位置姿勢取得部１０５により取得されたカメラの位置情報及び姿勢情報に基づき、対象物の地理座標を算出する。

例えば、測位部１０８は、次の式（２１）に従って、対象物の地理座標ｗを求めても良い。
ｗ＝Ｑ_t+1ｚ_t+1＋Ｐ_t+1 （２１）
ここで、ｚ_t+1は検出部１０７により算出された対象物の相対座標である。Ｑ_t+1は、時刻ｔ＋１におけるカメラ座標系から地理座標系へ座標変換を行うための行列であり、位置姿勢取得部１０５により取得されたカメラの姿勢情報から求めることができる。また、Ｐ_t+1は時刻ｔ＋１におけるカメラの地理座標であり、位置姿勢取得部１０５により取得されたカメラの位置情報から求めることができる。
通常、位置姿勢取得部１０５により取得されたカメラの位置情報及び姿勢情報には計測誤差が伴う。このため、位置姿勢取得部１０５により取得されたカメラの位置情報やカメラの姿勢情報に対して、例えばカルマンフィルタを用いて計測誤差を低減し、Ｑ_t+1及びＰ_t+1を算出する。

ステップＳＴ２０８において、表示部１０９は、検出部１０７及び測位部１０８により算出された対象物の情報を、ディスプレイ等の表示装置に表示する。例えば、表示部１０９は、測位部１０８により算出された対象物の地理座標を、既存の電子地図や映像の上に表示する。

ステップＳＴ２０９において、対象物が映像に写っている間、上記ステップＳＴ２０１からステップＳＴ２０８までの処理を繰り返す。

このように、推定部１０４は、前の時刻で求めた対象物の情報の確率分布を用いて、現在の時刻における確率分布の推定を行うことにより、各画像から得られる情報を時々刻々と累積していき、対象物の情報の確率分布を更新して対象物の情報の事後分布を推定していくことにより、結果として対象物の情報を精度良く求めることができる。

図７は速度規制の道路標識が入力された場合の画像取得部１０１と予測部１０６と推定部１０４の処理を説明する図である。
例えば、画像取得部１０１が“速度規制”の道路標識が写る画像を取得したところ、日照変化や遮蔽等が原因で、図７（ａ）に示す時刻ｔにおける対象物の種類に関する尤度関数が得られたとする。この場合、もし、尤度関数のみを使用して対象物の種類を判断すると、最も尤度の大きな“駐車禁止”が誤って選択されてしまう。

一方、予測部１０６によって図７（ｂ）に示す事前分布が予測されたとする。この事前分布は、過去の画像から得られる情報を累積しており、入力した“速度規制”の画像である確率が最も高い。このため、推定部１０４が、図７（ａ）示す尤度関数と図７（ｂ）に示す事前分布に基づき、図７（ｃ）に示す事後分布を推定した場合、本来入力した“速度規制”である確率が最も高くなり、対象物の誤検出を防ぐことができる。

同様な理由により、推定部１０４が、時系列画像から相対座標や大きさ等の情報の事後分布を時々刻々と求めることにより、画像やＧＰＳ信号等への外乱に対して頑健に情報を収集することが可能となる。

なお、この実施の形態１は、例えば、参考文献３に記載のＰａｒｔｉｃｌｅ Ｆｉｌｔｅｒを用いて実装することが可能である。Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒとは、サンプリングにより確率分布を近似して、確率分布の推定を行うアルゴリズムであり、この実施の形態１における確率分布の計算を効率的に行うことができる。
［参考文献３］
S.Arulampalam,S.Maskell,N.J.Gordon,and T.Clapp,“A Tutorial on Particle Filters for Online Non-linear/Non-Gaussian Bayesian Tracking”,IEEE Transactions on Signal Processing,Vol.50(2),pages 174-188,February 2002.

以上のように、この実施の形態１によれば、前の時刻で求めた対象物の情報の確率分布を用いて、現在の時刻における確率分布の推定を行い、各画像から得られる情報を時々刻々と累積していくことにより、ＧＰＳの測位誤差やステレオ立体視に伴う測位誤差を低減して精度良く対象物を測位することができ、対象物の種類や大きさ等の対象物の情報も効率的に求めることができるという効果が得られる。

また、この実施の形態１によれば、道路標識、信号、マンホール、ガードレール等の道路設備の情報を自動で収集するだけではなく、収集した情報を、設備管理や、カーナビのための地図作成に利用することができる。さらに、道路設備の情報収集だけではなく、例えば、本装置を電車に搭載して、路線状況の情報収集や管理を行うことも可能である。

実施の形態２．
図８はこの発明の実施の形態２による物体測位装置の構成を示すブロック図である。この物体測位装置は、上記実施の形態１の図１に示す構成に、対象物情報データベース１１０を追加したものであり、その他の構成は図１と同じである。
上記実施の形態１では、データベース１００に記憶されている映像情報並びにその映像情報に同期して計測されたカメラの位置情報及び姿勢情報に基づき、対象物の情報の確率分布を求める方法について説明しているが、この実施の形態２では、予め与えられた対象物情報データベース１１０を利用して、対象物の情報の確率分布を求める方法について説明する。

ここで、対象物情報データベース１１０とは、例えば、カーナビ等で使用されている地理情報を保存したデータベースや、道路設備管理に使用している設備管理情報を保存したデータベースのことを指す。以下、対象物情報データベース１１０に記憶された情報をＤとする。また、対象物情報データベース１１０には、情報Ｄの精度（情報Ｄに対する信頼度）も記録されていても良い。

次に動作について説明する。
事前分布計算部１０３は、対象物情報データベース１１０に記憶されている情報Ｄに基づき、対象物の事前情報を反映した確率分布を対象物の情報の事前分布として算出する。ここで、対象物の情報の事前分布とは、時刻１からｔ−１までの画像ｙ_1:t-1と、対象物情報データベース１１０に記憶されている情報Ｄを与えたときの、対象物の情報ｘ_tの条件付確率分布Ｐ（ｘ_t｜ｙ_1:t-1，Ｄ）のことを指す。

このとき、事前分布計算部１０３は、例えば、対象物情報データベース１１０を検索して、現在の時刻におけるカメラの地理座標の近辺に存在する対象物の情報を取得し、この対象物の大きさや種類や地理座標の情報を用いて、事前分布を算出することができる。

ここで、道路標識の大きさと、種類、カメラからの相対座標の情報を収集する状況を想定して、事前分布Ｐ（ｘ_t｜ｙ_1:t-1，Ｄ）を求める方法について説明する。また、事前分布計算部１０３が、対象物情報データベース１１０の中から、時刻ｔにおけるカメラの地理座標の近辺に存在する対象物の情報を検索して、対象物の地理座標ｗ₀と、対象物の大きさｓ₀と、対象物の種類ａ₀を取得したとする。

まず、事前分布は過去の画像ｙ_1:t-1に依存せず、また、対象物の位置と大きさと種類はそれぞれ独立であると仮定すると、事前分布は次の式（２２）のように分解できる。
Ｐ（ｘ_t｜ｙ_1:t-1，Ｄ）
＝Ｐｚ（ｚｔ｜Ｄ）Ｐａ（ａ_t｜Ｄ）Ｐｓ（ｓ_t｜Ｄ） （２２）
ここで、ｘ_t＝｛ｚ_t，ａ_t，ｓ_t｝であり、Ｐｚ（ｚ_t｜Ｄ）は対象物情報データベース１１０に基づいた対象物の相対距離の確率分布、Ｐａ（ａ_t｜Ｄ）は対象物情報データベース１１０に基づいた対象物の種類の確率分布、Ｐｓ（ｓ_t｜Ｄ）は対象物情報データベース１１０に基づいた対象物の大きさの確率分布である。

このとき、例えば、対象物の大きさの事前分布は、次の式（２３）のように設定することができる。
Ｐｚ（ｓ_t）＝Ａｓｅｘｐ（−Ｂｓ（ｓ_t−ｓ₀）²） （２３）
ここで、Ａｓは規格化定数であり、Ｂｓは分布の広がりを調節する定数である。対象物情報データベース１１０に記憶されている対象物の大きさｓ₀の精度が悪い（信頼度が低い）場合はＢｓを小さく設定する。また、ｓ₀の精度が良い（信頼度が高い）場合にはＢｓを大きく設定する。

また、例えば、対象物の種類の事前分布は、次の式（２４）のように設定することができる。
Ｐａ（ａ_t）＝１（ａ_t＝ａ₀の場合）
Ｐａ（ａ_t）＝０（その他の場合） （２４）

また、例えば、対象物の相対位置の事前分布は、次の式（２５）のように設定することができる。
Ｐｚ（ｚ_t）＝Ａｚｅｘｐ（−Ｂｚ（ｚ_t−Ｈ（ｗ₀））²） （２５）
ここで、Ｈは地理座標系の座標をカメラ座標系の座標に変換する関数であり、位置姿勢取得部１０５で取得したカメラの位置情報とカメラの姿勢情報から求めることができる。また、Ａｚは規格化定数であり、Ｂｚは分布の広がりを調節する定数である。例えば、対象物情報データベース１１０に記憶されている対象物の大きさｗ₀の精度が悪い場合は、Ｂｚを小さく設定する。また、ｗ₀の精度が良い場合には、Ｂｚを大きく設定する。

推定部１０４は、事前分布計算部により算出された事前分布と、観測部１０２により求められた尤度関数に基づき、最終的に対象物の情報の事後分布Ｐ（ｘ_t｜ｙ_1:t，Ｇ_1:t，Ｄ）を推定する。ここで、Ｐ（ｘ_t｜ｙ_1:t，Ｇ_1:t，Ｄ）は、時刻１からｔまでの画像ｙ_1:tと、時刻１からｔまでのカメラの位置情報及び姿勢情報Ｇ_1:tと、対象物情報データベース１１０に記憶されている情報Ｄとを与えたときの対象物の情報ｘ_tの条件付確率分布である。

その他の構成要素の処理については、上記実施の形態１と同じであるので、説明を省略する。

以上のように、この実施の形態２によれば、上記実施の形態１と同様の効果が得られると共に、対象物情報データベース１１０に記憶されている対象物の情報と、対象物情報データベース１１０に記憶されている対象物の情報の精度（情報の信頼度）を同時に利用して、新たに対象物の情報を収集することが可能であり、情報収集の効率化を図ることができるという効果が得られる。

また、この実施の形態２によれば、収集した対象物の情報と、対象物情報データベース１１０に記憶された情報とを比べることで、情報の差異を発見することも可能である。

この発明の実施の形態１による物体測位装置の構成を示すブロック図である。 カメラの位置情報及び姿勢情報を収集する装置の一例を示す図である。 この発明の実施の形態１による物体測位装置の初期化フェイズの処理手順を示すフローチャートである。 この発明の実施の形態１による物体測位装置の観測部が取得された画像から例えば色のヒストグラムを利用して対象物の情報の尤度関数を求める方法を説明する図である。 この発明の実施の形態１による物体測位装置の推定部が求めた事後分布の例を示す図である。 この発明の実施の形態１による物体測位装置の追跡フェイズの処理手順を示すフローチャートである。 この発明の実施の形態１による物体測位装置において、速度規制の道路標識が入力された場合の画像取得部と予測部と推定部の処理を説明する図である。 この発明の実施の形態２による物体測位装置の構成を示すブロック図である。

符号の説明

１００ データベース、１０１ 画像取得部、１０２ 観測部、１０３ 事前分布計算部、１０４ 推定部、１０５ 位置姿勢取得部、１０６ 予測部、１０７ 検出部、１０８ 測位部、１０９ 表示部、１１０ 対象物情報データベース。

Claims (14)

1. 映像情報並びにその映像情報に同期したカメラの位置情報及び姿勢情報を記憶するデータベースと、
   該データベースに記憶されている映像情報から画像を取得する画像取得部と、
   該画像取得部により取得された画像から対象物の情報の尤度関数を求める観測部と、
   該観測部により求められた対象物の情報の尤度関数と予測された対象物の情報の確率分布である事前分布に基づき、対象物の情報の確率分布を更新して対象物の情報の事後分布を推定する推定部と、
   上記データベースに記憶されているカメラの位置情報及び姿勢情報を取得する位置姿勢取得部と、
   該位置姿勢取得部により取得されたカメラの位置情報及び姿勢情報と上記推定部により推定された対象物の情報の事後分布に基づき、対象物の情報の事前分布を予測する予測部と、
   上記推定部により推定された対象物の情報の事後分布に基づき対象物の情報の検出値を算出する検出部と、
   該検出部により算出された対象物の情報の検出値と上記位置姿勢取得部により取得されたカメラの位置情報及び姿勢情報に基づき、対象物の地理座標を算出する測位部とを備えた物体測位装置。
2. 上記観測部は対象物の地理座標や相対座標、種類、大きさ、向き、経年変化、対象物の周辺情報等の対象物の情報の尤度関数を求めることを特徴とする請求項１記載の物体測位装置。
3. 上記観測部は、画像上に射影された対象物を囲む矩形領域に含まれる色のヒストグラムを利用して、対象物の情報の尤度関数を求めることを特徴とする請求項１記載の物体測位装置。
4. 上記推定部は、ベイズの定理を用いて、対象物の３次元座標、対象物の種類、対象物の大きさ等の対象物の情報の確率分布を同時に更新して対象物の情報の事後分布を推定することを特徴とする請求項１記載の物体測位装置。
5. 上記推定部は、推定した対象物の情報の事後分布の分布状況に基づき、対象物の情報の精度を推定することを特徴とする請求項４記載の物体測位装置。
6. 上記推定部は、前の時刻で求めた対象物の情報の確率分布を用いて現在の時刻における確率分布の推定を行うことにより、各画像から得られる対象物の情報を時々刻々と累積していき、対象物の情報の確率分布を更新して対象物の情報の事後分布を推定することを特徴とする請求項１記載の物体測位装置。
7. 上記予測部は、位置姿勢取得部により取得されたカメラの位置情報及び姿勢情報を用いて、次の時刻における対象物の相対位置の確率分布を求めて時々刻々と対象物の相対座標を追跡することを特徴とする請求項１記載の物体測位装置。
8. 上記予測部は、カメラの位置情報及び姿勢情報の測定精度を状態遷移確率分布によってモデル化して、対象物の情報の事前分布を予測することを特徴とする請求項１記載の物体測位装置。
9. 上記検出部は、上記推定部により推定された対象物の情報の事後分布を最大とするような値を検出値とすることを特徴とする請求項１記載の物体測位装置。
10. 上記検出部は、上記推定部により推定された事後分布の分布状況に基づき、算出した対象物の情報の検出値の精度を判定することを特徴とする請求項１記載の物体測位装置。
11. 上記検出部は、画像に複数の対象物が同時に写っている場合、対象物の情報をグルーピングすることにより複数の対象物の情報の検出値を算出することを特徴とする請求項１記載の物体測位装置。
12. 対象物の事前情報を反映した確率分布を対象物の情報の事前分布として算出する事前分布計算部を備え、
    上記推定部は、上記観測部により求められた対象物の情報の尤度関数と上記事前分布計算部により算出された対象物の情報の事前分布に基づき、対象物の情報の確率分布を更新して対象物の情報の事後分布を推定することを特徴とする請求項１記載の物体測位装置。
13. 対象物の情報を記憶する対象物情報データベースを備え、
    上記事前分布計算部は、上記対象物情報データベースに記憶されている対象物の情報に基づき、対象物の事前情報を反映した確率分布を対象物の情報の事前分布として算出することを特徴とする請求項１２記載の物体測位装置。
14. 上記対象物情報データベースは対象物の情報の精度を記憶し、
    上記事前分布計算部は、上記対象物情報データベースに記憶されている対象物の情報の精度に基づき、対象物の情報の事前分布を算出することを特徴とする請求項１３記載の物体測位装置。

Images