JP5854926B2

JP5854926B2 - 落下運動判定装置

Info

Publication number: JP5854926B2
Application number: JP2012114404A
Authority: JP
Inventors: 小幡　康; 康小幡; 洋志亀田
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2012-05-18
Filing date: 2012-05-18
Publication date: 2016-02-09
Anticipated expiration: 2032-05-18
Also published as: JP2013242183A

Description

この発明は、目標物が落下運動していることを検知する落下運動判定装置に関するものである。

レーダ等のセンサシステムでは、目標物の位置を観測するセンサから得られる時系列に沿った観測値から目標物の有無を判定し、目標物が存在する場合に目標物の位置、速度を推定する追尾処理により目標物の運動諸元の推定を必要とする場合がある。また、その推定された運動諸元より目標物の運動状態の変化を判定することが必要となる場合がある。

レーダ等のセンサにより目標物を観測して得られる観測値を用いて目標物を追尾する技術については多くの論文や特許文献等で取り上げられており、それらを実現する装置および方法については様々な提案がなされている。

ここで、例えば追尾していた航空機に故障が発生した場合に、航空機が落下する、あるいは、等速直進運動していた航空機がある時点で落下運動に転じる場合がある。このような落下運動及び直進運動の状態から落下運動状態への変化を航空機の運動諸元の推定値から素早く、かつ正確に検知することにより、落下している航空機に対する素早い対処が可能となる。

落下運動の判定には正確さが要求され、目標物は等速直進運動以外にも進行方向加速度運動、横旋回運動、下方旋回運動といった様々な運動を行う可能性がある。これら運動を落下運動であると誤判定することなく、落下運動のみを正しく検出する必要がある。
例えば非特許文献１には、追尾処理によって算出された残差から、目標物の運動が直進から旋回等の他の運動に転じたことを判定する方式が開示されている。

Ｙ．Ｂａｒ−Ｓｈａｌｏｍ，Ｘ．ＲｏｎｇＬｉａｎｄＴ．Ｋｉｒｕｂａｒａｊａｎ "ＥｓｔｉｍａｔｉｏｎｗｉｔｈＡｐｐｌｉｃａｔｉｏｎｓｔｏＴｒａｃｋｉｎｇａｎｄＮａｖｉｇａｔｉｏｎ", ＪｏｈｎＷｉｌｅｙ＆Ｓｏｎｓ，２００１．ｐｐ４２７−４３１

非特許文献１に記載された方式を落下判定に適用した落下判定システムは、センサ、追尾フィルタ部、加速度推定部から構成される。追尾フィルタ部は、等速直進運動状態の運動モデルに基づく追尾フィルタを用いて、センサの目標物の位置の観測値から目標物の運動諸元を推定し、サンプリング時刻ごとに運動諸元推定処理を行い、残差を算出する。
ここで残差とは、目標物が等速直進運動をしていると仮定した場合における予測位置と観測位置の差分のベクトルであり、この残差ベクトルを加速度推定部に入力する。

加速度推定部は、スライディングウィンドウ内の残差の集積により、加速度推定値を最小二乗法により算出する。このスライディングウィンドウは、１サンプリング時刻を経過する度に時刻を前方に１つずつずらして得られる、最新サンプリング時刻を含む連続するｎサンプリング時刻分のデータである。加速度推定部は、この平均加速度の下方向成分が推定誤差の範囲内で重力加速度（１Ｇ，９．８ｍ／ｓ²）相当である場合に、目標物が落下していると判定する。

ここで、センサの観測値は目標物の位置を正確に反映している訳でなく、ある程度の誤差を伴う。この観測誤差が大きい場合、発生する残差が落下運動に起因するのか、観測誤差に起因するのか区別するのが難しくなる。このような場合には、スライディングウィンドウの範囲を長くして、観測誤差によるランダム成分を除去することにより、落下運動を判定する必要がある。

しかしながら、例えば非特許文献１に記載された方式では、加速度推定部により算出される加速度推定値はスライディングウィンドウ内の加速度の平均値である。このため、例えば、図１３（ａ），（ｂ）に示された時刻ｔ１スライディングウィンドウ４１のように、スライディングウィンドウの途中で観測値４３に基づく目標軌道４４が落下し始めても、重力加速度１Ｇに相当する加速度推定値４５は算出されない。この方式により重力加速度に相当する加速度推定値４５が算出されるためには、時刻ｔ２スライディングウィンドウ４２のように、スライディングウィンドウ内の全てのサンプリング時刻で目標物が落下運動をしている必要がある。
しかし、スライディングウィンドウを長く取った場合、スライディングウィンドウ内の目標物の運動が全て落下運動状態になるまでには時間がかかる。すなわち、目標物が実際に落下運動を始めてからその落下運動を検知するまでに大きな遅れが生じるという課題があった。

また、従来の落下運動を検出する方式は、上記のように、スライディングウィンドウ内の加速度の平均値のみによって落下運動を判定するため、落下運動以外の運動、例えば下方に旋回する運動を誤って落下運動であると誤判定し易くなるという課題があった。

この発明は、上記のような課題を解決するためになされたものであり、目標物が落下運動を始めてから落下運動状態を検知するまでの検知ずれ時間を最小にし、また、落下運動以外の運動を落下運動であると誤判定するのを防止することができる落下運動判定装置を提供することを目的とする。

上記目的を達成するため、この発明に係る落下運動判定装置は、センサから得られる目標物の位置の観測値を用いて目標物を追尾し、かつこの追尾から得られた情報及び観測値を用いて目標物の落下運動及び等速直進運動状態から落下運動状態への変化を判定する落下運動判定装置において、センサから得られる観測値に基づいて目標物の運動諸元を推定する追尾フィルタ部と、追尾フィルタ部からの情報が加えられた観測値が反映されるスライディングウィンドウ内における目標物の平均の加速度を算出する加速度推定部と、スライディングウィンドウを目標物の直進期間と落下期間に２分割するスライディングウィンドウ分割部とを備え、スライディングウィンドウ分割部は、分割する前のスライディングウィンドウの長さを３乗した値と、加速度の下方向成分と、重力加速度とに基づき、分割の区切りから落下期間側のスライディングウィンドウの長さを３乗した値を算出し、当該値から落下期間側のスライドウィンドウの長さを求めるものである。

また、この発明に係る落下運動判定装置は、センサから得られる目標物の位置の観測値を用いて目標物を追尾し、かつこの追尾から得られた情報及びセンサの観測値を用いて目標物の落下運動及び等速直進運動状態から落下運動状態への変化を判定する落下運動判定装置において、センサから得られる観測値に基づいて目標物の運動諸元を推定する追尾フィルタ部と、追尾フィルタ部からの情報が加えられた観測値が反映されるスライディングウィンドウ内における目標物の平均の加速度を算出する加速度推定部と、スライディングウィンドウを目標物の直進期間と落下期間に２分割するスライディングウィンドウ分割部と、観測値を用いて落下運動の尤度を算出し、最終的に目標物が落下しているか否かを判断する落下運動尤度判定部とを備え、スライディングウィンドウ分割部は、分割する前のスライディングウィンドウの長さを３乗した値と、加速度の下方向成分と、重力加速度とに基づき、分割の区切りから落下期間側のスライディングウィンドウの長さを３乗した値を算出し、当該値から落下期間側のスライドウィンドウの長さを求めるものである。

また、この発明に係る落下運動判定装置は、センサから得られる目標物の位置の観測値を用いて目標物を追尾し、かつこの追尾から得られた情報及びセンサの観測値を用いて目標物の落下運動及び等速直進運動状態から落下運動状態への変化を判定する落下運動判定装置において、センサから得られる観測値に基づいて目標物の運動諸元を推定する追尾フィルタ部と、追尾フィルタ部からの情報が加えられた観測値が反映されるスライディングウィンドウ内における目標物の平均の加速度を算出する加速度推定部と、スライディングウィンドウを目標物の直進期間と落下期間に２分割するスライディングウィンドウ分割部と、観測値に基づく有限個の落下運動候補のそれぞれに対する落下運動の尤度を算出すると共に、尤度の総和を用いて最終的に目標物が落下しているか否かを判断する落下運動適合判定部とを備え、スライディングウィンドウ分割部は、分割する前のスライディングウィンドウの長さを３乗した値と、加速度の下方向成分と、重力加速度とに基づき、分割の区切りから落下期間側のスライディングウィンドウの長さを３乗した値を算出し、当該値から落下期間側のスライドウィンドウの長さを求めるものである。

この発明によれば、目標物が落下運動を始めてから落下運動を検知するまでの検知ずれ時間を最小にすることができ、また、落下運動以外の運動状態を落下運動であると誤判定することを防ぐことができる。

この発明の実施の形態１に係る落下運動判定装置のブロック構成図である。この発明の実施の形態１に係る落下運動判定装置の１サンプリング時刻分の処理の手順を示すフローチャートである。この発明の実施の形態１に係る落下運動判定装置によるスライディングウィンドウの分割例を示す図である。この発明の実施の形態１に係る落下運動判定装置のスライディングウィンドウの分割について説明する図である。この発明の実施の形態２に係る落下運動判定装置の構成を示すブロック図である。この発明の実施の形態２に係る落下運動判定装置の１サンプリング時刻分の処理の手順を示すフローチャートである。この発明の実施の形態２に係る落下運動判定装置の落下運動の尤度計算の例を示す図である。この発明の実施の形態２に係る落下運動判定装置の１サンプリング時刻分の他の処理の手順を示すフローチャートである。この発明の実施の形態３に係る落下運動判定装置のブロック構成図である。この発明の実施の形態３に係る落下運動判定装置の１サンプリング時刻分の処理の手順を示すフローチャートである。この発明の実施の形態３に係る落下運動判定装置の目標物の運動モデルの例を示す図である。この発明の実施の形態３に係る落下運動判定装置の各運動の尤度計算の例を示す図である。従来の落下運動判定装置のスライディングウィンドウと推定加速度について示した図である。

実施の形態１．
実施の形態１に係る落下運動判定装置を図面を用いて説明する。
図１は、この発明の実施の形態１に係る落下運動判定装置１０のブロック構成図である。落下運動判定装置１０は、図１に示すように、センサ４に接続されている。また、センサ４から得られる観測値に基づいて目標物の運動諸元を推定する追尾フィルタ部３が設けられている。また、追尾フィルタ部３からの情報が加えられた観測値が反映されるスライディングウィンドウを有して、目標物の加速度を算出する加速度推定部１が設けられている。また、スライディングウィンドウを２分割するスライディングウィンドウ分割部２が設けられている。

追尾フィルタ部３は、等速直進運動の運動モデルに基づく追尾フィルタを有し、センサ４から入力される目標物の位置の観測値から、追尾フィルタを用いてサンプリング時刻ごとに目標物の運動諸元を推定すると共に残差を算出するようになっている。
ここで、残差とは目標物が等速直進運動をしていると仮定した場合の予測位置と観測位置の差分ベクトルである。

加速度推定部１は、ｎサンプリング時刻分の観測値および残差が反映されるスライディングウィンドウを有し、スライディングウィンドウ内の残差の集積により、目標物のスライディングウィンドウ内の加速度推定値を最小二乗法により算出するようになっている。
スライディングウィンドウ分割部２は、加速度推定部１により算出された加速度推定値とスライディングウィンドウの長さに基づいて、スライディングウィンドウを直進期間スライディングウィンドウと落下期間スライディングウィンドウとに分割して、加速度推定部１へ出力するようになっている。

次に、落下運動判定装置１０による落下運動判定の処理手順について説明する。図２は、この発明の実施の形態１に係る落下運動判定装置１０の１サンプリング時刻分の処理の手順を示すフローチャートである。
図２に示すように、まず、追尾フィルタ部３が、センサ４から入力された目標物の位置情報から目標物の運動諸元を推定し、残差を算出する（ステップＳＴ１１）。

次に、加速度推定ステップＳＴ１２において、加速度推定部１が、追尾フィルタ部３から観測値および残差を受け取り、スライディングウィンドウに追加して、スライディングウィンドウ内の加速度推定値を算出する（ステップＳＴ１２）。

加速度推定ステップＳＴ１２の算出結果が、ステップＳＴ１３に示すように、有意な加速度でない場合、加速度推定部１は、「目標は落下していない」と判定して処理を終了する。一方、加速度推定ステップＳＴ１２の算出結果が、ステップＳＴ１３に示すように、有意な加速度推定値である場合には、加速度推定部１がこの加速度推定値が推定誤差の範囲内で重力加速度に相当するか判定する（ステップＳＴ１４）。
そして、ステップＳＴ１４に示すように、加速度推定値が重力加速度に相当する場合、加速度推定部１が「目標は落下している」と判定して処理を終了する。一方、ステップＳＴ１４に示すように、推定加速度が重力加速度に相当しない場合には、次のスライディングウィンドウ分割ステップＳＴ１５に移行する。

スライディングウィンドウ分割ステップＳＴ１５では、スライディングウィンドウ分割部２が、ステップＳＴ１２で算出された加速度推定値とスライディングウィンドウの長さに基づいて、スライディングウィンドウを直進期間スライディングウィンドウと落下期間スライディングウィンドウに２分割する（ステップＳＴ１５）。
図３は、この発明の実施の形態１に係る落下運動判定装置１０によるスライディングウィンドウの分割例を示す図であり、観測値２３と目標軌道２４上に示されたスライディングウィンドウ２０の前半部分を直進期間スライディングウィンドウ２１とし、後半部分を落下期間スライディングウィンドウ２２として分割している。このスライディングウィンドウ２０の分割の区切りは、以下に示す式（１）が成立するように決定する。

ここで、式（１）の左辺のｓは加速度推定ステップＳＴ１２で適用した元のスライディングウィンドウの長さであり、

は加速度推定ステップＳＴ１２で算出された加速度推定値の下方向の成分である。また右辺のｇは重力加速度であり、ｓ’は算出したい分割の区切りである。

図４は、この発明の実施の形態１に係る落下運動判定装置１０のスライディングウィンドウの分割について説明する図である。図４には、加速度推定値による残差を示す特性線Ａと実際の落下運動による残差を示す特性線Ｂが示されている。
加速運動中の目標物と等速直進運動中の目標物の位置の差異は各時刻で加速時間の２乗に比例する。また、スライディングウィンドウ内の残差の集積は、加速運動中の目標物と等速直進運動中の目標物の位置の差異を積分した値で３乗に比例すると見做す。
これにより、加速度推定ステップＳＴ１２で算出された加速度推定値がスライディングウィンドウの全サンプリング時刻で発生した場合のこの残差（特性線Ａ）の積分と、スライディングウィンドウ内のある途中のサンプリング時刻から発生した重力加速度（特性線Ｂ）の積分が一致することになる。これは式（１）が成立することを意味している。

算出されたｓ’より後のサンプリング時刻群で落下期間スライディングウィンドウを構成し、ｓ’より前のサンプリング時刻群で直進期間スライディングウィンドウを構成する。

例えば、分割の区切りがスライディングウィンドウの外として算出された場合等、ステップＳＴ１６に示すように、式（１）で分割の区切りが見つからない場合、スライディングウィンドウ分割部２が、「目標物は落下していない」と判定して処理を終了する。一方、ステップＳＴ１６に示すように、分割の区切りが見つかった場合には、次の加速度再推定ステップＳＴ１７に移行する。

加速度再推定ステップＳＴ１７では、加速度推定部１がスライディングウィンドウ分割ステップＳＴ１５で分割された直進期間スライディングウィンドウと落下期間スライディングウィンドウのそれぞれで加速度推定を再実行する（ステップＳＴ１７）。

なお、上記のスライディングウィンドウの分割の区切りについては、加速度の推定誤差を考慮して、式（１）で決定された値から前後数サンプル分を候補とし、それぞれの分割の区切りにおける落下期間で重力加速度を算出し、重力加速度に最も近い加速度が得られた区切りを選択するとしてもよい。

このステップにおいて、直進期間スライディングウィンドウで有意な加速度が算出されず、かつステップＳＴ１８に示すように、落下期間スライディングウィンドウで重力加速度相当の加速度推定値が算出された場合、加速度推定部１は「目標物は落下している」と判定して、処理を終了する。

一方、ステップＳＴ１８に示すように、落下期間スライディングウィンドウで重力加速度相当の加速度推定値が算出されない場合には、例えば直進期間スライディングウィンドウで有意な加速度が算出されるか、あるいは落下期間で落下に相当しない加速度推定値が算出された場合には、加速度推定部１は「目標物は落下していない」と判定して、処理を終了する。

以上のように、この実施の形態１では、センサ４から得られる観測値に基づいて目標物の運動諸元を推定する追尾フィルタ部３と、追尾フィルタ部３からの情報が加えられた観測値が反映されるスライディングウィンドウ内における目標物の平均の加速度を算出する加速度推定部１と、加速度とスライディングウィンドウの長さに基づき、スライディングウィンドウを目標物の直進期間と落下期間に２分割するスライディングウィンドウ分割部２とを具備し、スライディングウィンドウを分割して目標の直進期間と落下期間に分割して加速度推定を行うので、落下運動以外の運動を落下運動であると誤判定しないことは勿論のこと、スライディングウィンドウが長い場合でも、落下運動状態を検知するまでの検知ずれ時間を最小にして早期に落下判定ができる。また、スライディングウィンドウの分割の区切りを元のスライディングウィンドウで算出された加速度より決定するので、区切りを１つずつずらしながら最適な分割を探索する場合と比べて処理負荷が軽減される。

実施の形態２．
実施の形態２に係る落下運動判定装置を図面を用いて説明する。
図５は、この実施の形態２に係る落下運動判定装置１１の構成を示すブロック図である。この落下運動判定装置１１は、センサ４の観測値を用いて落下運動の尤度を算出し、最終的に目標が落下しているか否かを判断する落下運動尤度判定部５を有する点で、実施の形態１の図１に示す落下運動判定装置１０と異なる構成となっている。なお、実施の形態１と同様の構成については、同一の符号を付して説明を省略する。

落下運動尤度判定部５は、加速度推定部１によって加速度推定値が重力加速度に相当すると判定されたスライディングウィンドウまたは落下期間スライディングウィンドウ（以下、「落下運動のスライディングウィンドウ」と呼ぶ。）における観測値により落下運動尤度判定を行うようになっている。

次に、落下運動判定装置１１による落下運動判定の処理手順について説明する。図６は、この発明の実施の形態２に係る落下運動判定装置１１の１サンプリング時刻分の処理の手順を示すフローチャートである。図６に示されたステップＳＴ２１からステップＳＴ２８までの処理は、実施の形態１の図２のステップＳＴ１１からステップＳＴ１８までの処理と同様であるため説明を省略する。

図６に示すように、ステップＳＴ２４において加速度推定値が重力加速度に相当すると判定された場合、または、ステップＳＴ２８において加速度推定値が重力加速度に相当すると判定された場合に、落下運動尤度判定部５が、落下運動のスライディングウィンドウにおける観測値により落下運動尤度判定を行う（ステップＳＴ２９）。これにより判定の精度を高めることができる。

落下運動の尤度に関する以下に示す不等式（２）が成り立つ場合、すなわち、ステップＳＴ３０において落下運動尤度が大きい場合は、落下運動尤度判定部５が「目標物は落下している」と判定して処理を終了する。一方、落下運動の尤度に関する不等式（２）が成り立たない場合、すなわち、ステップＳＴ３０において落下運動尤度が大きくない場合には、落下運動尤度判定部５が「目標物は落下していない」と判定して処理を終了する。

ここで、式（２）のＺは、以下の式（３）に示すように、落下運動のスライディングウィンドウ内の全観測値である。

また、“ｆａｌｌ”は目標物が落下しているという事象を示す。また、
ｔｈｒｅｓｈｏｌｄは判定における閾値であり、事前に設定されるパラメータである。
不等式（２）の左辺の算出方法を以下の式（４）に示す。

ここで、Ｒは観測誤差共分散行列である。また、落下以外の運動のふらつきの要素を観測誤差に加算した誤差共分散行列であってもよい。また、

は以下の式（５）に示すように、落下開始時刻から重力加速度がかかることを仮定して算出した、目標軌道の各サンプリング時刻における目標物の位置である。より具体的には、図７に示すように、落下期間スライディングウィンドウの開始時刻の追尾平滑値２５を初期値とし、下方向に重力加速度がかかり続ける運動として目標軌道Ｃに示すような軌道の計算により特定される目標物の位置である。

なお、実施の形態２の処理の手順では、図６に示すように、加速度推定ステップ（ステップＳＴ２２）または加速度再推定ステップ（ステップＳＴ２７）において重力加速度に相当する加速度推定値が算出された場合に、その判定精度を高めるために落下運動尤度判定を行っている。
しかし、図８に示すように、加速度推定ステップ（ステップＳＴ３２）において重力加速度に相当する加速度推定値が算出された場合（ステップＳＴ３４）、または、加速度再推定ステップ（ステップＳＴ３７）において重力加速度に相当する加速度推定値が算出された場合（ステップＳＴ３８）は、「目標物は落下している」と判定して処理を終了し、加速度推定ステップＳＴ３２または加速度再推定ステップＳＴ３７において落下が検出されない場合に、落下運動尤度判定（ステップＳＴ３９とステップＳＴ４０）を行うとしてもよい。

以上のように、実施の形態２によれば、センサ４から得られる観測値に基づいて目標物の運動諸元を推定する追尾フィルタ部３と、追尾フィルタ部３からの情報が加えられた観測値が反映されるスライディングウィンドウ内における目標物の平均の加速度を算出する加速度推定部１と、加速度とスライディングウィンドウの長さに基づき、スライディングウィンドウを目標物の直進期間と落下期間に２分割するスライディングウィンドウ分割部２と、観測値を用いて落下運動の尤度を算出し、最終的に目標物が落下しているか否かを判断する落下運動尤度判定部５とを具備するように構成したので、実施の形態１と同様の効果が得られると共に、加速度推定の後で観測値から落下運動への尤度を算出しながら最終的に落下を判定するので、誤判定の確率が低減される。

実施の形態３．
実施の形態３の落下運動判定装置を図面を用いて説明する。
図９は、この発明の実施の形態３に係る落下運動判定装置１２のブロック構成図である。この落下運動判定装置１２は、観測値を用いて落下運動を含む有限個の運動の候補に対する尤度を算出し，候補となる運動の尤度の総和に対する落下運動の尤度から最終的に目標が落下しているか否かを判断する落下運動適合判定部６を有している点で、実施の形態１に示す落下運動判定装置１０と異なる構成となっている。なお、実施の形態１と同様の構成については、同一の符号を付して説明を省略する。
落下運動適合判定部６は、加速度推定部１内の落下運動のスライディングウィンドウにおける観測値により落下運動適合判定を行うようになっている。

次に落下運動判定装置１２による落下運動判定の処理手順について説明する。図１０は、この発明の実施の形態３に係る落下運動判定装置１２の１サンプリング時刻分の処理の手順を示すフローチャートである。図１０に示されたステップＳＴ４１からステップＳＴ４８までの処理は、実施の形態１の図２のステップＳＴ１１からステップＳＴ１８までの処理と同様であるため説明を省略する。

図１０に示すように、ステップＳＴ４４において、加速度推定値が重力加速度に相当すると判定された場合、または、ステップＳＴ４８において、加速度推定値が重力加速度に相当すると判定された場合、落下運動適合判定部６が、落下運動のスライディングウィンドウにおけるセンサ測定値により落下運動適合判定を行う（ステップＳＴ４９）。これにより判定の精度を高め、旋回等の落下と類似した運動の誤判定を防止する。

事後確率に関する以下に示す不等式（６）が成り立つ場合、すなわち、ステップＳＴ４９の落下運動適合判定の結果、ステップＳＴ５０で落下運動に適合すると判定された場合は、落下運動適合判定部６が「目標物は落下している」と判定して処理を終了する。一方、落下運動の尤度に関する不等式（６）が成り立たない場合、すなわち、ステップＳＴ５０で落下運動に適合しないと判定された場合には、落下運動適合判定部６が「目標物は落下していない」と判定して処理を終了する。

ここでＺは、以下の式（７）に示すように落下運動のスライディングウィンドウ内の全観測値である。

また、“ｆａｌｌ”は目標物が落下しているという事象を示す。また
ｔｈｒｅｓｈｏｌｄは判定における閾値であり、事前に設定するパラメータである。

不等式（６）の左辺の算出方法を以下の式（８）に示す。ベイズの定理より以下のように展開できる。

ここで“ｍｏｔｉｏｎ”は目標物がある特定の運動をしているという事象を示す。この運動は無数に有り得るが、この実施の形態３では、図１１に示された落下運動２６と、例えば第１の下方Ｓ字旋回２７や第２の下方Ｓ字旋回２８等の有限個の下方旋回運動に限定する。すなわち、それ以外の運動については本ステップ以前の処理により除外されたと見做すことにする。

式（８）の右辺の分子及び分母の第１項は、以下の式（９）に示すように、観測値に基く各運動の尤度である。

ここで、Ｒは観測誤差共分散行列である。また、モデルを有限個に限定したモデル化誤差を観測誤差に加算した誤差共分散行列でもよい。また、

は、以下の式（１０）に示すように、想定した運動モデルにより算出した目標軌道の各サンプリング時刻における目標物の位置である。

落下運動のスライディングウィンドウの開始時刻の追尾平滑値より、各運動の軌道を以上の式を用いて計算し、図１２に示された目標軌道を得る。ここで、図１２（ａ）には、落下運動について、下方向に重力加速度がかかり続ける運動として算出した目標軌道Ｄが示されている。また、図１２（ｂ）には、下旋回運動について、一定期間２９を旋回し、その後の一定期間３０を逆旋回し、その後の期間３１を直進する運動として算出した目標起動Ｅが示されている。

式（１０）の右辺の事前確率Ｐ（ｆａｌｌ），Ｐ（ｍｏｔｉｏｎ）は全ての運動モデルについて等確率とする。また、目標物の位置以外の観測情報がセンサ４から得られるならば、それによって事前確率を重み付けしてもよい。

以上のように、この実施の形態３では、センサ４から得られる観測値に基づいて目標物の運動諸元を推定する追尾フィルタ部３と、追尾フィルタ部３からの情報が加えられた観測値が反映されるスライディングウィンドウ内における目標物の平均の加速度を算出する加速度推定部１と、加速度とスライディングウィンドウの長さに基づき、スライディングウィンドウを目標物の直進期間と落下期間に２分割するスライディングウィンドウ分割部２と、観測値に基づく有限個の落下運動候補のそれぞれに対する尤度を算出すると共に、尤度の総和を用いて最終的に目標物が落下しているか否かを判断する落下運動適合判定部６とを具備するように構成したので、実施の形態１と同様の効果が得られると共に、加速度推定の後で観測値から特定の運動への適合度を算出しながら最終的に落下を判定するのでより正確に判定できる。

なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。

１加速度推定部、２スライディングウィンドウ分割部、３追尾フィルタ部、４センサ、５落下運動尤度判定部、６落下運動適合判定部、１０，１１，１２落下運動判定装置、２０スライディングウィンドウ、２１直進期間スライディングウィンドウ、２２落下期間スライディングウィンドウ、２３観測値、２４目標軌道、２５落下期間スライディングウィンドウの開始時刻の追尾平滑値、２６落下運動、２７第１の下方Ｓ字旋回、２８第２の下方Ｓ字旋回、２９，３０一定期間、３１期間、４１時刻ｔ１のスライディングウィンドウ、４２時刻ｔ２のスライディングウィンドウ、４３観測値、４４目標軌道、４５加速度推定値。

Claims

センサから得られる目標物の位置の観測値を用いて該目標物を追尾し、かつこの追尾から得られた情報及び前記観測値を用いて該目標物の落下運動及び等速直進運動状態から落下運動状態への変化を判定する落下運動判定装置において、
センサから得られる観測値に基づいて該目標物の運動諸元を推定する追尾フィルタ部と、
該追尾フィルタ部からの情報が加えられた前記観測値が反映されるスライディングウィンドウ内における前記目標物の平均の加速度を算出する加速度推定部と、
前記スライディングウィンドウを前記目標物の直進期間と落下期間に２分割するスライディングウィンドウ分割部とを備え、
前記スライディングウィンドウ分割部は、分割する前の前記スライディングウィンドウの長さを３乗した値と、前記加速度の下方向成分と、重力加速度とに基づき、分割の区切りから前記落下期間側のスライディングウィンドウの長さを３乗した値を算出し、当該値から前記落下期間側のスライドウィンドウの長さを求める
ことを特徴とする落下運動判定装置。
前記加速度推定部は、前記スライディングウィンドウ分割部が前記加速度の推定誤差範囲に応じて分割した分割スライディングウィンドウを用いて加速度推定を行うものであることを特徴とする請求項１記載の落下運動判定装置。
センサから得られる目標物の位置の観測値を用いて該目標物を追尾し、かつこの追尾から得られた情報及び前記センサの観測値を用いて該目標物の落下運動及び等速直進運動状態から該落下運動状態への変化を判定する落下運動判定装置において、
センサから得られる観測値に基づいて該目標物の運動諸元を推定する追尾フィルタ部と、
該追尾フィルタ部からの情報が加えられた前記観測値が反映されるスライディングウィンドウ内における前記目標物の平均の加速度を算出する加速度推定部と、
前記スライディングウィンドウを前記目標物の直進期間と落下期間に２分割するスライディングウィンドウ分割部と、
前記観測値を用いて落下運動の尤度を算出し、最終的に前記目標物が落下しているか否かを判断する落下運動尤度判定部とを備え、
前記スライディングウィンドウ分割部は、分割する前の前記スライディングウィンドウの長さを３乗した値と、前記加速度の下方向成分と、重力加速度とに基づき、分割の区切りから前記落下期間側のスライディングウィンドウの長さを３乗した値を算出し、当該値から前記落下期間側のスライドウィンドウの長さを求める
ことを特徴とする落下運動判定装置。
センサから得られる目標物の位置の観測値を用いて該目標物を追尾し、かつこの追尾から得られた情報及び前記センサの観測値を用いて該目標物の落下運動及び等速直進運動状態から該落下運動状態への変化を判定する落下運動判定装置において、
センサから得られる観測値に基づいて該目標物の運動諸元を推定する追尾フィルタ部と、
該追尾フィルタ部からの情報が加えられた前記観測値が反映されるスライディングウィンドウ内における前記目標物の平均の加速度を算出する加速度推定部と、
前記スライディングウィンドウを前記目標物の直進期間と落下期間に２分割するスライディングウィンドウ分割部と、
前記観測値に基づく有限個の落下運動候補のそれぞれに対する落下運動の尤度を算出すると共に、該尤度の総和を用いて最終的に前記目標物が落下しているか否かを判断する落下運動適合判定部とを備え、
前記スライディングウィンドウ分割部は、分割する前の前記スライディングウィンドウの長さを３乗した値と、前記加速度の下方向成分と、重力加速度とに基づき、分割の区切りから前記落下期間側のスライディングウィンドウの長さを３乗した値を算出し、当該値から前記落下期間側のスライドウィンドウの長さを求める
ことを特徴とする落下運動判定装置。