(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-08-13
(45)【発行日】2024-08-21
(54)【発明の名称】目標状態推定方法および目標状態推定装置
(51)【国際特許分類】
G01S 13/08 20060101AFI20240814BHJP
G01S 15/08 20060101ALI20240814BHJP
【ＦＩ】
G01S13/08
G01S15/08
【請求項の数】
5
(21)【出願番号】P 2021029513
(22)【出願日】2021-02-26
(65)【公開番号】P2022130874
(43)【公開日】2022-09-07
【審査請求日】2023-11-09
(73)【特許権者】
【識別番号】000000295
【氏名又は名称】沖電気工業株式会社
(74)【代理人】
【識別番号】110001461
【氏名又は名称】弁理士法人きさ特許商標事務所
(72)【発明者】
【氏名】武田 啓之
【審査官】東 治企
(56)【参考文献】
【文献】特開２０１７－２１１３４８（ＪＰ，Ａ）
【文献】Jason M. Aughenbaugh et al.，"Measurement-Guided Likelihood Sampling for Grid-Based Bayesian Tracking"，Journal of Advances in Information Fusion (JAIF)，2010年12月，Vol.5, No.2，pp.108-127，ISSN: 1557-6418, < URL; https://isif.org/issue/jaif-volume-5-issue-2 >
【文献】Lawrence D. Stone et al.，"Technical Documentation of Nodestar"，NAVAL RESEARCH LAB WASHINGTON DC，1995年12月11日，< URL; https://apps.dtic.mil/sti/citations/ADA302458 >
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０１Ｓ ７／００－７／６４
Ｇ０１Ｓ １３／００－１７／９５
ＪＳＴＰｌｕｓ／ＪＭＥＤＰｌｕｓ／ＪＳＴ７５８０（ＪＤｒｅａｍＩＩＩ）
ＩＥＥＥ Ｘｐｌｏｒｅ
(57)【特許請求の範囲】
【請求項1】
方位と距離とを含む観測値から、等速直線運動を行うと仮定した目標についての位置座標と速力とを含む状態量を推定する目標状態推定方法であって、複数の異なる前記速力ごとの前記位置座標を離散化して表す状態空間から、複数の異なる前記速力ごとに、前記目標が解析時間内に移動する小領域の範囲と分解能とを設定し、
複数の異なる前記小領域ごとの前記範囲と前記分解能とに基づいてベイズ推定を再帰的に行い、
前記ベイズ推定の結果から前記目標の前記状態量を推定する
目標状態推定方法。
【請求項2】
最初の前記観測値を用いて、複数の異なる前記速力における前記状態空間の前記範囲と前記分解能とに基づく尤度を計算し、前記尤度から、複数の異なる前記速力ごとの前記目標の概略位置を推定し、
前記概略位置と、前記解析時間と、前記速力とから、複数の異なる前記速力ごとに複数の異なる前記小領域の前記範囲を設定する
請求項１に記載の目標状態推定方法。
【請求項3】
複数の異なる前記小領域の前記範囲を設定する際にマージンを付与する請求項２に記載の目標状態推定方法。
【請求項4】
前記ベイズ推定の再帰処理の周期と、複数の異なる前記速力とから、複数の異なる前記小領域の前記分解能を設定する請求項１～３の何れか１項に記載の目標状態推定方法。
【請求項5】
方位と距離とを含む観測値から、等速直線運動を行うと仮定した目標についての位置座標と速力とを含む状態量を推定する目標状態推定装置であって、複数の異なる前記速力ごとの前記位置座標を離散化して表す状態空間から、複数の異なる前記速力ごとに、前記目標が解析時間内に移動する小領域の範囲と分解能とを設定する小領域設定部と、
複数の異なる前記小領域ごとの前記範囲と前記分解能とに基づいてベイズ推定を再帰的に行う再帰処理部と、
前記ベイズ推定の結果から前記目標の前記状態量を推定する状態推定部と、を備える
目標状態推定装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、ベイズ推定を用いた目標状態推定方法および目標状態推定装置に関するものである。
【背景技術】
【0002】
従来、目標の状態量を推定する目標状態推定方法、及びこの目標状態推定方法を行う目標状態推定装置が知られている。このような目標状態推定方法では、ベイズ推定が用いられることがある。先ず、一般的なベイズ推定について述べる。推定したい状態量のベクトルｕ（ｕ１，ｕ２，ｕ３，…，ｕ４）、及び観測値ｚをとすると、ベイズの定理に従い、事後確率密度関数は下記式（１）で表現される。ここで、左辺ｐ（ｕ｜ｚ）は、事後確率である。また、右辺分子にあるｐ（ｕ）は状態量ｕの事前確率であり、ｐ（ｚ｜ｕ）は状態量ｕ、及び観測値ｚから計算される尤度である。そして、分母は、正規化項である。観測値ｚが目標状態推定装置に入力されるたびに、ベイズ推定、即ち式（１）に基づく演算が再帰的に処理され、より正確な事後確率、即ち観測値ｚを前提とした状態量ｕが得られる。以下の説明において、ベイズ推定の再帰的な処理を単に、再帰処理と称することがある。
【0003】
【数1】
【0004】
特許文献１には、センサが観測した目標の特徴量を用いてベイズ推定を行い、目標の種別を識別する類識別装置が開示されている。また、非特許文献１には、離散的なグリッド位置に限定した状態空間に基づいて状態量を推定するグリッドベース方式での目標状態推定方法が開示されている。非特許文献１の目標状態推定方法では、ベイズ推定の再帰処理ごとに、グリッドの再設計を行う。グリッドの再設計では、事後確率の低いグリッドを削除し、グリッドの範囲を縮小した後、再設計後のグリッド数がオリジナルのグリッド数となるように補間処理を行うものである。これにより、非特許文献１は、グリッドの再設計を行い、状態空間のグリッド範囲を縮小させて計算量を削減しようとするものである。
【先行技術文献】
【特許文献】
【0005】
【文献】特開２０１６－１３３４４１号公報
【非特許文献】
【0006】
【文献】Stone, Lawrence D. , Corwin, Thomas L. , Hofmann, James B. , “Technical Documentation of Nodestar” , NAVAL RESEARCH LAB WASHINGTON DC, 1995.12.11, p.13-19
【発明の概要】
【発明が解決しようとする課題】
【0007】
しかしながら、非特許文献１の目標状態推定方法では、ベイズ推定の再帰処理のたびにグリッドの再設計が行われる。グリッド再設計には、補間処理が含まれているため、再帰処理を繰り返すことで、目標状態推定の精度が低下してしまう。
【0008】
ここで、精度の低下を抑制する方法としては、次の２つの方法が考えられる。１つ目は、再設計後のグリッド数を増加させ、補間処理の精度を高くする方法である。しかしながら、この場合、グリッド数が増加したことで、補間処理での計算量が増加してしまう。また、２つ目は、１回のグリッド再設計で削除するグリッドの数を抑える方法である。しかしながら、この場合では、１回のグリッド再設計で削除するグリッドの数が多い場合と同じ精度を得るためには再帰処理の繰り返し回数が多くなってしまい、計算量が増加してしまう。
【0009】
本発明は、上記のような課題を背景としたものであり、精度の低下を抑制しつつ、計算量を削減させた目標状態推定方法、及び目標状態推定装置を提供することを目的とする。
【課題を解決するための手段】
【0010】
本発明に係る目標状態推定方法は、方位と距離とを含む観測値から、等速直線運動を行うと仮定した目標についての位置座標と速力とを含む状態量を推定する目標状態推定方法であって、複数の異なる速力ごとの位置座標を離散化して表す状態空間から、複数の異なる速力ごとに、目標が解析時間内に移動する小領域の範囲と分解能とを設定し、複数の異なる小領域ごとの範囲と分解能とに基づいてベイズ推定を再帰的に行い、ベイズ推定の結果から目標の状態量を推定する。
【0011】
本発明に係る目標状態推定装置は、方位と距離とを含む観測値から、等速直線運動を行うと仮定した目標についての位置座標と速力とを含む状態量を推定する目標状態推定装置であって、複数の異なる速力ごとの位置座標を離散化して表す状態空間から、複数の異なる速力ごとに、目標が解析時間内に移動する小領域の範囲と分解能とを設定する小領域設定部、複数の異なる小領域ごとの範囲と分解能とに基づいてベイズ推定を再帰的に行う再帰処理部と、ベイズ推定の結果から目標の状態量を推定する状態推定部と、を備える。
【発明の効果】
【0012】
本発明によれば、複数の速力のそれぞれにおける小領域の範囲及び分解能が設定される。このため、速力ごとに必要なグリッドの範囲、及び分解能が設定されているため、計算量が削減される。また、ベイズ推定を再帰的に処理する度に、グリッド調整を実施する必要がなく、目標状態推定の精度の低下が抑制される。以上のように、本発明における目標状態推定方法によれば、精度の低下を抑制しつつ、計算量を削減させることができる。
【図面の簡単な説明】
【0013】
【図1】実施の形態１に係る目標状態推定装置１を示す機能ブロック図である。
【図2】目標状態推定方法における特徴量を説明するための図である。
【図3】実施の形態１に係る小領域設定部１３を示す機能ブロック図である。
【図4】実施の形態１に係る状態空間の領域Ａ１における尤度を示す図である。
【図5】尤度計算を説明するための図である。
【図6】実施の形態１に係る小領域における尤度を示す図である。
【図7】実施の形態１に係る小領域について説明するための図である。
【図8】実施の形態１に係る小領域について説明するための図である。
【図9】実施の形態１に係る目標状態推定方法を示すフローチャートである。
【図10】比較例２に係るグリッド再設計を説明するための図である。
【図11】比較例２に係る外周加算から最小辺削除までの流れを説明するための図である。
【発明を実施するための形態】
【0014】
実施の形態１．
図１は、実施の形態１に係る目標状態推定装置１を示す機能ブロック図である。目標状態推定装置１は、センサ９１で検出した目標との距離及び方位に基づいて、目標状態推定方法を実施することで、目標の現在位置及び速力を推定し、表示装置９２に出力するものである。実施の形態１の目標状態推定装置の目標状態推定方法では、グリッドベース方式が用いられている。センサ９１は、例えば、ソーナー又はレーダー等である。表示装置９２は、例えば、ディスプレイである。なお、目標状態推定装置１がセンサ９１又は表示装置９２を備えるように構成してもよい。図１に示すように、目標状態推定装置１は、処理装置２、及び記憶装置３を備える。処理装置２は、機能部として、入力部１１、状態空間設定部１２、小領域設定部１３、事前確率計算部１４、再帰処理部１５、状態推定部１６、及び出力部１７を備える。記憶装置３は、各機能部での処理結果等を記憶する装置である。
図１は、実施の形態１に係る目標状態推定装置１を示す機能ブロック図である。目標状態推定装置１は、センサ９１で検出した目標との距離及び方位に基づいて、目標状態推定方法を実施することで、目標の現在位置及び速力を推定し、表示装置９２に出力するものである。実施の形態１の目標状態推定装置の目標状態推定方法では、グリッドベース方式が用いられている。センサ９１は、例えば、ソーナー又はレーダー等である。表示装置９２は、例えば、ディスプレイである。なお、目標状態推定装置１がセンサ９１又は表示装置９２を備えるように構成してもよい。図１に示すように、目標状態推定装置１は、処理装置２、及び記憶装置３を備える。処理装置２は、機能部として、入力部１１、状態空間設定部１２、小領域設定部１３、事前確率計算部１４、再帰処理部１５、状態推定部１６、及び出力部１７を備える。記憶装置３は、各機能部での処理結果等を記憶する装置である。
【0015】
なお、目標状態推定方法を専用のハードウェア又はＣＰＵで実現するその他の方法としてパーティクルフィルタが知られている。パーティクルフィルタは、状態空間に粒子をばらまき、粒子の密度で確率を表現する方法である。
【0016】
処理装置２は、専用のハードウェア、又は記憶装置３に格納されるプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ又はプロセッサともいう）で構成される。処理装置２が専用のハードウェアである場合、処理装置２は、例えば、単一回路、複合回路、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、又はこれらを組み合わせたものが該当する。処理装置２が実現する各機能部のそれぞれを、個別のハードウェアで実現してもよいし、各機能部を一つのハードウェアで実現してもよい。
【0017】
処理装置２がＣＰＵの場合、処理装置２が実行する各機能は、ソフトウェア、ファームウェア、又はソフトウェアとファームウェアとの組み合わせにより実現される。ソフトウェア及びファームウェアはプログラムとして記述され、記憶装置３に格納される。ＣＰＵは、記憶装置３に格納されたプログラムを読み出して実行することにより、各機能を実現する。ここで、記憶装置３は、例えば、ＲＡＭ、ＲＯＭ、フラッシュメモリ、ＥＰＲＯＭ、ＥＥＰＲＯＭ等の不揮発性又は揮発性の半導体メモリである。なお、処理装置２の機能の一部を専用のハードウェアで実現し、一部をソフトウェア又はファームウェアで実現するようにしてもよい。
【0018】
入力部１１は、センサ９１からの観測値ｚを受け付ける。観測値ｚは、センサ９１から見た目標の方位θ、及びセンサ９１と目標との距離ｒである。
【0019】
状態空間設定部１２は、初期の状態部分空間（以降は単に状態空間と簡略して記す）の設定を行う。状態量ｕ（ｕ１，ｕ２，ｕ３，ｕ４）は、（ｘ，ｙ，ｖｘ，ｖｙ）の４種類である。図２は、目標状態推定方法における特徴量を説明するための図である。図２（ａ）に示すように、ｘ及びｙは、位置座標を示している。なお、位置座標は、単に位置と説明することがある。また、図２（ｂ）に示すように、ｖｘは、速力のｘ方向の成分であり、ｖｙは、速力のｙ方向の成分である。
【0020】
状態空間設定部１２は、具体的に、初期の状態空間の範囲及び分解能を設定する。ここで、実施の形態１における初期の状態空間は、速力（ｖｘ，ｖｙ）ごとの位置座標（ｘ，ｙ）を離散化させたグリッドによって表される。初期の状態空間の範囲は、目標状態推定装置１が取り扱う範囲に応じて設定され、状態空間の分解能は、後述する状態遷移を表現できる程度の所定の細かさを有する。また、状態空間の分解能は、グリッドの間隔の細かさである。状態空間の分解能の設定では、所定よりも粗い分解能を設定し、補間処理を実施するようにしてもよい。状態空間の分解能は、下記式（２）～（５）によって表される。なお、肩文字の（ｄ）は、離散グリッドであることを表している。
【0021】
【数2】
【0022】
【数3】
【0023】
【数4】
【0024】
【数5】
【0025】
例えば、位置座標（ｘ，ｙ）の範囲は、１０ｋｍオーダー、即ち１０ｋｍ×１０ｋｍに設定される。また、速力（ｖｘ，ｖｙ）の範囲は、２０ｋｔ×２０ｋｔに設定される。位置座標（ｘ，ｙ）の分解能は、後述の概略位置推定部２２において目標の大まかな位置を求めるに十分であれば、比較的粗くてよい。例えば、位置座標（ｘ，ｙ）の分解能は、（１０ｋｍ，１０ｋｍ）に対して、１／１００（１００ｍ）の分解能に設定される。速力（ｖｘ，ｖｙ）の分解能は、アプリケーションで推定したい速力の細かさに応じて設定される。例えば、比較的細かく速力を推定したい場合は、１ｋｔ間隔に設定される。また、比較的粗く速力を推定したい場合は、５ｋｔ間隔に設定される。
【0026】
小領域設定部１３は、小領域の範囲、及び小領域の分解能を設定する。小領域は、初期の状態空間のうち、ある速力での位置座標を示した領域から、解析時間、即ち目標状態推定方法を実施する時間内で、当該速力を有する目標が移動する領域を抽出したものに相当する。小領域は、初期の状態空間と同様に、速力（ｖｘ，ｖｙ）ごとの位置座標（ｘ，ｙ）を離散的に近似させたグリッドで表される。即ち、小領域設定部１３は、状態空間の断面ごと、即ち速力ごとに繰り返し動作し、範囲及び分解能が異なる小領域を速力ごとに複数設定する。図３は、実施の形態１に係る小領域設定部１３を示す機能ブロック図である。図３に示すように、小領域設定部１３は、初期尤度計算部２１、概略位置推定部２２、範囲設定部２３、及び分解能設定部２４を有する。
【0027】
初期尤度計算部２１は、最初の観測値から、初期の状態空間のうち、ある速力での位置座標を示した領域の範囲及び分解能に基づく尤度を計算する。図４は、状態空間の領域Ａ１における尤度を示す図である。初期の状態空間のうち、ある速力での位置座標を示した領域は、図４に示す領域Ａ１に相当し、ある速力（ｖｘ，ｖｙ）での位置座標（ｘ，ｙ）のグリッドが平面状に表現されている。初期尤度計算部２１では、図４に示すように、状態空間の領域Ａ１における尤度を計算する。図４では、明るい部分ほど、尤度が高いことを示している。
【0028】
尤度は、観測値と状態量との関係に基づいて計算される。例えば、方位θについては下記式（６）によって計算される。ここで、η（ａ，ｂ，ｃ）は平均ｂ、分散ｃのガウス分布の確率変数ａの確率密度関数を表している。また、下記式（６）のうち、下記式（７）は、方位誤差の分散値に相当する。ｘｏ及びｙｏは、センサ９１の位置である。
【0029】
【数6】
【0030】
【数7】
【0031】
同様に、距離ｒの尤度についても、下記式（８）によって計算される。下記式（８）のうち、下記式（９）は、距離誤差の分散値に相当する。
【0032】
【数8】
【0033】
【数9】
【0034】
図５は、尤度計算を説明するための図である。図５（ａ）は、方位θに対する尤度Ｌ１、即ち式（６）を示している。図５（ｂ）は、距離ｒに対する尤度Ｌ２、即ち式（８）を表している。方位θと距離ｒとが独立であるため、図５（ｃ）に示すように、尤度Ｌ１と距離に対する尤度Ｌ２との積をとり、下記式（１０）の最終的な尤度関数Ｌが得られる。
【0035】
【数10】
【0036】
図３に戻り、概略位置推定部２２は、下記式（１１）のとおり、初期の状態空間のうち、ある速力での位置座標を示した領域において、計算した尤度が最大となる位置座標（ｘ，ｙ）を目標の概略位置として推定する。図４では、位置Ａ２が目標の概略位置として推定される。
【0037】
【数11】
【0038】
図３に戻り、範囲設定部２３は、下記式（１２）のとおり、概略位置、解析時間、速力、及び運動モデルからそれぞれの小領域の範囲を設定する。ここで、解析時間をＴ秒とする。また、ｘＲ（ｖｘ，ｖｙ）＝［ａ，ｂ］、及びｙＲ（ｖｘ，ｖｙ）＝［ａ，ｂ］におけるａ及びｂは、小領域の範囲の端の座標を表している。δｘとδｙは、小領域の範囲に対して付与されたマージンとなる。これにより、式（１１）で示された概略位置（ｘｍａｘ（ｖｘ，ｖｙ），ｙｍａｘ（ｖｘ，ｖｙ））に対して、補正が行われている。運動モデルとしては、等速直線運動が仮定されている。以上を踏まえると、範囲設定部２３は、式（１１）で推定した概略位置からＴ秒間で目標が移動する範囲を決定する処理であると言える。図３では、範囲Ａ３が小領域の範囲に相当する。範囲設定部２３が設定した、速力ごとに異なる複数の小領域の範囲の座標は、記憶装置３に記憶される。
【0039】
【数12】
【0040】
図３に戻り、分解能設定部２４は、下記式（１３）のとおり、再帰処理部１５の処理周期ｄｔと、速力とから小領域の分解能を設定する。図６は、実施の形態１に係る小領域における尤度を示す図である。図６は、図４から抽出した小領域を示している。図６では、グリッド同士の間隔Ｒが小領域の分解能に相当する。分解能設定部２４が設定した、速力ごとに異なる複数の小領域の分解能は、記憶装置３に記憶される。
【0041】
【数13】
【0042】
図７は、実施の形態１に係る小領域について説明するための図である。図８は、実施の形態１に係る小領域について説明するための図である。図７は、速力（ｖｘ，ｖｙ）＝（５ｋｔ，５ｋｔ）の場合を示している。図８は、速力（ｖｘ，ｖｙ）＝（１０ｋｔ，５ｋｔ）の場合を示している。即ち、図７と図８とでは、速力が異なっている。図７及び図８を用いて、速力による小領域の範囲及び分解能の違いについて説明する。なお、解析時間は６００秒とし、処理周期は５秒とし、マージンδｘ及びδｙは簡易化のため０とする。
【0043】
図７の場合、即ち、速力（ｖｘ，ｖｙ）＝（５ｋｔ，５ｋｔ）の場合では、小領域の範囲は、ｘ方向及びｙ方向の何れも、１５４３．２ｍ（＝６００ｓ×５×０．５１４４ｍ／ｓ）である。また、グリッドの分解能についても、ｘ方向及びｙ方向の何れも１２．８６ｍ（＝５ｓ×５×０．５１４４ｍ／ｓ）である。
【0044】
図８の場合、即ち、速力（ｖｘ，ｖｙ）＝（１０ｋｔ，５ｋｔ）の場合では、小領域の範囲は、ｙ方向が１５４３．２ｍ（＝６００ｓ×５×０．５１４４ｍ／ｓ）であるのに対して、ｘ方向が３０８６．４ｍ（＝６００ｓ×１０×０．５１４４ｍ／ｓ）である。また、グリッドの分解能についても、ｙ方向が１２．８６ｍ（＝５ｓ×５×０．５１４４ｍ／ｓ）であるのに対して、ｘ方向が２５．７２ｍ（＝５ｓ×１０×０．５１４４ｍ／ｓ）である。このように、速力に応じて、範囲及び分解能が異なる小領域が形成される。
【0045】
図１に戻り、事前確率計算部１４は、それぞれの小領域に対して、記憶装置３に記憶された小領域ごとの範囲及び分解能を用いて、それぞれの小領域ごとの事前確率を設定する。事前確率は、再帰処理の最初に使用される初期値である。事前確率としては、一様な確率分布のほか、先験情報を用いた確率分布が与えられる。先験情報を用いた確率分布としては、例えば、速力は２ｋｔ～２０ｋｔまでしか取り得ないことが明らかであった場合、速力が２ｋｔ未満、及び２０ｋｔ超である確率を０としたものがある。事前確率計算部１４が設定した、速力ごとの事前確率は、記憶装置３に記憶される。
【0046】
次に、再帰処理部１５は、事前確率及び尤度に基づいたベイズ推定を再帰的に処理する。再帰処理部１５は、観測値の入力のたびに動作する。再帰処理部１５での演算は、記憶装置３に記憶された、それぞれの小領域の範囲と分解能とに基づいている。即ち、再帰処理部１５の計算量は、初期の状態空間に基づいて演算を行う場合よりも削減される。そして、削減された計算量は、演算範囲及び分解能を小領域ごと、即ち速力ごとに異なったものとなっている。再帰処理部１５は、尤度計算部３１、ベイズ推定部３２、及び状態遷移部３３を備える。
【0047】
尤度計算部３１は、それぞれの小領域の範囲と分解能とに基づいて、尤度を計算する。尤度計算部３１での演算では、式（６）、（８）、及び（１０）が用いられる。
【0048】
ベイズ推定部３２では、式（１）に示したベイズの定理に従い、事前確率、尤度、及び正規化項をあてはめて、下記式（１４）のとおり、事後確率を計算する。ここで、ｐｐｒｅ（Ａ）、ｐｐｏｓｔ（Ａ）は、それぞれ確率変数Ａに対する事前確率、事後確率を表している。また、ｘｋ、ｙｋ、ｖｘｋ、及びｖｙｋは、再帰処理における第ｋ時刻での確率変数であることを表している。時刻とは、再帰処理の処理された回数に相当する。
【0049】
【数14】
【0050】
状態遷移部３３は、再帰処理の次の時刻に遷移した場合の状態量を算出する。状態遷移にあたっては、１つ前の時刻の状態のみに依存する一次マルコフモデルを考える。具体的には、先ず、目標は等速直線運動をすること仮定し、ＮＣＶ（Ｎｅａｒｌｙ Ｃｏｎｓｔａｎａｔ Ｖｅｌｏｃｉｔｙ）モデルを適用した下記式（１５）に基づき、次の時刻への運動更新を行う。ここで、ｄｔは、前回時刻と今回時刻との時間差、即ち再帰処理の更新レートであり、σ１、及びσ２は、それぞれｖｘ、ｖｙに与える擾乱の値である。更新レートは、再帰処理の処理周期とも称することができる。
【0051】
【数15】
【0052】
式（１５）の運動更新に続いて、下記式（１６）のように今回の時刻ｋにおける事後確率を次の時刻ｋ＋１の事前確率に設定する。
【0053】
【数16】
【0054】
所定の最終時刻の再帰処理が行われた後、状態推定部１６では、事後確率から状態量が推定される。状態量の推定には、例えばＭＡＰ（Ｍａｘｉｍｕｍ Ａ Ｐｏｓｔｅｒｉｏｒｉ）推定が用いられる。ＭＡＰ推定は、下記式（１７）を用いる。ここで、Ｋは最終時刻を表している。
【0055】
【数17】
【0056】
出力部１７は、状態推定部１６によって推定された目標の状態量を表示装置９２に出力する。
【0057】
図９は、実施の形態１に係る目標状態推定方法を示すフローチャートである。図９を用いて、目標状態推定方法について説明する。先ず、状態空間設定部１２は、速力ごとの位置座標を離散的に近似させたグリッドで表される初期の状態空間を設定する（Ｓ１）。次に、小領域設定部１３の初期尤度計算部２１は、最初の観測値から、初期の状態空間のうち、ある速力での位置座標を示した領域における尤度を計算する（Ｓ２）。続いて、小領域設定部１３の初期尤度計算部２１は、計算された尤度が最大となる座標をある速力における目標の概略位置として推定する（Ｓ３）。また、小領域設定部１３の範囲設定部２３は、概略位置から小領域の範囲を設定する（Ｓ４）。更に、小領域設定部１３の分解能設定部２４は、再帰処理部１５の処理周期から小領域の分解能を設定する（Ｓ５）。
【0058】
ここで、小領域設定部１３は、全ての速力の小領域の設定が完了したか否かを判定する（Ｓ６）。全ての速力の小領域の設定が未完了である場合（Ｓ６：ＮＯ）、次の速力の小領域の設定を行う（Ｓ２～Ｓ５）。全ての速力の小領域の設定が完了した場合（Ｓ６：ＹＥＳ）、事前確率計算部１４は、全ての小領域に対して、小領域ごとの範囲及び分解能を用いて事前確率を計算する（Ｓ７）。続いて、再帰処理部１５は、尤度計算、ベイズ推定、及び状態遷移を再帰的に処理する（Ｓ８）。そして、再帰処理部１５は、最終時刻での処理が完了したか否かを判定する（Ｓ９）。最終時刻での処理が未完了の場合（Ｓ９：ＮＯ）、再帰処理部１５は、次の時刻の処理を行う（Ｓ８）。最終時刻での処理が完了した場合（Ｓ９：ＹＥＳ）、状態推定部１６は、最終時刻の計算結果から目標の状態量を推定する（Ｓ１０）。
【0059】
ここで、実施の形態１の目標状態推定方法と、比較例１の目標状態推定方法とを比較する。比較例１の目標状態推定方法は、小領域の設定を行わない点で実施の形態１と異なり、それ以外の点で実施の形態１と共通している。初期の状態空間は、位置については１０ｋｍ×１０ｋｍの範囲とし、速力については２ｋｔ～２０ｋｔの範囲とする。また、速力は、１ｋｔ刻みのグリッドで表される。また、更新レートは５秒、解析時間を１０分とする。また、計算を簡単にするため、最も遅い速力の（２ｋｔ，２ｋｔ）について考える。上記の条件で計算した場合、位置座標の分解能としては、（５．２ｍ（＝２ｋｔ（０．５１４４×２ｍ／ｓ）×５ｓの計算結果を切り上げ），５．２ｍ）が必要とされる。
【0060】
比較例１の目標状態推定方法では、状態空間は、１９２４（＝１０ｋｍ／５．２ｍの計算結果を切り上げ）×１９２４のグリッドサイズとなる。
【0061】
本実施の形態１では、小領域の範囲は、６１８ｍ（＝０．５１４４×２ｍ／ｓ×６００ｓの計算結果を切り上げ）×６１８ｍとなる。このため、マージンも６１８ｍ取った場合、２３８（＝６１８×２／５．２の計算結果を切り上げ）×２３８のグリッドサイズとなる。このため、運動更新の対象となるグリッドの数を約１／６６（＝１／（１９２４２／２３８２））に減らすことができる。これにより、本実施の形態１では、比較例１よりも計算量を削減することができる。
【0062】
また、実施の形態１の目標状態推定方法と、比較例２の目標状態推定方法とを比較する。比較例２では、再帰処理のたびに、補間処理を含むグリッド再設計を行っている。図１０は、比較例２に係るグリッド再設計を説明するための図である。
【0063】
先ず、比較例２の目標状態推定方法では、周辺分布計算が行われる（Ｓ９１）。周辺分布計算は、ベイズ推定後の事後確率ｐｐｏｓｔ（ｘｋ，ｙｋ，ｖｘｋ，ｖｙｋ）に対して、下記式（１８）のとおり、ｘｋ及びｙｋに関する周辺分布を計算するものである。
【0064】
【数18】
【0065】
次に、中心の捜索が行われる（Ｓ９２）。中心の捜索では、周辺分布の中心として、周辺分布の最大値を求める。周辺分布の最大値は、下記式（１９）のとおり、算出される。なお、周辺分布の中心は、周辺分布の中央値、又は平均値などから推定してもよい。
【0066】
【数19】
【0067】
また、外周加算（Ｓ９３）～最小辺削除（Ｓ９７）が繰り返し行われる。外周加算から最小辺削除までの処理によって、状態空間のグリッドのうち、事後確率の程度が低いグリッドが削除される。この処理は、削除されたグリッドの事後確率の値の合計が所定の閾値に達するまで繰り返される。
【0068】
先ず、外周加算が行われる（Ｓ９３）。図１１は、比較例２に係る外周加算から最小辺削除までの流れを説明するための図である。外周加算では、下記式（２０）及び（２１）を用いて、グリッドで表現された周辺分布の最外周及びその内側の周、即ち８つの辺において、それぞれの辺を構成する事後確率の値を加算する。ここで、ｘｋ（ｍ）及びｙｋ（ｎ）は、それぞれ、時刻ｋにおける周辺分布のｍ行目、及びｎ列目を意味している。また、Ｍは、グリッドの行数であり、Ｎは、グリッドの列数である。外周加算は、図１１を用いて次のようにも説明することができる。即ち、式（２０）は、周辺分布の４行のそれぞれ、即ち１行目、２行目、Ｍ－１行目、及びＭ行目のグリッドの加算値を算出する。また、式（２１）は、周辺分布の４列のそれぞれ、即ち１列目、２列目、Ｎ－１列目、及びＮ列目のグリッドの加算値を算出する。以上のように、式（２０）及び（２１）によって、周辺分布の最外周及びその内側の周の８辺について、８つの加算値が算出される。
【0069】
【数20】
【0070】
【数21】
【0071】
図１０に戻り、８つの加算値から最小となる辺が選択される（Ｓ９４）。最小となる辺をｌ、最小値をｍｉｎｐｏｓｔｍｅｒとする。図１１の右図では、太枠で囲まれた１行目、即ちｏｕｔｓｕｍｒｏｗ（１）が最小であった場合を例にしている。
【0072】
図１０に戻り、この最小値ｍｉｎｐｏｓｔｍｅｒと、前回の累積削除確率ｒｅｃとを加算して累積削除確率が更新される（Ｓ９５）。累積削除確率の初期値は、０である。
【0073】
続いて、予め設定した閾値と、累積削除確率ｒｅｃとが比較される（Ｓ９６）。累積削除確率が閾値未満の場合（Ｓ９６：閾値未満）、状態空間のグリッドのうち、最小辺ｌを構成する事後確率のグリッドが削除され（Ｓ９７）、再び、外周加算（Ｓ９３）が行われる。
【0074】
累積削除確率が閾値以上である場合（Ｓ９６：閾値以上）、グリッド調整が行われる（Ｓ９８）。グリッド調整では、グリッドの中心を捜索したＥｓｔｍａｒとし、グリッドを削除した後のグリッドサイズがオリジナルのグリッドサイズと等しくになるように調整する。例えば、グリッドを削除した後のグリッド範囲が２ｋｍ×２ｋｍであって、グリッドサイズ１０×１０であり、オリジナルのグリッドサイズが５０×５０であった場合を例にする。この場合は、グリッドの範囲が２ｋｍ×２ｋｍのまま、グリッドサイズが５０×５０となるように調整される。周辺分布の中心、グリッド範囲、及びグリッド数が決まっているため、分解能、及びグリッド位置が一意に決定される。これにより、グリッド再設計後のグリッドがオリジナルよりも密な分解能を有する。なお、グリッド再設計後における事後確率への推移は、線形補間、又は最近傍グリッドの選択などによって実施される。
【0075】
以上のように、比較例２では、再帰処理ごとに、状態空間のグリッド範囲を縮小させて計算量を削減していた。しかしながら、非特許文献２の目標状態推定方法では、ベイズ推定の再帰処理のたびにグリッドの再設計が行われる。グリッド再設計には、補間処理が含まれているため、再帰処理を繰り返すことで、目標状態推定の精度が低下してしまう。
【0076】
ここで、精度の低下を抑制する方法としては、次の２つの方法が考えられる。１つ目は、再設計後のグリッド数を増加させ、補間処理の精度を高くする方法である。しかしながら、この場合、グリッド数が増加したことで、補間処理での計算量が増加してしまう。また、２つ目は、累積削除確率ｒｅｃと比較する閾値を小さくし、１回のグリッド再設計で削除するグリッドの数を抑える方法である。しかしながら、この場合では、１回のグリッド再設計で削除するグリッドの数が多い場合と同じ精度を得るためには再帰処理の繰り返し回数が多くなってしまい、計算量が増加してしまう。
【0077】
本実施の形態１によれば、複数の速力のそれぞれにおける小領域の範囲及び分解能が設定される。このため、速力ごとに必要なグリッドの範囲、及び分解能が設定されているため、計算量が削減される。また、ベイズ推定を再帰的に処理する度に、グリッド調整を実施する必要がなく、目標状態推定の精度の低下が抑制される。以上のように、本実施の形態１における目標状態推定方法によれば、精度の低下を抑制しつつ、計算量を削減させることができる。
【0078】
以上が本発明の実施の形態の説明であるが、本発明は、上記の実施の形態の構成に限定されるものではなく、その技術的思想の範囲内で様々な変形または組み合わせが可能である。実施例では、式（６）、（８）、及び（１０）を用いて方位及び距離に対する尤度を計算している。想定される対応関係が定式化できればよく、例えば、下記式（２２）を尤度の計算に用いてもよい。
【0079】
【数22】
【0080】
また、式（６）、（８）、及び（１０）では、誤差分布をガウス分布と想定した定式化をしているが、想定する誤差分布はガウス分布でなくてもよい。
【0081】
また、概略位置推定では尤度の最大値を用いて推定しているが、平均値など他の統計的な処理により求めてもよい。
【0082】
また、各機能部が動作する順番は、実施の形態１に開示した順番に限定されない。例えば、小領域の設定を行うごとに事前確率計算部１４が動作するように変更してもよい。
【0083】
また、小領域の範囲及び分解能が速力ごとに設定されれば、初期尤度計算部２１、及び概略位置推定部２２の処理は、速力ごとに共通していてもよい。この場合、推定される概略位置がどの速力でも同じ位置になる。
【符号の説明】
【0084】
１ 目標状態推定装置、２ 処理装置、３ 記憶装置、１１ 入力部、１２ 状態空間設定部、１３ 小領域設定部、１４ 事前確率計算部、１５ 再帰処理部、１６ 状態推定部、１７ 出力部、２１ 初期尤度計算部、２２ 概略位置推定部、２３ 範囲設定部、２４ 分解能設定部、３１ 尤度計算部、３２ ベイズ推定部、３３ 状態遷移部、９１ センサ、９２ 表示装置、９０１ 目標状態推定装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】