JP2009098877A - 状態変数推定装置 - Google Patents

Abstract

【課題】センサの未検出に対する脆弱性の改善を図った状態変数推定装置を提供すること。
【解決手段】複数の観測を観測セットとし、観測セット内の観測値に対する尤度のうちの最高値を最高尤度として算出し、算出された最高尤度に基づいてパーティクル分布を更新する。これにより、観測値の未検出があった場合でも、観測セット内のその他の観測値に対する尤度（最高尤度）を採用して、パーティクル分布を更新する。このように、最高尤度を観測セットの尤度として計算することで、未検出に対する脆弱性を回避することができる。
【選択図】図３

Description

本発明は、センサによる対象物の観測値に基づいて、対象物の状態変数を推定する状態変数推定装置に関する。

従来、このような分野において、不完全な観測に基づいて、対象物の状態変数を推定する技術として、ＢＳＥ（Bayesian Sequential Estimation）手法がある。このＢＳＥ手法としては、カルマンフィルタ（Kalman Filter）、パーティクルフィルタ（Particle Filter）等が挙げられる。カルマンフィルタは従来から広く用いられ、パーティクルフィルタは、近年注目を集めている。このパーティクルフィルタは、複数のパーティクルを用いて確率分布を表現し、各パーティクルの観測値に対する尤度を用いて確率分布を更新して、対象物の状態変数を推定している（例えば、特許文献１参照）。
特開２００６−３０２１３３号公報

しかしながら、従来のＢＳＥ手法では、確率分布を積算するため、観測値の誤検出や誤差に対して適切に対処できるという利点があるものの、未検出に対しては、対処できないという問題があった。

本発明は、このような課題を解決するために成されたものであり、センサの未検出に対処して、信頼性の向上を図った状態変数推定装置を提供することを目的とする。

本発明による状態変数推定装置は、対象物の観測値に対してパーティクルフィルタ処理を実行し、対象物の状態変数の解を推定する状態変数推定装置であって、対象物の状態変数の解候補である複数のパーティクルを含んだパーティクル分布を生成するパーティクル分布生成手段と、パーティクル分布に含まれる複数のパーティクルを複製するパーティクル複製手段と、パーティクル複製手段によって複製されたパーティクルを、対象物の運動モデルに従い移動させて拡散させるパーティクル拡散手段と、対象物の観測値に対する尤度を算出する尤度算出手段と、対象物に対する異なる時刻の複数の観測を観測セットとし、観測セット内の観測値に対する尤度の最高値を最高尤度として算出する最高尤度算出手段と、最高尤度算出手段によって算出された最高尤度に基づいてパーティクル分布を更新するパーティクル分布更新手段と、を備えたことを特徴としている。

このような状態変数推定装置によれば、異なる時刻の複数の観測を観測セットとし、観測セット内の観測値に対する尤度のうちの最高値を最高尤度として算出し、算出された最高尤度に基づいてパーティクル分布を更新するため、観測値の未検出があった場合でも、観測セット内のその他の観測値に対する尤度（最高尤度）が採用されて、パーティクル分布が更新される。このように、最高尤度を観測セットの尤度として計算することで、センサの未検出に対処して信頼性の向上を図ることができ、未検出に対する脆弱性を回避することができる。

ここで、観測セットにおける観測回数Ｌは、下記式：

（ただし、上記式中、Ｑは観測セットの回数、Ｐ_aimは目標とする未検出率、Ｐは各観測における未検出率である）で表されることが好ましい。なお、観測値が未検出の場合でも、観測回数Ｌとして加算される。

また、本発明による状態変数推定装置は、対象物の観測値に対してパーティクルフィルタ処理を実行し、対象物の状態変数の解を推定する状態変数推定装置であって、対象物の状態変数の解候補である複数のパーティクルを含んだパーティクル分布を生成するパーティクル分布生成手段と、パーティクル分布に含まれる複数のパーティクルを複製するパーティクル複製手段と、パーティクル複製手段によって複製されたパーティクルを、対象物の運動モデルに従い移動させて拡散させるパーティクル拡散手段と、対象物の観測値に対する尤度を算出する尤度算出手段と、対象物に対する異なるセンサによる複数の観測を観測セットとし、観測セット内の観測値に対する尤度の最高値を最高尤度として算出する最高尤度算出手段と、最高尤度算出手段によって算出された最高尤度に基づいてパーティクル分布を更新するパーティクル分布更新手段と、を備えたことを特徴としている。

このような状態変数推定装置によれば、異なるセンサによる複数の観測を観測セットとし、観測セット内の観測値に対する尤度のうちの最高値を最高尤度として算出し、算出された最高尤度に基づいてパーティクル分布を更新するため、観測値の未検出があった場合でも、観測セット内のその他の観測値に対する尤度（最高尤度）が採用されて、パーティクル分布が更新される。このように、最高尤度を観測セットの尤度として計算することで、センサの未検出に対処して信頼性の向上を図ることができ、未検出に対する脆弱性を回避することができる。

本発明による状態変数推定装置によれば、センサの未検出に対処して信頼性の向上を図ることができ、未検出に対する脆弱性を回避することができる。

以下、本発明の状態変数推定装置の好適な実施形態について図面を参照しながら説明する。なお、図面の説明において、同一または相当要素には同一の符号を付し、重複する説明は省略する。

（第１実施形態）
まず、第１実施形態に係る情報処理装置（状態変数推定装置）について説明する。図１は、本発明の第１実施形態に係る情報処理装置を示すブロック図である。

図１に示す情報処理装置１０は、被検出物（対象物）の物理的情報（例えば光、温度、圧力や位置など）を検出し、その検出結果である観測値を出力するセンサ１２と、センサ１２の観測値を処理して被検出物の状態変数を求める電子制御ユニット（以下「ＥＣＵ」という）１４とを備えている。情報処理装置１０は、例えば、車両などに搭載され、車両周辺の歩行者や他車両などの被検出物を検出して、被検出物の位置及び移動を認識するものである。

センサ１２は、例えばＣＣＤカメラであり、車両のフロントウィンドウ上部（例えば、バックミラーの裏側）に設置され、前方に向けられている。センサ１２は、車両前方の風景を撮像して画像情報を取得する。センサ１２により取得された画像情報は、ＥＣＵ１４に出力される。

ＥＣＵ１４は、演算を行うマイクロプロセッサ、マイクロプロセッサに各処理を実行させるためのプログラム等を記憶するＲＯＭ、演算結果などの各種データを記憶するＲＡＭおよび１２Ｖバッテリによってその記憶内容が保持されるバックアップＲＡＭ等により構成されている。

ＥＣＵ１４は、被検出物の観測値に対してパーティクルフィルタ処理を実行して、被検出物の状態変数の解を推定するものである。情報処理装置１０では、複数回の観測を「観測セット」として設定し、複数の観測値に対してそれぞれ尤度を算出する。ＥＣＵ１４では、観測セットの観測値に対する尤度のうちで、最高である尤度を最高尤度として算出する。

ＥＣＵ１４では、記憶部であるＲＯＭに記憶されたプログラムを実行することで、観測値検出部１６、パーティクル生成部１８、パーティクル複製部２０、パーティクル拡散部２２、尤度算出部２４、尤度更新部２６、パーティクル分布更新部２８、及び観測セット数設定部３０が構築されている。

観測値検出部１６は、センサから入力された情報に基づいて、所定の単位時間毎に被検出物の観測値を検出する。パーティクル生成部１８は、被検出物の状態変数の解候補である複数のパーティクルを含んだパーティクル分布を生成する（図２（ａ））。パーティクル生成部１８は、初期パーティクル｛ｘ^（0,i）｝_i=1…Nを相空間全域にばらまいてパーティクル分布を生成する。図２に示す相空間は、横軸に被検出物の速度を示し、縦軸に被検出物の位置を示している。

パーティクル複製部２０は、それぞれのパーティクルｘ^（t，1）をＭ個ずつ複製する（｛ｘ^（t,i,j）｝_{i=1…N,j＝1…M}、図２（ｂ））。パーティクル拡散部２２は、被検出物の運動モデルp(x_t|x_t-1)に従って、各パーティクルｘ_{（ｔ,i,j）}を移動させる（｛ｘ^{（t+1,i,j）}｝_{i=1…N,j＝1…M}、図２（ｃ））。ここでは、被検出物の運動モデルを等速直線運動として取り扱うこととしている。そして、パーティクル拡散部２２は、運動モデルにノイズ項（変動分）を考慮して、各パーティクルを移動させる。なお、被検出物の運動モデルは、等速直線運動に限定されない。

尤度算出部２４は、被検出物の観測値y_t+1に対する各パーティクルｘ^{（t+1,i,j）}の尤度p(y_t+1|ｘ^{（t+1,i,j）})を算出する（図２（ｄ））。尤度更新部２６は、被検出物に対する異なる時刻の複数の観測を観測セットとし、観測セット内の観測値に対する尤度の最高値を最高尤度として算出する最高尤度算出手段として機能する。尤度更新部２６では、算出された尤度p(y_t+1|ｘ^{（t+1,i,j）})と過去の観測値に対する最高尤度p_max(ｘ^{（t+1,i,j）})との比較を行う。なお、過去の観測値に対する最高尤度は、例えば、記憶部に記憶されている。算出された尤度p(y_t+1|ｘ^{（t+1,i,j）})が過去の観測値に対する最高尤度p_max(ｘ^{（t+1,i,j）})より大きい場合には、尤度p(y_t+1|ｘ^{（t+1,i,j）})を最高尤度として更新する。

パーティクル分布更新部２８は、最高尤度p_max(ｘ^{（t+1,i,j）})に比例する確率で、パーティクル｛ｘ^{（t+1,i,j）}｝_{i=1…N,j＝1…M}の中からＮ個をランダムに選択し、選択されたものをパーティクル｛ｘ^（0,i）｝_i=1…Nとして更新する（図２（ｅ））。

観測セット数設定部２８は、観測セットにおける観測回数Ｌを設定する。観測セットにおける観測回数Ｌは、下記式：

（ただし、上記式中、Ｑは観測セットの回数、Ｐ_aimは目標とする未検出率、Ｐは各観測における未検出率である）によって、算出される。

なお、全観測セットで未検出しない確率は、（１−ｐ^Ｌ）Ｑ≒１−Ｑｐ^Ｌ（ただし、ｐ^Ｌは観測セットで未検出となる確率＝観測セット中の全ての観測で未検出となる確率、１−Ｐ^Ｌは、観測セットで未検出とならない確率である）によって表現され、（１−ｐ^Ｌ）Ｑ≒１−Ｑｐ^Ｌとなればよいので、上記式（１）を満たせばよい（ただし、logｐ、logｐ_aimは負である）。例えば、各観測における未検出率ｐ＝０．０１、目標とする未検出率Ｐ_aim＝１０^−１０、観測セットの回数Ｑ＝１０である場合、Ｌ＝６となる。

次に、ＥＣＵ１４で実行される制御処理について、図３を参照して説明する。図３は、パーティクルフィルタ処理の一例の動作手順を示すフローチャートである。まず、ＥＣＵ１４は、初期パーティクル｛ｘ^（0,i）｝_i=1…Nをランダムに生成してパーティクル分布を生成する（Ｓ１）。次に、ＥＣＵ１４は、各パーティクルｘ^（t，1）をＭ個ずつ複製（｛ｘ^（t,i,j）｝_{i=1…N,j＝1…M}）する（Ｓ２）。

続いて、ＥＣＵ１４は、過去の観測値に対する最高尤度p_max(ｘ^{（t+1,i,j）})をリセットする（Ｓ３）。なお、初回など、過去の観測値に対する最高尤度p_max(ｘ^{（t+1,i,j）})が記憶されていない場合には、リセットしない。

次に、ＥＣＵ１４は、運動モデルp(x_t|x_t-1)に従って各パーティクルｘ_（t,i,j）を移動（｛ｘ^{（t+1,i,j）}｝_{i=1…N,j＝1…M}）させる（Ｓ４）。続いて、ＥＣＵ１４は、観測値y_t+1に対する各パーティクルｘ^{（ｔ+1,i,j）}の尤度p(y_t+1|ｘ^{（t+1,i,j）})を算出する（Ｓ５）。なお、観測値が未検出の場合には、尤度を算出せずに、ステップ６に進む。

次に、ＥＣＵ１４は、観測値y_t+1に対する各パーティクルｘ^{（t+1,i,j）}の尤度p(y_t+1|ｘ^{（t+1,i,j）})が観測セット内の過去の観測値に対する最高尤度p_max(ｘ^{（t+1,i,j）})より大きいか否かを判定する（Ｓ６）。尤度p(y_t+1|ｘ^{（t+1,i,j）})が記憶部に記憶されている最高尤度p_max(ｘ^{（t+1,i,j）})より大きいと判定された場合には、ステップ７に進み、尤度p(y_t+1|ｘ^{（t+1,i,j）})が最高尤度p_max(ｘ^{（t+1,i,j）})より大きいと判定されなかった場合には、ステップ８に進む。

ステップ７では、ＥＣＵ１４は、記憶部に記憶されている最高尤度p_max(ｘ^{（t+1,i,j）})を尤度p(y_t+1|ｘ^{（t+1,i,j）})に更新する。

ステップ８では、ＥＣＵ１４は、一つの観測セットにおける観測回数分の観測ができたか否かを判定する。情報処理装置１０では、単位時間毎に観測を実行しているので、設定された観測回数Ｌに対応する時間が経過した場合には、観測回数分の観測ができたと判定し、ステップ９に進む。観測回数に対応する時間が経過していない場合には、ステップ４に進み、ステップ４〜７の処理を繰り返し、観測回数分の観測が実行された後にステップ９に進む。なお、観測を実行して未検出であった場合も、観測回数としてカウントされている。

ステップ９では、ＥＣＵ１４は、最高尤度p_max(ｘ^{（t+1,i,j）})に比例する確率で｛ｘ^{（t+1,i,j）}｝_{i=1…N,j＝1…M}の中からN個をランダムに選択し、これを｛ｘ（0,i）｝_i=1…Nとして、パーティクル分布の更新を行い、ステップ２〜９の処理を繰り返す。

このような情報処理装置１０では、異なる時刻の複数の観測を観測セットとし、観測セット内の観測値に対する尤度のうちの最高値を最高尤度として算出し、算出された最高尤度に基づいてパーティクル分布を更新している。一般的に、センサによる観測では、「未検出」の場合は、偶然観測できない場合である。そのため、偶然観測できないことは、稀であり、観測セット内で最も良い観測をその物体（対象物）の観測とみなし、観測セットをパーティクルフィルタの一観測とみなすことができる。従って、観測値の未検出があった場合でも、観測セット内のその他の観測値に対する尤度（最高尤度）が採用されて、パーティクル分布が更新される。最高尤度を観測セットの尤度として計算することで、未検出に対する脆弱性が回避され、装置の信頼性を向上させることができる。

（第２実施形態）
次に、第２実施形態に係る情報処理装置について説明する。図４は、本発明の第２実施形態に係る情報処理装置を示すブロック図である。この第２実施形態に係る情報処理装置４０が第１実施形態に係る情報処理装置１０と異なる点は、複数の異なる種類のセンサを備え、異なるセンサによる複数の観測を観測セットとしている点である。

図４に示す情報処理装置４０は、例えば車両に搭載され、他車両、歩行者等を含む被検出物を検出し、被検出物の位置、相対速度等の状態変数を推定するものである。情報処理装置４０は、ミリ波レーダ４２、モノクロカメラ４４、画像処理ＥＣＵ４６、レーザレーダ４８、ＥＣＵ５０を備えている。

ミリ波レーダ４２は、ミリ波を利用して対象物を検出するレーダである。ミリ波レーダ４２は、自車両の前面の中央に取り付けられている。ミリ波レーダ４２では、自車両から対象物までの距離（dist[i]）、対象物の方向（dir[i]）、自車両に対する対象物の相対速度（V_r[i]）に関する情報を観測値として、ＥＣＵ５０に出力する。

モノクロカメラ４４は、自車両の車室前方中央に配置され、フロントガラス越しに自車両前方の画像を取得するものである。画像処理ＥＣＵ４６は、モノクロカメラ４４から入力された映像信号に基づいて、画像処理を行い、自車両前方の対象物を検出する。画像処理ＥＣＵ４６では、対象物の中心位置（u[i],v[i]）に関する情報を観測値として、ＥＣＵ５０に出力する。

レーザレーダ４８は、レーザを利用して対象物を検出するレーダである。レーザレーダ４８は、自車両の前面の中央に取り付けられている。レーザレーダ４８では、自車両から対象物までの距離（dist[i]）、対象物の方向（dir[i]）に関する情報を観測値として、ＥＣＵ５０に出力する。

ＥＣＵ５０は、被検出物の観測値に対してパーティクルフィルタ処理を実行して、被検出物の状態変数の解を推定するものである。情報処理装置４０では、複数回の観測を「観測セット」として設定し、複数の観測値に対してそれぞれ尤度を算出する。ＥＣＵ１４では、観測セットの観測値に対する尤度のうちで、最高である尤度を最高尤度として算出する。

ＥＣＵ５０では、記憶部であるＲＯＭに記憶されたプログラムを実行することで、観測値検出部１６、パーティクル生成部１８、パーティクル複製部２０、パーティクル拡散部２２、尤度算出部２４、尤度更新部２６、パーティクル分布更新部２８、及び観測セット数設定部３０が構築される。

尤度算出部２４では、ミリ波レーダ４２の観測値yに対する各パーティクルｘ^（t,i,j）の尤度_{ミリ波レーダ}(観測値：dist,dir,v_r|ｘ^（t,i,j）)、モノクロカメラ４４の観測値y_tに対する各パーティクルｘ^（t,i,j）の尤度_{モノクロカメラ}(観測値：u,v|ｘ^（t,i,j）)、レーザレーダ４２の観測値y_tに対する各パーティクルｘ^（t,i,j）の尤度_{レーザレーダ}(観測値：dist,dir|ｘ^（t,i,j）)を算出する。

尤度更新部２６は、被検出物に対する異なるセンサの複数の観測を観測セットとし、観測セット内の観測値に対する尤度の最高値を最高尤度として算出する最高尤度算出手段として機能する。尤度更新部２６では、算出された尤度_{ミリ波レーダ}(観測値：dist,dir,v_r|ｘ^（t,i,j）)、尤度_{モノクロカメラ}(観測値：u,v|ｘ^（t,i,j）)、及び尤度_{レーザレーダ}(観測値：dist,dir|ｘ^（t,i,j）)のうちの最高値を最高尤度として更新する。

このような情報処理装置４０では、異なるセンサ４２，４４，４８による複数の観測を観測セットとし、観測セット内の観測値に対する尤度のうちの最高値を最高尤度として算出し、算出された最高尤度に基づいてパーティクル分布を更新している。そのため、観測値の未検出があった場合でも、観測セット内のその他の観測値に対する尤度（最高尤度）が採用されて、パーティクル分布が更新される。このように、最高尤度を観測セットの尤度として計算することで、未検出に対する脆弱性が回避される。なお、複数のセンサは、同じ種類のセンサを複数備えるものでもよい。

以上、本発明をその実施形態に基づき具体的に説明したが、本発明は、上記実施形態に限定されるものではない。例えば、センサによる検出対象物は、他車両、歩行者等に限定されず、例えば、道路上の障害物、白線、建築物を始め、その他の物であってもよい。また、状態変数推定装置は、飛行機、船舶、ロボット等、その他の機器に搭載されていてもよい。

また、観測セットの観測回数（観測セットの長さ）は、実験等によって算出されたデータ等に基づいて設定してもよい。

本発明の第１実施形態に係る情報処理装置を示すブロック図である。 パーティクル分布の一例を示す図である。 パーティクルフィルタ処理の動作手順を示すフローチャートである。 本発明の第２実施形態に係る情報処理装置を示すブロック図である。

符号の説明

１０，４０…情報処理装置、１２…センサ、１４、５０…電子制御ユニット（ＥＣＵ）、１６…観測値検出部、１８…パーティクル生成部、２０…パーティクル複製部、２２…パーティクル拡散部、２４…尤度算出部、２６…尤度更新部、２８…パーティクル分布更新部、３０…観測セット数設定部、４２…ミリ波レーダ、４４…モノクロカメラ、４６…画像処理ＥＣＵ、４８…レーザレーダ。

Claims (3)

1. 対象物の観測値に対してパーティクルフィルタ処理を実行し、前記対象物の状態変数の解を推定する状態変数推定装置であって、
   前記対象物の状態変数の解候補である複数のパーティクルを含んだパーティクル分布を生成するパーティクル分布生成手段と、
   前記パーティクル分布に含まれる前記複数のパーティクルを複製するパーティクル複製手段と、
   前記パーティクル複製手段によって複製された前記パーティクルを、前記対象物の運動モデルに従い移動させて拡散させるパーティクル拡散手段と、
   前記対象物の観測値に対する尤度を算出する尤度算出手段と、
   前記対象物に対する異なる時刻の複数の観測を観測セットとし、前記観測セット内の観測値に対する尤度の最高値を最高尤度として算出する最高尤度算出手段と、
   前記最高尤度算出手段によって算出された前記最高尤度に基づいて前記パーティクル分布を更新するパーティクル分布更新手段と、を備えたことを特徴とする状態変数推定装置。
2. 前記観測セットにおける観測回数Ｌは、下記式：
   

   

   （ただし、上記式中、Ｑは観測セットの回数、Ｐ_aimは目標とする未検出率、Ｐは各観測における未検出率である）
   で表されることを特徴とする請求項１記載の状態変数推定装置。
3. 対象物の観測値に対してパーティクルフィルタ処理を実行し、前記対象物の状態変数の解を推定する状態変数推定装置であって、
   前記対象物の状態変数の解候補である複数のパーティクルを含んだパーティクル分布を生成するパーティクル分布生成手段と、
   前記パーティクル分布に含まれる前記複数のパーティクルを複製するパーティクル複製手段と、
   前記パーティクル複製手段によって複製された前記パーティクルを、前記対象物の運動モデルに従い移動させて拡散させるパーティクル拡散手段と、
   前記対象物の観測値に対する尤度を算出する尤度算出手段と、
   前記対象物に対する異なるセンサによる複数の観測を観測セットとし、前記観測セット内の観測値に対する尤度の最高値を最高尤度として算出する最高尤度算出手段と、
   前記最高尤度算出手段によって算出された前記最高尤度に基づいて前記パーティクル分布を更新するパーティクル分布更新手段と、を備えたことを特徴とする状態変数推定装置。

Images