障害物追跡装置及びその方法
【課題】検出された障害物候補領域に対してあいまい性を考慮して、障害物の正確な位置を検出する障害物追跡装置を提供する。
【解決手段】移動体に搭載されて障害物に関する時系列画像を取り込む画像入力部１と、時系列画像から現時刻の障害物候補領域を検出する障害物検出部２と、障害物の前時刻に関する状態仮説群を保持し、また、障害物候補領域の位置毎の観測仮説と障害物が検出されなかったとする観測仮説を合わせた観測仮説群を生成する仮説生成部３と、状態仮説群と観測仮説群との組み合わせ毎の尤度をそれぞれ計算し、まつ、尤度が最大になる観測仮説群と状態仮説群の組み合わせを選択し、この選択した状態仮説群を現在時刻の状態仮説群として更新する尤度計算部５と、現在時刻の状態仮説群の中で前記尤度が最大となる状態仮説を障害物の検出した状態とする仮説選択部６とを具備する。
【解決手段】移動体に搭載されて障害物に関する時系列画像を取り込む画像入力部１と、時系列画像から現時刻の障害物候補領域を検出する障害物検出部２と、障害物の前時刻に関する状態仮説群を保持し、また、障害物候補領域の位置毎の観測仮説と障害物が検出されなかったとする観測仮説を合わせた観測仮説群を生成する仮説生成部３と、状態仮説群と観測仮説群との組み合わせ毎の尤度をそれぞれ計算し、まつ、尤度が最大になる観測仮説群と状態仮説群の組み合わせを選択し、この選択した状態仮説群を現在時刻の状態仮説群として更新する尤度計算部５と、現在時刻の状態仮説群の中で前記尤度が最大となる状態仮説を障害物の検出した状態とする仮説選択部６とを具備する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自動車などの車両に代表される移動体に取り付けられたＴＶカメラから得られる画像を用いて、車両等の障害物を検出し、追跡する障害物追跡装置及びその方法に関するものである。
【背景技術】
【０００２】
従来、車両等の障害物を追跡する方法として、移動物体計測装置がある（特許文献１参照）。これは、画像上でエッジの点数が多い領域を車両として検出し、実空間上における車両車尾の位置を、カルマンフィルタを用いて追跡する。
【０００３】
また、粒子型フィルタに基づく複数の非剛体オブジェクトの追跡という方法もある（非特許文献１参照）。これは、追跡領域を小領域に分割し、各々の小領域の移動仮説を粒子とした粒子型フィルタ（パーティクルフィルタ）を用いて、車両等の物体の追跡を行っている。
【特許文献１】特開平８−９４３２０号公報
【非特許文献１】阿部他，「粒子型フィルタに基づく複数の非剛体オブジェクトの追跡」，電子情報通信学会技術研究報告PRMU2003-241，pp63-66
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかし、特許文献１及び非特許文献１の技術において、複数の障害物を同時に追跡するときに問題点がある。
【０００５】
すなわち、特許文献１の方法では、照明条件の変動や隠れ等により、障害物が未検出になった場合に、他に検出された障害物を対応付けて追跡位置に大きな誤差が生じることや、間違った障害物の追跡をすること等の問題点がある。
【０００６】
また、非特許文献１の方法では、ロバストな追跡が可能になるが、数個の離散的な観測位置に対して、多くのパーティクルを用いる必要があり、無駄が多いときもあり、処理量が増大する等の問題点がある。
【０００７】
そこで、本発明では、上記問題点に鑑みて、検出された障害物候補領域に対してあいまい性を考慮して、障害物の正確な位置を検出する障害物追跡装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明は、移動体に搭載されて障害物が写った時系列画像を取得する画像取得部と、前記時系列画像から現時刻の障害物候補領域を検出する障害物検出部と、前記障害物の前時刻に関する１つあるいは複数の状態仮説を含んだ状態仮説群を格納する状態仮説格納部と、前記障害物候補領域の位置毎の観測仮説と前記障害物が検出されなかったと仮定した場合の観測仮説を合わせた１つあるいは複数の観測仮説を含んだ観測仮説群を生成する観測仮説生成部と、前記状態仮説群に含まれる状態仮説のそれぞれと前記観測仮説群に含まれる観測仮説のそれぞれの組み合わせ毎の尤度をそれぞれ計算する尤度計算部と、前記観測仮説毎の尤度の中で最大の尤度を求め、前記最大の尤度に対応する状態仮説の集合である現在時刻の状態仮説群を用いて、前記状態仮説格納部に格納された前時刻の状態仮説を更新する状態仮説更新部と、前記現在時刻の状態仮説群の中で前記尤度が最大となる状態仮説を、前記障害物が検出された状態として選択する仮説選択部と、を具備することを特徴とする障害物追跡装置である。
【発明の効果】
【０００９】
本発明によれば、車両に搭載された画像入力部により取得される時系列画像から障害物の位置を安定に検出することができる。
【発明を実施するための最良の形態】
【００１０】
以下、本発明の一実施形態の障害物追跡装置を図１から図５を参照して説明する。
【００１１】
本実施形態の障害物追跡装置では、検出された障害物候補領域に対して、あいまい性を考慮して、複数の障害物の観測位置を用いて、複数の観測仮説を設定し、その複数の観測仮説から一番正しい仮説を選択する。
【００１２】
（１）障害物追跡装置の構成
図１に本実施形態の障害物追跡装置のブロック図を兼ねたフローチャートである。
【００１３】
図１に示すように、障害物追跡装置は、画像入力部１、障害物検出部２、観測位置設定部３、仮説生成部４、尤度計算部５、仮説選択部６、信頼性評価部７から構成される。
【００１４】
障害物追跡装置は、例えば、汎用のコンピュータ装置を基本ハードウェアとして用いることでも実現することが可能である。すなわち、障害物検出部２、観測位置設定部３、仮説生成部４、尤度計算部５、仮説選択部６、信頼性評価部７は、上記のコンピュータ装置に搭載されたプロセッサにプログラムを実行させることにより実現することができる。
【００１５】
（２）画像入力部１
画像入力部１は、車両に取り付けられた車両の前方周辺の画像を取得可能なステレオ視のＴＶカメラを有する。
【００１６】
また、図２に本実施形態における座標系を示し、世界座標系の水平方向をＸ、奥行き方向をＺ、画像座標系の横方向をｘ、縦方向をｙとする。また、時刻ｔは、この時系列画像のフレーム数によって決まる。
【００１７】
（３）障害物検出部２
障害物検出部２は、画像入力部１により取得された時系列画像から障害物候補領域を決定する。
【００１８】
障害物検出部２は、ステレオ視の左右画像を用いて、車両等の障害物候補領域を検出する。このとき左右画像の視差から、図２に示すように、画像座標系での位置（ｘ，ｙ）と、世界座標系での位置（Ｘ，Ｚ）が求まる。
【００１９】
ステレオ視の方法としては、非特許文献２（「ステレオ画像を用いた運転支援のための前方状況認識システム」，電子情報通信学会技術研究報告，PRMU97-30，1997，pp39-46）方法がある。
【００２０】
（４）観測位置設定部３
観測位置設定部３は、障害物の現在時刻ｔの観測位置を設定する。その設定方法は次の通りである。
【００２１】
まず、現在追跡している障害物に関して前時刻ｔ−１の障害物の予測位置を仮説生成部４から呼び出す。この予測位置は、後から説明する現在時刻ｔにおける予測位置の求め方と同じ方法で求めた前時刻ｔ−１における予測位置である。
【００２２】
次に、図３に示すように、その予測位置と、障害物検出部２により得られた現在時刻ｔの検出位置を比較し、予測位置と検出位置との距離が閾値以下の場合は、その検出位置を「観測位置」とする。なお、この観測位置は１つとは限らず、前記距離が閾値以下にある検出位置は全て観測位置とする。例えば、下記の数１ではＭ個の観測位置が存在する。
【００２３】
なお、観測位置は、奥行き方向の距離Ｚと水平方向の距離Ｘでが表されているが、以下の説明では、奥行き方向の距離Ｚのみで観測位置を表す。このとき得られるＭ個の観測位置の観測分布は、
【数１】
【００２４】
となり、ガウシアン分布を用いて表す。Ｚｍは観測位置、σＲは観測分布の標準偏差である。
【００２５】
また、観測位置が存在しなかったという観測位置の観測分布は、
【数２】
【００２６】
となり、一様分布を用いて表す。係数αにより大きさが調節される。
【００２７】
このように、観測分布を適切に設定すれば、様々な検出方法によって得られた観測位置でも扱うことが可能である。
【００２８】
（５）仮説生成部４
仮説生成部４は、障害物毎に観測位置設定部３で設定されたＭ個の観測位置に対して、それぞれ観測仮説を生成する。観測仮説０は観測位置なしという仮説である。そして、観測仮説１は観測位置１と設定し、観測仮説ｊは観測位置ｊと設定する（但し、ｊ＝１，２，・・・Ｍである）。
【００２９】
また、仮説生成部４は、Ｎ個の状態仮説、すなわち、下記で説明する状態仮説群を保持している。ここで「状態」とは障害物の位置や速度などの障害物の運動情報に関する状態を意味し、「状態仮説」とは、それら状態に関する仮説をいう。この保持している状態仮説群についてさらに詳しく説明する。
【００３０】
時刻ｔにおいて状態空間中に状態ベクトルｘｔ、状態ベクトルの分散共分散行列Ｐｔ、距離Ｚｔ、観測位置の誤差分散行列Ｒ（標準偏差σＲ）を持つとする。また、状態遷移行列をＡ、状態遷移ノイズをＱとする。等加速度運動モデルを仮定するので、観測行列をＨ、奥行き方向の距離の観測位置をＺｍとすると、
【数３】
【００３１】
となる。状態空間中にＮ個の状態仮説が、ベクトルｘｔ（ｉ）とそれに対応する尤度がπｔ（ｉ）とする状態仮説群｛（ベクトルｘｔ−１（ｉ）；πｔ−１（ｉ））｝（但し、ｉ＝０，１，・・・Ｎ−１である）を持つとする。但し、明細書中では、太字でベクトルを表記できないため、「ベクトルｘ」などと記載する。また、「ベクトルｘ」は、障害物の世界座標系での位置や速度を表し、また、ｘとπの下の添え字は時刻を表し、上の添え字は状態仮説の番号を表す。さらに、「尤度」とは、信頼の度合いをいい、尤度が高ければ、その信頼性も高い。
【００３２】
（６）尤度計算部５
尤度計算部５は、下記で説明する予測、観測、推定のステップを通じて、仮説生成部４で保持している状態仮説群の更新を行い、さらに尤度を計算する。
【００３３】
（６−１）予測ステップ
時刻ｔ−１において推定されたＮ個の状態仮説群｛（ベクトルｘｔ（ｉ）；πｔ（ｉ））｝（但し、ｉ＝０，１，・・・Ｎ−１である）に対して、それぞれカルマンフィルタを用いて予測位置及び事前分布Ｐｐｒｉｏｒｉ（Ｚ）を数４、数６により計算する。Ｐ０（ｉ）は、Ｐｔ−（ｉ）の第１行第１列の成分である。＜ベクトルｘｔ／ｔ−１＞は現在の状態の予測位置、＜ベクトルｘｔ−１／ｔ−１＞は過去の状態の推定位置、Ｐｔ／ｔ−１は、予測された状態の誤差共分散行列、Ｐｔ−１／ｔ−１は過去の状態の誤差共分散行列である。但し、明細書中ではベクトルｘの上に「＾」をつけた記号が記載できないため、＜ベクトルｘ＞と表記する。
【数４】
【数５】
【００３４】
（６−２）観測ステップ
時刻ｔにおいて得られたＭ個の観測位置から観測分布を数１、数２を用いて決定する。
【００３５】
また、カルマンゲインＫｔ（ｉ，ｊ）を数６によって計算する。
【数６】
【００３６】
（６−３）推定ステップ
時刻ｔ−１において推定された状態仮説群｛（ベクトルｘｔ−１（ｉ）；πｔ−１（ｉ））｝（但し、ｉ＝０，１，・・・Ｎ−１である）とＭ個の観測仮説の組み合わせからカルマンフィルタを用いてＮ×Ｍ個の推定位置と事後分布を数７により計算する。
【数７】
【００３７】
推定位置における事前分布の確率を事前確率、推定位置における観測分布の確率を観測確率とすると、ベイズの法則により、事後確率は事前確率と観測確率の積として数８を用いて計算する。
【数８】
【００３８】
また、観測仮説と状態仮説間の組み合わせの重み係数ｗとし、数９により求まった事後確率を新しい状態仮説の尤度として計算する。
【数９】
【００３９】
この観測仮説と状態仮説間の組み合わせの重み係数ｗは、付加的な情報がある場合に計算される。
【００４０】
（７）仮説選択部６
仮説選択部６は、新しい状態仮説から、数１０によりＭ個の観測仮説毎に尤度が最大となる組み合わせを選択する。
【数１０】
【００４１】
新しい状態仮説群は、｛（ベクトルｘｔ（ｊ）；πｔ（ｊ））｝（但し、ｊ＝０，１，・・・Ｍ−１である）として更新する。このときπｔ（ｊ）は、全ての状態仮説における尤度の合計が１となるように正規化を行う。
【００４２】
また、Ｍ個の新しい状態仮説群において、πｔ（ｊ）が最大になるものが、現在時刻ｔにおける障害物の状態仮説として選択される。
【００４３】
（８）信頼性評価部７
信頼性評価部７は、仮説選択部６によって選択された状態仮説から、障害物の位置、速度、加速度などの運動情報を取得する。
【００４４】
また、運動状態の誤差分散が大きさ等から信頼性が評価し、信頼性が低い障害物は追跡を打ち切る。
【００４５】
さらに、障害物検出部２から得られた障害物候補領域が新規である場合は、追跡を開始する。
【００４６】
以上のような処理を時系列画像に対して処理を行い、車両等の障害物を追跡し、正確な位置を検出する。
【００４７】
（９）処理内容
障害物追跡装置の処理内容を図４に基づいて説明する。
【００４８】
時刻ｔ−２では、観測位置２により状態仮説２が選択されているとする。
【００４９】
時刻ｔ−１では、正しい観測位置が得られなかったため、時刻ｔ−２から時刻ｔ−１における状態仮説と観測仮説の組み合わせの尤度を計算すると、状態仮説２と観測仮説０の尤度が最大になり、時刻ｔ−１の状態では観測位置なしという観測仮説０に基づく状態仮説０が選択される。
【００５０】
時刻ｔの状態では、障害物検出部２による結果が良好のため、時刻ｔ−１から時刻ｔにおける状態仮説と観測仮説の組み合わせの尤度を計算すると、状態仮説０と観測仮説２の組み合わせの尤度が最大になり、ステレオ視の結果という観測仮説２に基づく状態仮説２が選択される。
【００５１】
このように、前回の時刻（ｔ−１）の状態仮説群と現在の時刻ｔの観測仮説群とで全ての組み合わせを考え、全ての組み合わせの尤度を計算して、最大尤度になる組み合わせが、正しい対応付けと考える。
【００５２】
（１０）効果
本実施形態の障害物追跡装置であると、複数の仮説を保持しつつ追跡し、尤度が最大となる仮説を選択することにより、障害物の位置を決定する。これにより、障害物候補領域が検出されなかった場合に、他の障害物を間違って追跡する現象が減り、そして、一時的に追跡が間違ったとしても、再び正しい障害物を追跡できる。また、検出される障害物候補領域の数に応じて、適切に仮説を生成するため、処理量を少なくできる。
【００５３】
（１１）変更例
なお、本発明は上記実施形態に限らず、その主旨を逸脱しない限り種々に変更することができる。
【００５４】
例えば、障害物検出部２においては、ミリ波レーダ等のアクティブセンサにより障害物を検出しても良い。
【図面の簡単な説明】
【００５５】
【図１】本発明の一実施形態の障害物追跡装置のブロック図を兼ねたフローチャートである。
【図２】本実施形態における座標系についての説明図である。
【図３】複数の観測位置が存在する場合の説明図である。
【図４】仮説の選択手順の説明図である。
【符号の説明】
【００５６】
１ 画像入力部
２ 障害物検出部
３ 観測位置設定部
４ 仮説生成部
６ 尤度計算部
６ 仮説選択部
７ 信頼性評価部
【技術分野】
【０００１】
本発明は、自動車などの車両に代表される移動体に取り付けられたＴＶカメラから得られる画像を用いて、車両等の障害物を検出し、追跡する障害物追跡装置及びその方法に関するものである。
【背景技術】
【０００２】
従来、車両等の障害物を追跡する方法として、移動物体計測装置がある（特許文献１参照）。これは、画像上でエッジの点数が多い領域を車両として検出し、実空間上における車両車尾の位置を、カルマンフィルタを用いて追跡する。
【０００３】
また、粒子型フィルタに基づく複数の非剛体オブジェクトの追跡という方法もある（非特許文献１参照）。これは、追跡領域を小領域に分割し、各々の小領域の移動仮説を粒子とした粒子型フィルタ（パーティクルフィルタ）を用いて、車両等の物体の追跡を行っている。
【特許文献１】特開平８−９４３２０号公報
【非特許文献１】阿部他，「粒子型フィルタに基づく複数の非剛体オブジェクトの追跡」，電子情報通信学会技術研究報告PRMU2003-241，pp63-66
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかし、特許文献１及び非特許文献１の技術において、複数の障害物を同時に追跡するときに問題点がある。
【０００５】
すなわち、特許文献１の方法では、照明条件の変動や隠れ等により、障害物が未検出になった場合に、他に検出された障害物を対応付けて追跡位置に大きな誤差が生じることや、間違った障害物の追跡をすること等の問題点がある。
【０００６】
また、非特許文献１の方法では、ロバストな追跡が可能になるが、数個の離散的な観測位置に対して、多くのパーティクルを用いる必要があり、無駄が多いときもあり、処理量が増大する等の問題点がある。
【０００７】
そこで、本発明では、上記問題点に鑑みて、検出された障害物候補領域に対してあいまい性を考慮して、障害物の正確な位置を検出する障害物追跡装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明は、移動体に搭載されて障害物が写った時系列画像を取得する画像取得部と、前記時系列画像から現時刻の障害物候補領域を検出する障害物検出部と、前記障害物の前時刻に関する１つあるいは複数の状態仮説を含んだ状態仮説群を格納する状態仮説格納部と、前記障害物候補領域の位置毎の観測仮説と前記障害物が検出されなかったと仮定した場合の観測仮説を合わせた１つあるいは複数の観測仮説を含んだ観測仮説群を生成する観測仮説生成部と、前記状態仮説群に含まれる状態仮説のそれぞれと前記観測仮説群に含まれる観測仮説のそれぞれの組み合わせ毎の尤度をそれぞれ計算する尤度計算部と、前記観測仮説毎の尤度の中で最大の尤度を求め、前記最大の尤度に対応する状態仮説の集合である現在時刻の状態仮説群を用いて、前記状態仮説格納部に格納された前時刻の状態仮説を更新する状態仮説更新部と、前記現在時刻の状態仮説群の中で前記尤度が最大となる状態仮説を、前記障害物が検出された状態として選択する仮説選択部と、を具備することを特徴とする障害物追跡装置である。
【発明の効果】
【０００９】
本発明によれば、車両に搭載された画像入力部により取得される時系列画像から障害物の位置を安定に検出することができる。
【発明を実施するための最良の形態】
【００１０】
以下、本発明の一実施形態の障害物追跡装置を図１から図５を参照して説明する。
【００１１】
本実施形態の障害物追跡装置では、検出された障害物候補領域に対して、あいまい性を考慮して、複数の障害物の観測位置を用いて、複数の観測仮説を設定し、その複数の観測仮説から一番正しい仮説を選択する。
【００１２】
（１）障害物追跡装置の構成
図１に本実施形態の障害物追跡装置のブロック図を兼ねたフローチャートである。
【００１３】
図１に示すように、障害物追跡装置は、画像入力部１、障害物検出部２、観測位置設定部３、仮説生成部４、尤度計算部５、仮説選択部６、信頼性評価部７から構成される。
【００１４】
障害物追跡装置は、例えば、汎用のコンピュータ装置を基本ハードウェアとして用いることでも実現することが可能である。すなわち、障害物検出部２、観測位置設定部３、仮説生成部４、尤度計算部５、仮説選択部６、信頼性評価部７は、上記のコンピュータ装置に搭載されたプロセッサにプログラムを実行させることにより実現することができる。
【００１５】
（２）画像入力部１
画像入力部１は、車両に取り付けられた車両の前方周辺の画像を取得可能なステレオ視のＴＶカメラを有する。
【００１６】
また、図２に本実施形態における座標系を示し、世界座標系の水平方向をＸ、奥行き方向をＺ、画像座標系の横方向をｘ、縦方向をｙとする。また、時刻ｔは、この時系列画像のフレーム数によって決まる。
【００１７】
（３）障害物検出部２
障害物検出部２は、画像入力部１により取得された時系列画像から障害物候補領域を決定する。
【００１８】
障害物検出部２は、ステレオ視の左右画像を用いて、車両等の障害物候補領域を検出する。このとき左右画像の視差から、図２に示すように、画像座標系での位置（ｘ，ｙ）と、世界座標系での位置（Ｘ，Ｚ）が求まる。
【００１９】
ステレオ視の方法としては、非特許文献２（「ステレオ画像を用いた運転支援のための前方状況認識システム」，電子情報通信学会技術研究報告，PRMU97-30，1997，pp39-46）方法がある。
【００２０】
（４）観測位置設定部３
観測位置設定部３は、障害物の現在時刻ｔの観測位置を設定する。その設定方法は次の通りである。
【００２１】
まず、現在追跡している障害物に関して前時刻ｔ−１の障害物の予測位置を仮説生成部４から呼び出す。この予測位置は、後から説明する現在時刻ｔにおける予測位置の求め方と同じ方法で求めた前時刻ｔ−１における予測位置である。
【００２２】
次に、図３に示すように、その予測位置と、障害物検出部２により得られた現在時刻ｔの検出位置を比較し、予測位置と検出位置との距離が閾値以下の場合は、その検出位置を「観測位置」とする。なお、この観測位置は１つとは限らず、前記距離が閾値以下にある検出位置は全て観測位置とする。例えば、下記の数１ではＭ個の観測位置が存在する。
【００２３】
なお、観測位置は、奥行き方向の距離Ｚと水平方向の距離Ｘでが表されているが、以下の説明では、奥行き方向の距離Ｚのみで観測位置を表す。このとき得られるＭ個の観測位置の観測分布は、
【数１】
【００２４】
となり、ガウシアン分布を用いて表す。Ｚｍは観測位置、σＲは観測分布の標準偏差である。
【００２５】
また、観測位置が存在しなかったという観測位置の観測分布は、
【数２】
【００２６】
となり、一様分布を用いて表す。係数αにより大きさが調節される。
【００２７】
このように、観測分布を適切に設定すれば、様々な検出方法によって得られた観測位置でも扱うことが可能である。
【００２８】
（５）仮説生成部４
仮説生成部４は、障害物毎に観測位置設定部３で設定されたＭ個の観測位置に対して、それぞれ観測仮説を生成する。観測仮説０は観測位置なしという仮説である。そして、観測仮説１は観測位置１と設定し、観測仮説ｊは観測位置ｊと設定する（但し、ｊ＝１，２，・・・Ｍである）。
【００２９】
また、仮説生成部４は、Ｎ個の状態仮説、すなわち、下記で説明する状態仮説群を保持している。ここで「状態」とは障害物の位置や速度などの障害物の運動情報に関する状態を意味し、「状態仮説」とは、それら状態に関する仮説をいう。この保持している状態仮説群についてさらに詳しく説明する。
【００３０】
時刻ｔにおいて状態空間中に状態ベクトルｘｔ、状態ベクトルの分散共分散行列Ｐｔ、距離Ｚｔ、観測位置の誤差分散行列Ｒ（標準偏差σＲ）を持つとする。また、状態遷移行列をＡ、状態遷移ノイズをＱとする。等加速度運動モデルを仮定するので、観測行列をＨ、奥行き方向の距離の観測位置をＺｍとすると、
【数３】
【００３１】
となる。状態空間中にＮ個の状態仮説が、ベクトルｘｔ（ｉ）とそれに対応する尤度がπｔ（ｉ）とする状態仮説群｛（ベクトルｘｔ−１（ｉ）；πｔ−１（ｉ））｝（但し、ｉ＝０，１，・・・Ｎ−１である）を持つとする。但し、明細書中では、太字でベクトルを表記できないため、「ベクトルｘ」などと記載する。また、「ベクトルｘ」は、障害物の世界座標系での位置や速度を表し、また、ｘとπの下の添え字は時刻を表し、上の添え字は状態仮説の番号を表す。さらに、「尤度」とは、信頼の度合いをいい、尤度が高ければ、その信頼性も高い。
【００３２】
（６）尤度計算部５
尤度計算部５は、下記で説明する予測、観測、推定のステップを通じて、仮説生成部４で保持している状態仮説群の更新を行い、さらに尤度を計算する。
【００３３】
（６−１）予測ステップ
時刻ｔ−１において推定されたＮ個の状態仮説群｛（ベクトルｘｔ（ｉ）；πｔ（ｉ））｝（但し、ｉ＝０，１，・・・Ｎ−１である）に対して、それぞれカルマンフィルタを用いて予測位置及び事前分布Ｐｐｒｉｏｒｉ（Ｚ）を数４、数６により計算する。Ｐ０（ｉ）は、Ｐｔ−（ｉ）の第１行第１列の成分である。＜ベクトルｘｔ／ｔ−１＞は現在の状態の予測位置、＜ベクトルｘｔ−１／ｔ−１＞は過去の状態の推定位置、Ｐｔ／ｔ−１は、予測された状態の誤差共分散行列、Ｐｔ−１／ｔ−１は過去の状態の誤差共分散行列である。但し、明細書中ではベクトルｘの上に「＾」をつけた記号が記載できないため、＜ベクトルｘ＞と表記する。
【数４】
【数５】
【００３４】
（６−２）観測ステップ
時刻ｔにおいて得られたＭ個の観測位置から観測分布を数１、数２を用いて決定する。
【００３５】
また、カルマンゲインＫｔ（ｉ，ｊ）を数６によって計算する。
【数６】
【００３６】
（６−３）推定ステップ
時刻ｔ−１において推定された状態仮説群｛（ベクトルｘｔ−１（ｉ）；πｔ−１（ｉ））｝（但し、ｉ＝０，１，・・・Ｎ−１である）とＭ個の観測仮説の組み合わせからカルマンフィルタを用いてＮ×Ｍ個の推定位置と事後分布を数７により計算する。
【数７】
【００３７】
推定位置における事前分布の確率を事前確率、推定位置における観測分布の確率を観測確率とすると、ベイズの法則により、事後確率は事前確率と観測確率の積として数８を用いて計算する。
【数８】
【００３８】
また、観測仮説と状態仮説間の組み合わせの重み係数ｗとし、数９により求まった事後確率を新しい状態仮説の尤度として計算する。
【数９】
【００３９】
この観測仮説と状態仮説間の組み合わせの重み係数ｗは、付加的な情報がある場合に計算される。
【００４０】
（７）仮説選択部６
仮説選択部６は、新しい状態仮説から、数１０によりＭ個の観測仮説毎に尤度が最大となる組み合わせを選択する。
【数１０】
【００４１】
新しい状態仮説群は、｛（ベクトルｘｔ（ｊ）；πｔ（ｊ））｝（但し、ｊ＝０，１，・・・Ｍ−１である）として更新する。このときπｔ（ｊ）は、全ての状態仮説における尤度の合計が１となるように正規化を行う。
【００４２】
また、Ｍ個の新しい状態仮説群において、πｔ（ｊ）が最大になるものが、現在時刻ｔにおける障害物の状態仮説として選択される。
【００４３】
（８）信頼性評価部７
信頼性評価部７は、仮説選択部６によって選択された状態仮説から、障害物の位置、速度、加速度などの運動情報を取得する。
【００４４】
また、運動状態の誤差分散が大きさ等から信頼性が評価し、信頼性が低い障害物は追跡を打ち切る。
【００４５】
さらに、障害物検出部２から得られた障害物候補領域が新規である場合は、追跡を開始する。
【００４６】
以上のような処理を時系列画像に対して処理を行い、車両等の障害物を追跡し、正確な位置を検出する。
【００４７】
（９）処理内容
障害物追跡装置の処理内容を図４に基づいて説明する。
【００４８】
時刻ｔ−２では、観測位置２により状態仮説２が選択されているとする。
【００４９】
時刻ｔ−１では、正しい観測位置が得られなかったため、時刻ｔ−２から時刻ｔ−１における状態仮説と観測仮説の組み合わせの尤度を計算すると、状態仮説２と観測仮説０の尤度が最大になり、時刻ｔ−１の状態では観測位置なしという観測仮説０に基づく状態仮説０が選択される。
【００５０】
時刻ｔの状態では、障害物検出部２による結果が良好のため、時刻ｔ−１から時刻ｔにおける状態仮説と観測仮説の組み合わせの尤度を計算すると、状態仮説０と観測仮説２の組み合わせの尤度が最大になり、ステレオ視の結果という観測仮説２に基づく状態仮説２が選択される。
【００５１】
このように、前回の時刻（ｔ−１）の状態仮説群と現在の時刻ｔの観測仮説群とで全ての組み合わせを考え、全ての組み合わせの尤度を計算して、最大尤度になる組み合わせが、正しい対応付けと考える。
【００５２】
（１０）効果
本実施形態の障害物追跡装置であると、複数の仮説を保持しつつ追跡し、尤度が最大となる仮説を選択することにより、障害物の位置を決定する。これにより、障害物候補領域が検出されなかった場合に、他の障害物を間違って追跡する現象が減り、そして、一時的に追跡が間違ったとしても、再び正しい障害物を追跡できる。また、検出される障害物候補領域の数に応じて、適切に仮説を生成するため、処理量を少なくできる。
【００５３】
（１１）変更例
なお、本発明は上記実施形態に限らず、その主旨を逸脱しない限り種々に変更することができる。
【００５４】
例えば、障害物検出部２においては、ミリ波レーダ等のアクティブセンサにより障害物を検出しても良い。
【図面の簡単な説明】
【００５５】
【図１】本発明の一実施形態の障害物追跡装置のブロック図を兼ねたフローチャートである。
【図２】本実施形態における座標系についての説明図である。
【図３】複数の観測位置が存在する場合の説明図である。
【図４】仮説の選択手順の説明図である。
【符号の説明】
【００５６】
１ 画像入力部
２ 障害物検出部
３ 観測位置設定部
４ 仮説生成部
６ 尤度計算部
６ 仮説選択部
７ 信頼性評価部
【特許請求の範囲】
【請求項１】
移動体に搭載されて障害物が写った時系列画像を取得する画像取得部と、
前記時系列画像から現時刻の障害物候補領域を検出する障害物検出部と、
前記障害物の前時刻に関する１つあるいは複数の状態仮説を含んだ状態仮説群を格納する状態仮説格納部と、
前記障害物候補領域の位置毎の観測仮説と前記障害物が検出されなかったと仮定した場合の観測仮説を合わせた１つあるいは複数の観測仮説を含んだ観測仮説群を生成する観測仮説生成部と、
前記状態仮説群に含まれる状態仮説のそれぞれと前記観測仮説群に含まれる観測仮説のそれぞれの組み合わせ毎の尤度をそれぞれ計算する尤度計算部と、
前記観測仮説毎の尤度の中で最大の尤度を求め、前記最大の尤度に対応する状態仮説の集合である現在時刻の状態仮説群を用いて、前記状態仮説格納部に格納された前時刻の状態仮説を更新する状態仮説更新部と、
前記現在時刻の状態仮説群の中で前記尤度が最大となる状態仮説を、前記障害物が検出された状態として選択する仮説選択部と、
を具備する
ことを特徴とする障害物追跡装置。
【請求項２】
前記状態仮説は、前記障害物の位置を含む運動情報及び尤度で表される
ことを特徴とする請求項１記載の障害物追跡装置。
【請求項３】
前記観測仮説は、前記障害物の検出位置毎のガウシアン分布で表される
ことを特徴とする請求項１記載の障害物追跡装置。
【請求項４】
前記障害物が検出されなかったとする観測仮説は、一様分布で表される
ことを特徴とする請求項１記載の障害物追跡装置。
【請求項５】
前記状態仮説群に含まれる状態仮説と前記観測仮説群に含まれる観測仮説との組み合わせ毎の前記尤度は、カルマンフィルタを用いて計算する
ことを特徴とする請求項１記載の障害物追跡装置。
【請求項６】
前記選択された状態仮説から前記障害物の運動情報を求める
ことを特徴とする請求項１記載の障害物追跡装置。
【請求項７】
前記運動情報について信頼性を評価する
ことを特徴とする請求項６記載の障害物追跡装置。
【請求項８】
移動体に搭載されて障害物が写った時系列画像を取得し、
前記時系列画像から現時刻の障害物候補領域を検出し、
前記障害物の前時刻に関する１つあるいは複数の状態仮説を含んだ状態仮説群を格納し、
前記障害物候補領域の位置毎の観測仮説と前記障害物が検出されなかったと仮定した場合の観測仮説を合わせた１つあるいは複数の観測仮説を含んだ観測仮説群を生成し、
前記状態仮説群に含まれる状態仮説のそれぞれと前記観測仮説群に含まれる観測仮説のそれぞれの組み合わせ毎の尤度をそれぞれ計算し、
前記観測仮説毎の尤度の中で最大の尤度を求め、前記最大の尤度に対応する状態仮説の集合である現在時刻の状態仮説群を用いて、前記格納された前時刻の状態仮説を更新し、
前記現在時刻の状態仮説群の中で前記尤度が最大となる状態仮説を、前記障害物が検出された状態として選択する
ことを特徴とする障害物追跡方法。
【請求項９】
前記状態仮説は、前記障害物の位置を含む運動情報及び尤度で表される
ことを特徴とする請求項８記載の障害物追跡方法。
【請求項１０】
前記観測仮説は、前記障害物の検出位置毎のガウシアン分布で表される
ことを特徴とする請求項８記載の障害物追跡方法。
【請求項１１】
前記障害物が検出されなかったとする観測仮説は、一様分布で表される
ことを特徴とする請求項８記載の障害物追跡方法。
【請求項１２】
前記状態仮説群に含まれる状態仮説と前記観測仮説群に含まれる観測仮説との組み合わせ毎の前記尤度は、カルマンフィルタを用いて計算する
ことを特徴とする請求項８記載の障害物追跡方法。
【請求項１３】
前記選択された状態仮説から前記障害物の運動情報を求める
ことを特徴とする請求項８記載の障害物追跡方法。
【請求項１４】
前記運動情報について信頼性を評価する
ことを特徴とする請求項１３記載の障害物追跡方法。
【請求項１５】
移動体に搭載されて障害物が写った時系列画像を取得する画像取得機能と、
前記時系列画像から現時刻の障害物候補領域を検出する障害物検出機能と、
前記障害物の前時刻に関する１つあるいは複数の状態仮説を含んだ状態仮説群を格納する状態仮説格納機能と、
前記障害物候補領域の位置毎の観測仮説と前記障害物が検出されなかったと仮定した場合の観測仮説を合わせた１つあるいは複数の観測仮説を含んだ観測仮説群を生成する観測仮説生成機能と、
前記状態仮説群に含まれる状態仮説のそれぞれと前記観測仮説群に含まれる観測仮説のそれぞれの組み合わせ毎の尤度をそれぞれ計算する尤度計算機能と、
前記観測仮説毎の尤度の中で最大の尤度を求め、前記最大の尤度に対応する状態仮説の集合である現在時刻の状態仮説群を用いて、前記状態仮説格納機能によって格納された前時刻の状態仮説を更新する状態仮説更新機能と、
前記現在時刻の状態仮説群の中で前記尤度が最大となる状態仮説を、前記障害物が検出された状態として選択する仮説選択機能と、
をコンピュータによって実現する
ことを特徴とする障害物追跡プログラム。
【請求項１６】
前記状態仮説は、前記障害物の位置を含む運動情報及び尤度で表される
ことを特徴とする請求項１５記載の障害物追跡プログラム。
【請求項１７】
前記観測仮説は、前記障害物の検出位置毎のガウシアン分布で表される
ことを特徴とする請求項１５記載の障害物追跡プログラム。
【請求項１８】
前記障害物が検出されなかったとする観測仮説は、一様分布で表される
ことを特徴とする請求項１５記載の障害物追跡プログラム。
【請求項１９】
前記状態仮説群に含まれる状態仮説と前記観測仮説群に含まれる観測仮説との組み合わせ毎の前記尤度は、カルマンフィルタを用いて計算する
ことを特徴とする請求項１５記載の障害物追跡プログラム。
【請求項２０】
前記選択された状態仮説から前記障害物の運動情報を求める
ことを特徴とする請求項１５記載の障害物追跡プログラム。
【請求項２１】
前記運動情報について信頼性を評価する
ことを特徴とする請求項２０記載の障害物追跡プログラム。
【請求項１】
移動体に搭載されて障害物が写った時系列画像を取得する画像取得部と、
前記時系列画像から現時刻の障害物候補領域を検出する障害物検出部と、
前記障害物の前時刻に関する１つあるいは複数の状態仮説を含んだ状態仮説群を格納する状態仮説格納部と、
前記障害物候補領域の位置毎の観測仮説と前記障害物が検出されなかったと仮定した場合の観測仮説を合わせた１つあるいは複数の観測仮説を含んだ観測仮説群を生成する観測仮説生成部と、
前記状態仮説群に含まれる状態仮説のそれぞれと前記観測仮説群に含まれる観測仮説のそれぞれの組み合わせ毎の尤度をそれぞれ計算する尤度計算部と、
前記観測仮説毎の尤度の中で最大の尤度を求め、前記最大の尤度に対応する状態仮説の集合である現在時刻の状態仮説群を用いて、前記状態仮説格納部に格納された前時刻の状態仮説を更新する状態仮説更新部と、
前記現在時刻の状態仮説群の中で前記尤度が最大となる状態仮説を、前記障害物が検出された状態として選択する仮説選択部と、
を具備する
ことを特徴とする障害物追跡装置。
【請求項２】
前記状態仮説は、前記障害物の位置を含む運動情報及び尤度で表される
ことを特徴とする請求項１記載の障害物追跡装置。
【請求項３】
前記観測仮説は、前記障害物の検出位置毎のガウシアン分布で表される
ことを特徴とする請求項１記載の障害物追跡装置。
【請求項４】
前記障害物が検出されなかったとする観測仮説は、一様分布で表される
ことを特徴とする請求項１記載の障害物追跡装置。
【請求項５】
前記状態仮説群に含まれる状態仮説と前記観測仮説群に含まれる観測仮説との組み合わせ毎の前記尤度は、カルマンフィルタを用いて計算する
ことを特徴とする請求項１記載の障害物追跡装置。
【請求項６】
前記選択された状態仮説から前記障害物の運動情報を求める
ことを特徴とする請求項１記載の障害物追跡装置。
【請求項７】
前記運動情報について信頼性を評価する
ことを特徴とする請求項６記載の障害物追跡装置。
【請求項８】
移動体に搭載されて障害物が写った時系列画像を取得し、
前記時系列画像から現時刻の障害物候補領域を検出し、
前記障害物の前時刻に関する１つあるいは複数の状態仮説を含んだ状態仮説群を格納し、
前記障害物候補領域の位置毎の観測仮説と前記障害物が検出されなかったと仮定した場合の観測仮説を合わせた１つあるいは複数の観測仮説を含んだ観測仮説群を生成し、
前記状態仮説群に含まれる状態仮説のそれぞれと前記観測仮説群に含まれる観測仮説のそれぞれの組み合わせ毎の尤度をそれぞれ計算し、
前記観測仮説毎の尤度の中で最大の尤度を求め、前記最大の尤度に対応する状態仮説の集合である現在時刻の状態仮説群を用いて、前記格納された前時刻の状態仮説を更新し、
前記現在時刻の状態仮説群の中で前記尤度が最大となる状態仮説を、前記障害物が検出された状態として選択する
ことを特徴とする障害物追跡方法。
【請求項９】
前記状態仮説は、前記障害物の位置を含む運動情報及び尤度で表される
ことを特徴とする請求項８記載の障害物追跡方法。
【請求項１０】
前記観測仮説は、前記障害物の検出位置毎のガウシアン分布で表される
ことを特徴とする請求項８記載の障害物追跡方法。
【請求項１１】
前記障害物が検出されなかったとする観測仮説は、一様分布で表される
ことを特徴とする請求項８記載の障害物追跡方法。
【請求項１２】
前記状態仮説群に含まれる状態仮説と前記観測仮説群に含まれる観測仮説との組み合わせ毎の前記尤度は、カルマンフィルタを用いて計算する
ことを特徴とする請求項８記載の障害物追跡方法。
【請求項１３】
前記選択された状態仮説から前記障害物の運動情報を求める
ことを特徴とする請求項８記載の障害物追跡方法。
【請求項１４】
前記運動情報について信頼性を評価する
ことを特徴とする請求項１３記載の障害物追跡方法。
【請求項１５】
移動体に搭載されて障害物が写った時系列画像を取得する画像取得機能と、
前記時系列画像から現時刻の障害物候補領域を検出する障害物検出機能と、
前記障害物の前時刻に関する１つあるいは複数の状態仮説を含んだ状態仮説群を格納する状態仮説格納機能と、
前記障害物候補領域の位置毎の観測仮説と前記障害物が検出されなかったと仮定した場合の観測仮説を合わせた１つあるいは複数の観測仮説を含んだ観測仮説群を生成する観測仮説生成機能と、
前記状態仮説群に含まれる状態仮説のそれぞれと前記観測仮説群に含まれる観測仮説のそれぞれの組み合わせ毎の尤度をそれぞれ計算する尤度計算機能と、
前記観測仮説毎の尤度の中で最大の尤度を求め、前記最大の尤度に対応する状態仮説の集合である現在時刻の状態仮説群を用いて、前記状態仮説格納機能によって格納された前時刻の状態仮説を更新する状態仮説更新機能と、
前記現在時刻の状態仮説群の中で前記尤度が最大となる状態仮説を、前記障害物が検出された状態として選択する仮説選択機能と、
をコンピュータによって実現する
ことを特徴とする障害物追跡プログラム。
【請求項１６】
前記状態仮説は、前記障害物の位置を含む運動情報及び尤度で表される
ことを特徴とする請求項１５記載の障害物追跡プログラム。
【請求項１７】
前記観測仮説は、前記障害物の検出位置毎のガウシアン分布で表される
ことを特徴とする請求項１５記載の障害物追跡プログラム。
【請求項１８】
前記障害物が検出されなかったとする観測仮説は、一様分布で表される
ことを特徴とする請求項１５記載の障害物追跡プログラム。
【請求項１９】
前記状態仮説群に含まれる状態仮説と前記観測仮説群に含まれる観測仮説との組み合わせ毎の前記尤度は、カルマンフィルタを用いて計算する
ことを特徴とする請求項１５記載の障害物追跡プログラム。
【請求項２０】
前記選択された状態仮説から前記障害物の運動情報を求める
ことを特徴とする請求項１５記載の障害物追跡プログラム。
【請求項２１】
前記運動情報について信頼性を評価する
ことを特徴とする請求項２０記載の障害物追跡プログラム。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２００７−２４９３０９（Ｐ２００７−２４９３０９Ａ）
【公開日】平成１９年９月２７日（２００７．９．２７）
【国際特許分類】
【出願番号】特願２００６−６８４０２（Ｐ２００６−６８４０２）
【出願日】平成１８年３月１３日（２００６．３．１３）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
【公開日】平成１９年９月２７日（２００７．９．２７）
【国際特許分類】
【出願日】平成１８年３月１３日（２００６．３．１３）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
[ Back to top ]