(57)【要約】【課題】複数のセンサを用いて移動体の方位角を計測し、その計測結果から移動体の位置及び速度を追尾する場合において、移動体が複数のセンサを結ぶ直線に近い位置にある場合には、方位角のみによる位置の算出を行うと、計測誤差が大きくなるという課題があった。【解決手段】複数のセンサに到来する電波の方位角を算出する測角手段２に加えて、移動体からセンサに電波が到来する時間差を算出する到来時間差計測手段３とを備え、測角値と到来時間差を用いて追尾処理を行うようにした。【選択図】              図３
2004301586.tif 000002
【特許請求の範囲】
【請求項１】
移動体からの電波を受信する複数個のセンサと、
前記複数個のセンサに到着する前記電波の到着時間差を計測する到着時間差計測手段と、
前記複数個のセンサに前記電波が到着する方位角を計測する測角手段と、
前記到着時間差計測手段と前記測角手段が出力した計測値に基づいて移動体の移動状態を追尾し、該移動状態の平滑値及び予測値を出力する追尾手段とを備えたことを特徴とする移動軌跡計測装置。
【請求項２】
移動体からの電波を受信する複数個のセンサと、
前記複数個のセンサに到着する前記電波の到着時間差を計測する到着時間差計測手段と、
前記複数個のセンサに前記電波が到着する方位角を計測する測角手段と、
前記到着時間差計測手段と前記測角手段が出力した計測値から前記移動体の追尾に有意な計測値のみを選択し出力する測定値選択手段と、
前記測定値選択手段が出力した計測値に基づいて移動体の移動状態を追尾し、該移動状態の平滑値及び予測値を出力する追尾手段とを備えたことを特徴とする移動軌跡計測装置。
【請求項３】
前記測定値選択手段は、前記追尾手段が出力した前回サンプリングまでの移動状態に含まれる移動体の位置と前記複数個のセンサの位置との距離を算出するセンサ距離算出手段と、
前記複数個のセンサのうち、前記センサ距離算出手段が算出した距離が所定の値以下であるセンサを選択し、前記到着時間差計測手段と前記測角手段から前記選択したセンサについての計測値を取得して出力するセンサ選択手段とを備えたことを特徴とする請求項２に記載した移動軌跡計測装置。
【請求項４】
前記測定値選択手段は、前記追尾手段が出力した前回サンプリングまでの移動状態に基づいて、前記複数個のセンサについての計測値のうち、その測位精度が所定の推定精度となる計測値のみを選択する観測値選択手段を備えることを特徴とする請求項２に記載した移動軌跡計測装置。
【請求項５】
前記追尾手段は、前記移動体についての前回サンプリングまでの平滑ベクトル及び平滑誤差共分散行列に基づくカルマンフィルターによる予測値を用いて、前記到着時間差計測手段の計測値が有効か否かを判定する時間差相関判定手段と、
前記測角手段の計測値のうち、前記時間差相関判定手段が有効であると判定した前記センサからの計測値のみを選択する測角値選択手段と、
前記測角値選択手段が選択した計測値と前記予測値との相関の有無を判定する測角相関判定手段と、
前記測角相関判定手段が相関を有すると判定した前記測角手段の計測値について、今回サンプリング分の平滑ベクトル及び平滑誤差共分散行列とを算出する測角平滑手段とを備えることを特徴とする請求項１乃至請求項４のいずれか一に記載された移動軌跡計測装置。
【請求項６】
前記追尾手段は、前記移動体についての前回サンプリングまでの平滑ベクトル及び平滑誤差共分散行列に基づくカルマンフィルターによる予測値を用いて、前記到着時間差計測手段の計測値が有効か否かを判定する時間差相関判定手段と、
前記測角手段の計測値のうち、前記時間差相関判定手段が有効であると判定した前記到着時間差計測手段の計測値について、今回サンプリング分の平滑ベクトル及び平滑誤差共分散行列とを算出する時間差平滑手段と、
前記測角手段の計測値と、前記時間差平滑手段が算出した今回サンプリング分の平滑ベクトル及び平滑誤差共分散行列と、前記予測値との相関の有無を判定する測角相関判定手段と、
前記測角相関判定手段が相関を有すると判定した前記測角手段の計測値について、今回サンプリング分の平滑ベクトル及び平滑誤差共分散行列とを算出する測角平滑手段とを備えることを特徴とする請求項１乃至請求項４のいずれか一に記載された移動軌跡計測装置。
【請求項７】
前記追尾手段は、前記移動体についての前回サンプリングまでの平滑ベクトル及び平滑誤差共分散行列に基づくカルマンフィルターによる予測値を用いて、前記到着時間差計測手段の計測値が有効か否かを判定する時間差相関判定手段と、
前記測角手段の計測値のうち、前記時間差相関判定手段が有効であると判定した前記到着時間差計測手段の計測値について、今回サンプリング分の平滑ベクトル及び平滑誤差共分散行列とを算出する時間差平滑手段と、
前記測角手段の計測値のうち、前記時間差相関判定手段が有効であると判定した前記センサからの計測値のみを選択する測角値選択手段と、
前記測角値選択手段が選択した計測値と、前記時間差平滑手段が算出した今回サンプリング分の平滑ベクトル及び平滑誤差共分散行列と、前記予測値との相関の有無を判定する測角相関判定手段と、
前記測角相関判定手段が相関を有すると判定した前記測角手段について、今回サンプリング分の平滑ベクトル及び平滑誤差共分散行列とを算出する測角平滑手段とを備えることを特徴とする請求項１乃至請求項４のいずれか一に記載された移動軌跡計測装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、競走馬，自転車，船挺，自動車などの移動体の各時刻の位置及び速度を電波を用いて計測する移動軌跡計測装置に係るものであり、特に電波の到着方向に加えて電波のセンサへの到着時間差を用いることで、計測値の精度を向上する移動軌跡計測装置に関する。
【０００２】
【従来の技術】
従来、競馬場の競走馬や競艇場の船艇などの移動体の位置と速度を測定する方法としては、図１に示すように経路側帯にセンサを複数個配置し、これらの複数のセンサがそれぞれ移動体から到着する電波の到着角度を取得して、これらの到着角度に基づいて算出する方法があった。これは複数のセンサにおける電波の到着角度をなす直線同士が交わる点に移動体が存在する、という原理に基づくものである（例えば、特許文献１）。
【０００３】
【特許文献１】
特開平１１−２４８８３０「走行経路計測装置」（第１図、第３頁〜第１４頁）
【０００４】
【発明が解決しようとする課題】
到着角度の計測には計測誤差が存在する。図２は、この計測誤差による位置の計算誤差を概念的に示した図である。図において、点Ａはセンサ１０１とセンサ１０２とを結ぶ直線から十分に離れた点である。点Ａに移動体が存在する場合、センサ１０１への電波到着方向は、理想的には点Ａとセンサ１０１とを結ぶ線分の方向となる。
【０００５】
しかし現実には測角誤差のため、電波の到着方向検出結果は、線分ａ１と線分ａ２のなす角度のような広がりを有する。同様にセンサ１０２においても、測角結果は線分ｂ１と線分ｂ２のなす角度のような広がりを有する。このため移動体の位置は、線分ａ１、ａ２、ｂ１、ｂ２で囲まれた四角形Ａ１を最大誤差とする精度で検出される。仮に移動体が、センサ１０１とセンサ１０２とを結ぶ直線との距離が近い点Ｂにある場合、点Ａの場合と同様に測角誤差を考慮すると、移動体の位置の測定誤差は四角形Ｂ１のようになり、点Ａの場合に比べて誤差が大きくなってしまう。
【０００６】
この発明は上記のような問題を解決するためになされたもので、電波の到着方向に加えて、移動体から各センサへの電波の到着時間差を利用してセンサに近い位置に移動体が存在するような場合であっても、精度よく位置及び速度の計測を行うものである。
【０００７】
【課題を解決するための手段】
この発明に係る移動軌跡計測装置は、移動体からの電波を受信する複数個のセンサと、
前記複数個のセンサに到着する前記電波の到着時間差を計測する到着時間差計測手段と、
前記複数個のセンサに前記電波が到着する方位角を計測する測角手段と、
前記到着時間差計測手段と前記測角手段が出力した計測値に基づいて移動体の移動状態を追尾し、該移動状態の平滑値及び予測値を出力する追尾手段とを備えたものである。
【０００８】
【発明の実施の形態】
以下、この発明の実施の形態について説明する。
実施の形態１．
図３は、この発明の実施の形態１による移動軌跡計測装置の構成を示すブロック図である。図において、センサ１−１〜センサ１−Ｉは、移動体から到着する電波を検出するものであって、移動体の移動経路の周囲に配置されている。例えば、図１に示したような競馬場の走行トラック沿いに配置してもよいし、また高速道路の路側帯に配置してもよい。ここで、移動体とは競走馬，自転車，船挺，自動車など、一定範囲の走路を移動する物体（生物を含む）を意味するものとする。また移動体からの電波とは、移動体自身に装着した発信器が送信する電波であってもよいし、センサ１−１〜１−Ｉそれぞれにレーダ波源が備えられており、これらのレーダ波源から放射されたビームを移動体が反射した反射波であってもよい。
【０００９】
続いて測角手段２は、センサ１−１〜１−Ｉに移動体からの電波が到着する方向を取得するものである。到着時間差計測手段３は、移動体から電波がセンサ１−１〜１−Ｉに到着した時刻の差を取得するものである。追尾手段４は、測角手段２と到着時間差計測手段３の出力から移動体の追尾を行うものである。
【００１０】
次に図４は、追尾手段４の詳細な構成を示すブロック図である。図において、初期値設定手段５は、カルマンフィルターの平滑ベクトル及び平滑誤差共分散行列の初期値を設定するものである。平滑ベクトル用メモリ６は、平滑ベクトルの算出値を記憶する記憶装置である。平滑誤差共分散行列用メモリ７は、平滑誤差共分散行列の算出値を記憶する記憶装置である。予測手段８は、平滑ベクトル用メモリ６の記憶する平滑ベクトル及び平滑誤差共分散行列用メモリ７の記憶する平滑誤差共分散行列に基づいて、移動体の運動諸元の予測処理を行うものである。測角相関判定手段９は、入力した測角値が追尾処理に有効なデータであるか否かを判定するものである。測角平滑手段１０は、測角相関判定手段９より入力した測角値を使用して平滑処理を行うものである。
【００１１】
続いて、測角時間差選別手段１１は、追尾手段４の外部にある測角手段２が出力した測角値と到着時間差測定手段３が出力した到着時間差観測値が混在してメモリ上に存在する場合に、それぞれを分類して出力するものである。時間差相関判定手段１２は、測角時間差選別手段１１が出力する到着時間差観測値が追尾処理に有効なデータであるか否かを判定するものである。また測角値選択手段１３は、センサ１−１〜１−Ｉのうち、時間差相関判定手段１２で有効と判断された到着時間差を出力したセンサからの測角値のみを選択するものである。
【００１２】
次に実施の形態１の動作の説明に先立って、この発明における移動体の追尾方法の原理を説明する。まず、その概要は次のようなものである。すなわち、まず移動体から発信される電波を競走場内の複数のセンサで受信する。次に、各センサより見た移動体の方位角、及び、各センサから移動体への電波の到着時刻差の各時々刻々の観測結果に基づいて、拡張カルマンフィルタを使用することにより、移動体の位置や速度を追尾する。なお、拡張カルマンフィルタのアルゴリズムについては、例えば、片山徹著「応用カルマンフィルタ」などに詳しい。
【００１３】
以下では、説明の便宜上、競争場内の走行トラックを競走する競走馬などの単独の移動体についての追尾処理方法を述べるが、この処理を各移動体毎に実行すれば、複数の移動体の追尾が可能である。
【００１４】
次に原理の詳細を説明する。図１に示すように、競走場内に固定のｘ−ｙ直交座標系を設定する。センサ１−１〜１−Ｉは走行トラック沿いに所定間隔で設置されている。また、センサ１−１〜１−Ｉのうち、ｉ番目のセンサを１−ｉ（ｉ＝１，．．．，Ｉ）と表し、その位置を［Ｘｉ，Ｙｉ］Ｔと表す（以降Ｔは行列の転置を表す）。また、競走が開始されてからのサンプリング時刻をｔｋ（ｋ＝０，１，２，．．．）とする。
【００１５】
あるサンプリング時刻ｔｋにおいて、移動体からの電波を探知するのは、Ｉ個のセンサの中の全部または一部である。このセンサ番号の集合をＩｏｂｓ，ｋと呼ぶ。Ｉｏｂｓ，ｋに含まれるセンサの数をＮ（Ｉｏｂｓ，ｋ）と呼ぶこととする。サンプリング時刻ｔｋにおいて、センサ１−ｉ（ただしｉはＩｏｂｓ，ｋの任意の要素）より見た移動体の方位を角θｋ，ｉ（ｉはＩｏｂｓ，ｋの任意の要素）と表す。方位角θｋｊ（ｉはＩｏｂｓ，ｋの任意の要素）は、図１に示すように、ｙ軸と平行な方向を基準に時計回りの回転を正と定義する。
【００１６】
また、サンプリング時刻ｔｋにおいて、発信機から２個のセンサへの到着の時間差を得ることを考える。このとき、到着時間差の得られる２個のセンサの番号のペアの集合をＭｏｂｓ，ｋとする。サンプリング時刻ｔｋおいて、センサ１−ｉ１、１−ｉ２（ただしｉ１とｉ２はＭｏｂｓ，ｋの任意の２個の要素）の到着時間差を、ΔＴｋ，（ｉ１，ｉ２）（ｉ１とｉ２はＭｏｂｓ，ｋの任意の２個の要素）と表す。また、到着時間差の数をＮ（Ｍｏｂｓ，ｋ）とする。
【００１７】
サンプリング時刻ｔｋにおける移動体の位置と速度の真値で与えられる移動体の状態ベクトルｘｋを式（１）で定義する。ここで、ｄｘｋ／ｄｔ、ｄｙｋ／ｄｔは、それぞれの時間微分を示す。
【数１】
2004301586.tif 000003
【００１８】
状態ベクトルｘｋを使用して、移動体が直線コースをほぼ等速で直進運動している際の運動モデル（等速直進モデルと呼ぶ）を式（２）のように定義する。
【数２】
2004301586.tif 000004
ここで、Φｋは等速直進運動の状態遷移行列であって、式（３）によって与えられる。
【数３】
2004301586.tif 000005
【００１９】
式（２）において、ｗｋは移動体の実際の運動を等速直進運動と見なしたことに伴う誤差を表現するための加速度相当の駆動雑音ベクトルであって、式（４）、式（５）の性質も持つ正規性白色雑音であるとする。
【数４】
2004301586.tif 000006
【数５】
2004301586.tif 000007
ただし、Ｅ［］は集合平均を表す記号である。
【００２０】
またΓｋは駆動雑音の変換行列で、式（６）で表される。
【数６】
2004301586.tif 000008
【００２１】
次に、観測モデルとして、方位角の観測モデルと、到着時間差（到着距離差）の観測モデルを定義する。まず、到着時間差（到着距離差）の観測モデルを定義する。サンプリング時刻ｔｋにおいて、移動体からの電波がセンサ１−ｉ１及び１−ｉ２に探知される時間（到着時間）の差の真値ΔＴｋ，（ｉ１，ｉ２）は、移動体の状態ベクトルｘｋにより、次式によって表わされる。
【数７】
2004301586.tif 000009
ただし、ｈ（ｉ１，ｉ２）（ｘｋ）は次の通りである。
【数８】
2004301586.tif 000010
ここで、ｃは光速である。
【００２２】
したがって、センサ１−ｉ１と１−ｉ２の到着時間差の観測モデルは、式（９）によって定義される。
【数９】
2004301586.tif 000011
【００２３】
ここで、ΔＴ＊ｋ，（ｉ１，ｉ２）は移動体からセンサ１−ｉ１及び１−ｉ２への送信信号の到着時間の差の観測値であって、センサ１−ｉ１及び１−ｉ２の探知した電波に基づいて到着時間差計測手段３により算出されるものである。またνΔＴ，ｋ，（ｉ１，ｉ２）は到着時間差の観測雑音を表す。また観測雑音νΔＴ，ｋ，（ｉ１，ｉ２）は、式（１０）、式（１１）の性質を持つ正規性白色雑音であるとする。
【数１０】
2004301586.tif 000012
【数１１】
2004301586.tif 000013
【００２４】
次に、方位角の観測モデルについて定義する。サンプリング時刻ｔｋにおいて、センサ１−ｉより見た移動体の方位角の真値θｋ，ｉは、移動体の状態ベクトルｘｋにより、次式のとおり表される。
【数１２】
2004301586.tif 000014
ただし、ｆｉ（ｘｋ）は式（１３）による。
【数１３】
2004301586.tif 000015
【００２５】
したがって、センサ１−ｉの観測モデルは式（１４）によって定義される。
【数１４】
2004301586.tif 000016
ここで、θ＊ｋ，ｉはセンサ１−ｉによって探知された電波に基づいて、測角手段２によって出力される測角値である。またνθ，ｋ，ｉはこの測角値の観測雑音を表す。また観測雑音νθ，ｋ，ｉは、式（１５）、式（１６）の性質を持つ正規性白色雑音であるとする。
【数１５】
2004301586.tif 000017
【数１６】
2004301586.tif 000018
【００２６】
次に、この発明において、上記の運動モデルと観測モデルに基づき、拡張カルマンフィルタのアルゴリズムを使用して行う移動体の追尾処理方法を説明する。まず、以下のように変数を定義しておく。
【数１７】
2004301586.tif 000019
【数１８】
2004301586.tif 000020
【数１９】
2004301586.tif 000021
【数２０】
2004301586.tif 000022
【００２７】
ここで、ｘ’ｋはサンプリング時刻ｔｋまでの観測情報によりサンプリング時刻ｔｋの状態ベクトルを推定した平滑ベクトルである。またｘ”ｋはサンプリング時刻ｔｋ−１までの観測情報によりサンプリング時刻ｔｋの状態ベクトルを推定した予測ベクトルである。さらにＰ’ｋ，Ｐ”ｋは、それぞれ平滑ベクトル、予測ベクトルの誤差共分散行列である。
【００２８】
競走の開始時刻を初期サンプリング時刻ｔ０とし、平滑ベクトルの初期値ｘ’０および平滑誤差共分散行列の初期値Ｐ’０を予め設定する。ここで、平滑ベクトルの初期値には、通常の目標追尾のように、平滑ベクトルの初期値として、最初の１サンプル目で得られた観測結果より算出した目標の観測位置と、次の２サンプル目で得られた各観測結果より算出した目標の２点の観測位置に基づいて、この２点間の変位と時間間隔より算出した速度を設定する。
【００２９】
以降、時刻ｔｋにセンサ１−１〜１−Ｉのいずれかから、新たな観測値が得られる毎に、サンプリング時刻を１つ進めて、追尾処理を行う。以下に、サンプリング時刻ｔｋにおいて実行される追尾処理の具体的な方法を説明していく。サンプリング時刻ｔｋにおいて、測角値または到着時間差観測値が得られると、まず、拡張カルマンフィルタの処理アルゴリズムに従い、以下の予測処理を行う。
【数２１】
2004301586.tif 000023
【数２２】
2004301586.tif 000024
【００３０】
次に、取得された各観測値毎に相関判定および平滑処理を、到着時間差観測値についてＭ＝Ｎ（Ｍｏｂｓ，ｋ）回、測角値についてＪ＝Ｎ（Ｉｏｂｓ，ｋ）回、合計Ｍ＋Ｊ回繰り返し実行する。到着時間差観測値についての相関判定および平滑処理と、測角値についての相関判定および平滑処理との間には、特に優先順位や依存関係があるわけではない。しかし以下では、説明を簡潔にするために、まず同時刻に得られた到着時間差観測値及び測角値の処理の順番に特に優先順位はない。すなわち、同時刻に得られた到着時間差観測値間、測角値間で処理の順番が入れ替わったり、到着時間差と測角値とで処理の順番が入れ替わってもよい。しかし、以下、及び、実施の形態では、説明を簡潔にするために、まず到着時間差観測値について相関判定及び平滑処理をＭ回行い、その後、測角値について相関判定及び平滑処理をＪ回行うこととする。
【００３１】
以下、表記を簡潔にするため、Ｍ個の到着時間差観測値をΔＴ＊ｋ，１，…，ΔＴ＊ｋ，Ｍと記す。それぞれの値は、あるセンサペアから得られており、以後、必要に応じて、ΔＴ＊ｋ，１，…，ΔＴ＊ｋ，Ｍと、ΔＴ＊ｋ，（ｉ１，ｉ２），…，ΔＴ＊ｋ，（ｉ２Ｍ−１，ｉ２Ｍ）の表現を併用する。
以後、必要に応じて、例えば、時刻ｋにおけるｍ個目の到着時間差観測値がセンサａ，ｂより得られる場合、ΔＴ＊ｋ，ｍとΔＴ＊（ａ，ｂ）の表現を併用する。その他の変数についても同様である。
【００３２】
時刻ｔｋにおいて、ｍ番目の到着時間差観測値ΔＴ＊ｋ，ｍまでにより平滑処理を行った結果で得られた状態ベクトルの推定値をｘ’ｋ，ｍ、その誤差共分散行列をＰ’ｋ，ｍと表す。まず、ｍ＝０の場合の初期値として、式（２３）、式（２４）に示すように、予測ベクトルと予測誤差共分散行列を設定する。
【数２３】
2004301586.tif 000025
【数２４】
2004301586.tif 000026
【００３３】
上記初期値を設定した後、次のような到着時間差相関判定処理と到着時間差平滑処理とを、ｍ＝１，…，Ｍについて繰り返し行う。
【００３４】
まず、取得した到着時間差観測値について相関判定を行う。すなわち、この到着時間差観測値が、すでに追尾している移動体から得られる到着時間差値に相当するデータであるか否かを判定する。この到着時間差相関判定処理は、追尾処理を行う上で、極端に精度の悪い到着時間差観測値を除外する効果がある。このような精度の悪い到着時間差観測値が生ずる場合としては、例えば、センサが、移動体から発信された電波の直接波とともに、他の構造物に一旦反射してからセンサに到着する間接波を、混信して受信したことなどが考えられる。到着時間差相関判定処理では、まず、上記ｘ’ｋ，ｍ−１を使用し、式（２５）に従って、移動体の予測到着時間差ΔＴ”ｋ，ｍを算出する。
【数２５】
2004301586.tif 000027
【００３５】
次に、実際の到着時間差観測値と上記により得られた予測到着時間差値との差（残差と呼ぶ）ΔＴ＊ｋ，ｍ−ΔＴ”ｋ，ｍの大きさに基づいて、到着時間差観測値ΔＴ＊ｋ，ｍの有効性を判定する。式（９）および式（２５）によれば、残差ΔＴ＊ｋ，ｍ−ΔＴ”ｋ，ｍは、次式のようになる。
【数２６】
2004301586.tif 000028
【００３６】
ベクトルｘ’ｋ，ｍ−１と真値ｘｋの誤差が微少であることを仮定すれば、式（２６）右辺の第１項は、式（２７）のように近似することができる。
【数２７】
2004301586.tif 000029
ここで、Ｈｍ（ｘ’ｋ，ｍ−１）は、式（２８）に示すように、式（８）の関数ｈｍ（ｘｋ）の勾配のｘ’ｋ，ｍ−１における値である。
【数２８】
2004301586.tif 000030
なお、Ｘｋはｘｋを時間で微分したもの（ｄｘｋ／ｄｔ）である。またＹｋについても同様（ｄｙｋ／ｄｔ）である。
【００３７】
上記、式（２６）に、式（１１）、式（２７）、式（２８）を用いれば、ΔＴ＊ｋ，ｍ−ΔＴ”ｋ，ｍの分散値ｓｋ，ｍは、次のように計算できる。
【数２９】
2004301586.tif 000031
【００３８】
到着時間差相関判定処理では、上記分散値ｓｋ，ｍを使用し、ｄをパラメータとして式（３０）を満たす場合にのみ、この到着時間差観測値ΔＴ’ｋ，ｍを有効であると判定する。
【数３０】
2004301586.tif 000032
【００３９】
上記相関判定処理により、到着時間差観測値ΔＴ＊ｋ，ｊが有効であると判断された場合には、拡張カルマンフィルタのアルゴリズムに従い、以下の式（３１）、式（３２）、式（３３）の計算を行う。ただし、Ｉは４×４の単位行列である。
【数３１】
2004301586.tif 000033
【数３２】
2004301586.tif 000034
【数３３】
2004301586.tif 000035
【００４０】
一方、到着時間差観測値ΔＴ’ｋ，ｍが有効でないと判断された場合は、到着時間差観測値を用いた平滑処理は行わず、以下の式に示すように、平滑値および平滑誤差共分散行列を予測値および予測誤差共分散行列で置き換える。
【数３４】
2004301586.tif 000036
【００４１】
上記、式（２５）〜式（３４）の一連の処理をｍ＝１，．．．，Ｍについて繰り返し実行する。以上が、到着時間差観測値ΔＴ’ｋ，ｍを用いた相関判定および平滑処理の内容である。
【００４２】
次に、測角値について、相関判定及び平滑処理を行う。まず、到着時間差の最終平滑値を以下のとおり設定する。
【数３５】
2004301586.tif 000037
【００４３】
次に、観測モデル式（１２）〜式（１６）を用いて、測角値θ＊ｋ，ｊ（ｊ＝１，…，Ｊ）について、到着時間差観測値の場合と同様にして以下の処理を行う。まず、上記ｘ’ｋ，ｊ−１を使用し、式（３６）に従って、移動体の予測方位θ”ｋ，ｊを算出する。
【数３６】
2004301586.tif 000038
【００４４】
実際の測角値と上記得られた予測方位との差（残差と呼ぶ）θ＊ｋ，ｊ−θ”ｋ，ｊの大きさに基づいて、測角値θ＊ｋ，ｊの有効性を判定する。到着時間差の場合と同様に、θ＊ｋ，ｊ−θ”ｋ，ｊの分散値ｓｋ，ｊは式（３７）と式（３８）を用いて計算する。
【数３７】
2004301586.tif 000039
【数３８】
2004301586.tif 000040
【００４５】
測角相関判定処理では、上記分散値ｓｋ，ｊを使用し、ｄをパラメータとして式（３９）を満たす場合にのみ、この測角値θ＊ｋ，ｊを有効であると判定する。
【数３９】
2004301586.tif 000041
【００４６】
上記相関判定処理により、測角値θ’ｋ，ｊが有効であると判断された場合には、拡張カルマンフィルタのアルゴリズムに従い、以下の式（４０）、式（４１）、式（４２）による計算を行う。ただし、Ｉは４×４の単位行列である。
【数４０】
2004301586.tif 000042
【数４１】
2004301586.tif 000043
【数４２】
2004301586.tif 000044
【００４７】
一方、測角値θ＊ｋ，ｊが有効でないと判断された場合は、測角値を用いた平滑処理は行わず、式（４３）に示すように、平滑値および平滑誤差共分散行列を予測値および予測誤差共分散行列で置き換える。
【数４３】
2004301586.tif 000045
上記、式（３６）〜式（４３）の一連の処理をｊ＝１，．．．，Ｊについて繰り返し実行する。
【００４８】
この結果、最終的に得られたｘ’ｋ，ＪおよびＰ’ｋ，Ｊを、式（４４）、式（４５）に示すように、サンプリング時刻ｔｋにおける平滑ベクトルおよび平滑誤差共分散行列の算出結果として、サンプリング時刻ｔｋでの処理を終了する。
【数４４】
2004301586.tif 000046
【数４５】
2004301586.tif 000047
【００４９】
以上が、この発明における移動体の追尾方法の原理である。次に上記原理に基づいて、実施の形態１による移動軌跡計測装置の動作について説明する。
【００５０】
まず追尾手段４において、追尾の開始時刻である初期サンプリング時刻をｔ０とする。まず初期値設定手段５は、通常のカルマンフィルタの理論に基づいて、平滑値ｘ’ｋの初期値ｘ’０、及び、平滑誤差共分散行列Ｐ’ｋの初期値Ｐ’０を算出し、追尾処理を開始する。
【００５１】
次に、ある時刻ｔｋにおいてセンサ１−１〜１−ｉが移動体からの電波を探知すると、測角手段２は、この移動体からの到着した電波に基づいて測角値を算出する。さらに到着時間差計測手段３は、この移動体からの到着した電波の到着時間差を算出する。追尾手段４は、測角手段２が算出した測角値と到着時間差計測手段３が算出した到着時間差とを観測値として取得し、サンプリング時刻を１つ進めて追尾処理を行う。以下、サンプリング時刻ｔｋにおける処理を説明する。
【００５２】
追尾手段４が観測値を取得すると、予測手段８は、平滑ベクトル用メモリ６より１サンプリング前の平滑ベクトルｘ’ｋ−１と、平滑誤差共分散行列用メモリ７より１サンプリング前の平滑誤差共分散行列Ｐ’ｋ−１を読み出す。予測手段８は、予め設定された駆動雑音共分散行列Ｑｋを使用して、式（２１）及び式（２２）に基づき、予測ベクトルｘ”ｋと予測誤差共分散行列Ｐ”ｋを算出する。
【００５３】
ここで、サンプリング時刻ｔｋに、Ｊ＝Ｎ（Ｉｏｂｓ，ｋ）個の測角値θ＊ｋ，ｊ（ｊ＝１，．．，Ｊ）、及び、Ｍ＝Ｎ（Ｍｏｂｓ，ｋ）個の到着時間差観測値ΔＴ＊ｋ，ｍ（ｍ＝１，…，Ｍ）が得られたとする。そうすると、測角時間差選別手段１１は、メモリ上の測角値と到着時間差観測値の分類を行う。その結果、測角値θ＊ｋ，ｊ（ｊ＝１，．．，Ｊ）を測角値選択手段１３へ出力し、到着時間差観測値ΔＴ＊ｋ，ｍ（ｍ＝１，…，Ｍ）を、時間差相関判定手段１２へ出力する。
【００５４】
時間差相関判定手段１２は、予測ベクトルｘ”ｋ、予測誤差共分散行列Ｐ”ｋ、及び、到着時間差観測値ΔＴ＊ｋ，ｍ（ｍ＝１，…，Ｍ）の情報を取得して、式（２５）により到着時間差予測値を算出する。次に式（２８）及び式（２９）に基づき、予め設定されたｄを用いて、到着時間差観測値が式（３０）を満たすかどうかを判定する。ここで、この判定処理の結果を表す変数として、Ｃｏｒｋ，ｍを用いることとする。判定の結果、式（３０）を満たす場合、Ｃｏｒｋ，ｍに１を設定し、満たさない場合はＣｏｒｋ，ｍに０を設定する。そして時間差相関判定手段１２はＣｏｒｋ，ｍの値を出力する。
【００５５】
測角値選択手段１３は、時間差相関判定手段１２から測角値θ＊ｋ，ｊ（ｊ＝１，…，Ｊ）とＣｏｒｋ，ｍ（ｍ＝１，…，Ｍ）を取得する。続いてＣｏｒｋ，ｍ＝０となるセンサ１−ｉ１と１−ｉ２からの測角値を削除する。その後、削除されなかった測角値について、その数をＪｉｎとし、改めて番号を振りなおし、θ＊ｋ，ｊ（ｊ＝１，…，Ｊｉｎ）を出力する。
【００５６】
次に測角相関判定手段９は、予測手段８より予測ベクトルｘ”ｋと予測誤差共分散行列Ｐ”ｋを取得する。また測角値選択手段１３から測角値θ＊ｋ，ｊ（ｊ＝１，…，Ｊｉｎ）を取得する。そして式（３６）に基づき、予測ベクトルｘ”ｋを用いて、測角予測値θ”ｋ，ｊを算出する。さらに式（３７）、式（３８）を用いて、式（３９）を満たすかどうかの判定を行う。式（３９）を満たす場合、θ＊ｋ，ｊを出力する。
【００５７】
続いて測角平滑手段１０は、測角相関判定手段９からθ＊ｋ，ｊを取得する。そして式（４０）〜式（４３）を用いて、平滑処理を行う。さらに処理した測角値θ＊ｋ，ｊが、その時刻の最終測角値でない場合、その平滑ベクトルｘ’ｋ，ｊ及び平滑誤差共分散行列Ｐ’ｋ，ｊを測角相関判定手段９に出力し、その値を用いて、次の測角値に対し相関処理を行う。測角平滑手段１０が、最後の測角値の処理を終了すると、式（４４）及び式（４５）を用いて、ｘ’ｋとＰ’ｋとを算出する。最後に、測角平滑手段１０は、平滑ベクトル用メモリ６にｘ’ｋを記憶させ、さらにＰ’ｋを平滑誤差共分散行列用メモリ７に記憶させる。
【００５８】
以上から明らかなように、実施の形態１の移動軌跡計測装置は、到着時間差観測値を、時間差相関判定手段１２によって処理し、時間差相関判定手段１２で到着時間差が有効であると判断されたセンサからの測角値のみを、測角値選択手段１３によって選択することとした。この結果、信頼性の低い測角値を測角平滑処理に用いることがなくなるため、高精度な追尾結果を得ることができる。
【００５９】
なお、実施の形態１において測角時間差選別手段１１は測角手段２の出力と時間差計測手段３の出力が混在した場合に、それらの出力を分類する機能を有するものとしたが、測角手段２の出力が測角値選択手段１３に直接なされ、さらに到着時間差計測手段３の出力が直接時間差相関判定手段１２に直接なされるのであれば、測角時間差選別手段１１は必須の構成要素ではない。
【００６０】
また実施の形態１は、走行トラックを移動する競走馬のような移動体を例に説明した。しかし、その他経路に沿って複数のセンサが配置されている場所（陸上・海上を問わない）を移動する移動体の位置や速度を計測する上でも、実施の形態１による移動軌跡計測装置が適用できることはいうまでもない。
【００６１】
実施の形態２．
実施の形態１による移動軌跡計測装置は、所定の条件に合致しなかった測角値を排除することで、測角平滑処理に用いる測角値の信頼性を向上し、その結果として高精度な追尾結果を得るものであった。これに対して実施の形態２による移動軌跡計測装置は、電波の到着時刻について平滑処理を行う点を特徴とするものである。なお実施の形態２は、実施の形態１と比して、追尾手段４の構成のみが異なるものである。したがって以下では、追尾手段４の詳細な構成と動作についてのみ説明することとする。
【００６２】
図５は、実施の形態２による追尾手段４の詳細な構成を示すブロック図である。図において、時間差平滑手段１４は、移動体から電波がセンサ１−１〜１−Ｉに到着した到着時間差について平滑処理を行うものである。また、実施の形態２では、実施の形態１の測角値選択手段１３を用いない。その他、実施の形態１の追尾手段４と同じ符号を付した構成要素については、実施の形態１と同様であるので説明を省略する。
【００６３】
次に実施の形態２における追尾手段４の動作について説明する。センサ１−１〜１−Ｉが探知した電波について、測角手段２と到着時間差計測手段３が観測値を算出すると、予測手段８は、実施の形態１と同様に、予測ベクトルｘ”ｋと予測誤差共分散行列Ｐ”ｋを算出する。そして、測角時間差選別手段１１は、測角値と到着時間差観測値の整理を行い、測角値θ＊ｋ，ｊ（ｊ＝１，…，Ｊ）を測角相関判定手段９に出力し、到着時間差観測値ΔＴ＊ｋ，ｍ（ｍ＝１，…，Ｍ）を、時間差相関判定手段１２に出力する。
【００６４】
続いて、時間差相関判定手段１２は、予測ベクトルｘ”ｋと予測誤差共分散行列Ｐ”ｋ、及び到着時間差観測値到着時間差観測値ΔＴ＊ｋ，ｍ（ｍ＝１，…，Ｍ）を取得する。そして式（２５）を用いて到着時間差予測値を算出し、式（２８）、式（２９）、及び式（３０）に従い、予め設定されたｄを用いて、到着時間差観測値ΔＴ＊ｋ，ｍ（ｍ  ＝１，．．．，Ｍ）が式（３０）を満たすかどうかの判定を行う。判定の結果、式（３０）を満たす場合、Ｃｏｒｋ，ｍ＝１とし、到着時間差観測値ΔＴ＊ｋ，ｍを、時間差平滑器１４へ出力する。満たさない場合は、Ｃｏｒｋ，ｍ＝０とする。
【００６５】
時間差平滑手段１４は、時間差相関判定手段１２から、到着時間差観測値ΔＴ＊ｋ，ｍ、予測ベクトルｘ”ｋ及び予測誤差共分散行列Ｐ”ｋを取得する。その後、まず時刻ｋにおいて初めての到着時間差観測値である場合に、式（２３）及び式（２４）を用いて、ｘ’ｋ，ｍ−１とＰ’ｋ，ｍ−１を設定する。そして、式（３１）〜式（３４）に従い、状態ベクトルの推定値ｘ’ｋ，ｍ及びその誤差共分散行列Ｐ’ｋ，ｍを計算し、時間差相関判定手段１２へ出力する。この一連の動作をすべての到着時間差観測値について行い、時間差平滑手段１４が最後の到着時間差観測値を処理する。さらにそのときの平滑ベクトルｘ’ｋ，ｍ及びその誤差共分散行列Ｐ’ｋ，ｍを、式（３５）のように設定し、測角相関判定手段９、及び測角平滑手段１０に出力する。
【００６６】
次に測角相関判定手段９は、測角時間差選別手段１１から測角値θ＊ｋ，ｉ（ｉはＩｏｂｓ，ｋの任意の要素）、また、時間差平滑手段１４から、ｘ’ｋ，ｊ及びその誤差共分散行列Ｐ’ｋ，ｊを取得する。そして、推定ベクトルｘ’ｋ，ｊを用いて式（３６）から測角予測値θ＊ｋ，ｊを算出する。そして、式（３７）、式（３８）を用いて、式（３９）を満たすかどうかの判定を行う。式（３９）を満たす場合は、θ＊ｋ，ｊを測角平滑手段１０に出力する。
【００６７】
測角平滑手段１０は、測角相関判定手段９から測角値θ＊ｋ，ｊとｘ’ｋ，ｊ、及びその誤差共分散行列Ｐ’ｋ，ｊを取得すると、式（４０）〜式（４３）による平滑処理を行い、ｘ’ｋ，ｊ及びＰ’ｋ，ｊを測角相関判定手段９に出力する。測角相関判定手段９と測角平滑手段１０がすべての測角値についての処理を繰り返し行う。測角平滑手段１０が、最後の測角値の処理を終了すると、式（４４）及び式（４５）を用いて、ｘ’ｋとＰ’ｋとを算出する。最後に、測角平滑手段１０は、平滑ベクトル用メモリ６にｘ’ｋを記憶させ、さらにＰ’ｋを平滑誤差共分散行列用メモリ７に記憶させる。
【００６８】
以上から明らかなように、実施の形態２の移動軌跡計測装置は、到着時間差観測値について、時間差相関判定手段１２によって相関処理し、さらに時間差平滑手段１４によって平滑処理することとした。この結果、測角値のみを用いる場合に比べ、高精度な追尾結果を得ることができる。
【００６９】
実施の形態３．
なお、実施の形態１による移動軌跡計測装置における測角値選択手段１３と、実施の形態２による移動軌跡計測装置における時間差平滑手段１４の双方を備えるようにしてもよい。図６は、このような移動軌跡計測装置の追尾手段４の構成図である。実施の形態１及び実施の形態２の追尾手段４と同じ符号を付した構成要素については、実施の形態１及び実施の形態２と同様であるので、説明を省略することとする。
【００７０】
次に実施の形態３における追尾手段４の動作について説明する。追尾手段４では、実施の形態２における追尾処理と同様に時間差平滑手段１４による電波の到着時間差の平滑処理を行う。その詳細は実施の形態２と同様である。
【００７１】
さらに加えて、実施の形態３における追尾手段４では、実施の形態１による移動軌跡計測装置における測角値選択手段１３によって、時間差相関結果による測角値の選択処理を行う。具体的には、時間差相関判定手段１２において、式（２８）及び式（２９）に基づき、予め設定されたｄを用いて、到着時間差観測値が式（３０）を満たすかどうかを判定する。次に測角値選択手段１３は、式（３０）を満たす測角値のみを選択して出力する。以降、出力された測角値は測角相関判定手段９に入力され、実施の形態１と同様に平滑値及び平滑誤差共分散行列を算出する。
【００７２】
以上から明らかなように、実施の形態３による移動軌跡計測装置では、到着時間差観測値について、時間差相関判定手段１２が相関処理を行い、時間差平滑手段１４が平滑処理を行うとともに、時間差相関判定手段１２が有効であると判断した到着時間差を算出したセンサからの測角値のみを、測角値選択手段１３が選択することとした。これにより、信頼性の低い測角値を測角平滑処理に用いることがなくなることにより、また測角値のみを用いる場合に比べ、高精度な追尾結果を得ることができる。
【００７３】
実施の形態４．
さらに、追尾処理を行う前に優れた測位精度が得られる観測値のみを選択し、選択された観測値に基づいて追尾処理を行うようにしてもよい。実施の形態４はこのような移動軌跡計測装置に関するものである。
【００７４】
図７は、実施の形態４による移動軌跡計測装置の構成を示すブロック図である。図において、測定値選択手段１５は追尾処理に有意な観測値を選択するものである。ここで、有意な観測値とは、観測値の精度が著しく劣るものや、目標とセンサの距離が長いため、測位にほとんど有用でない観測値などを除いた観測値をいう。その他、図３と同じ符号を付した構成要素については、実施の形態１と同様であるので説明を省略する。また実施の形態４における追尾手段４には、実施の形態１乃至実施の形態３のどの追尾手段を採用してもよい。ただし、実施の形態４における測定値選択手段１５は、観測値を選択する基準として追尾手段４の算出する予測値を用いるので、測定値選択手段１５に追尾手段４における予測手段８の算出結果を入力できるようになっているものとする。
【００７５】
次に測定値選択手段１５の詳細な構成について説明する。図８は、測定値選択手段１５の詳細な構成を示すブロック図である。図において、センサ距離算出手段１６は、移動体とセンサ１−１〜１−Ｉとの距離を算出するものである。センサ選択手段１７は、センサ距離算出手段１６によって算出されたセンサ１−１〜１−Ｉの移動体からの距離が所定値以下の場合に、そのセンサからの観測値を選択するものである。観測値選択手段１８は、測角手段２及び到着時間差計測手段３が出力する観測値のうち、信頼性の高い観測値を選択するものである。
【００７６】
続いて、実施の形態４による移動軌跡計測装置の動作について説明する。なお、実施の形態４における移動軌跡計測装置は実施の形態１と比して、測定値選択手段１５のみが異なるので、以下においては、測定値選択手段１５の動作の説明を中心に行う。
【００７７】
まず測定値選択手段１５は、追尾手段４の出力する予測値ｘ”ｋを取得する。するとセンサ距離算出手段１６は、予測値ｘ”ｋとセンサ１−１〜１−Ｉの位置との距離を算出する。次にセンサ選択手段１７は、センサ距離算出手段１６が算出した各センサ１−１〜１−Ｉの位置と予測値との距離情報、及び、測角値及び到着時間差観測値ΔＴ＊ｋ，ｍを取得する。そして、移動体の位置の予測値との距離が著しく遠いセンサからの測角値及び到着時間差観測値を削除し、残りの観測値を出力する。
【００７８】
観測値選択手段１８は、センサ選択手段１７が出力した観測値を取得する。そして、センサ位置と目標予測値ｘ”ｋの情報を用い、測位精度情報から、優れた推定精度が得られ、かつ無駄のないように観測値の選出を行う。測位精度情報については、例えば、Ｄ．  Ｊ．  Ｔｏｒｒｉｅｒｉ，  ”Ｓｔａｔｉｓｔｉｃａｌ  Ｔｈｅｏｒｙ  ｏｆ  Ｐａｓｓｉｖｅ  Ｌｏｃａｔｉｏｎ  Ｓｙｓｔｅｍｓ，”  ＩＥＥＥ  Ｔｒａｎｓ．  ｏｎ  ＡＥＳ，  Ｖｏｌ．  ＡＥＳ−２０，  Ｎｏ．２，  ｐｐ．１８３−ｐｐ．１９８，  Ｍａｒ  １９８４．に示されている方法を用いて算出を行う。
【００７９】
上記方法による測位精度情報から、優れた測位精度の得られる観測値を選択して、残りの観測値を削除する。そして、選択された測角値及び到着時間差観測値を観測値を追尾手段４に出力する。以後、実施の形態１乃至実施の形態３と同様の処理を行う。
【００８０】
以上から明らかなように、実施の形態４の移動軌跡計測装置によれば、予め有意な観測値のみを選択してから追尾処理を行うようにするので、計算負荷の軽減が可能となり、また追尾処理の精度を向上させることができる。
【００８１】
なお、距離情報に基づく有意な観測値の選択処理、及び、測位精度情報による観測値の選択処理は、それぞれ単独で用いても同様の効果を奏することはいうまでもない。
【００８２】
【発明の効果】
この発明に係る移動軌跡計測装置は、移動体から到着した電波の測角値に加えて、前記複数個のセンサの各々へ到着する前記電波の到着時間差を計測する到着時間差計測手段と、前記到着時間差計測手段が出力する到着時間差と測角値に基づいて移動体の位置及び速度を追尾する追尾手段とを備えることにより、測角値のみでは高い精度が得られない位置に移動体が存在する場合にも、位置と速度の測定を高精度に行うことができるという効果を奏するものである。
【図面の簡単な説明】
【図１】この発明及び従来技術によるセンサと移動体の関係を示す説明図である。
【図２】従来技術による測定誤差の説明図である。
【図３】この発明の実施の形態１の構成を示すブロック図である。
【図４】この発明の実施の形態１の追尾手段４の構成を示すブロック図である。
【図５】この発明の実施の形態２の追尾手段４の構成を示すブロック図である。
【図６】この発明の実施の形態３の追尾手段４の構成を示すブロック図である。
【図７】この発明の実施の形態４の構成を示すブロック図である。
【図８】この発明の実施の形態４の測定値選択手段の構成を示すブロック図である。
【符号の説明】
１−１〜１−Ｉ：センサ、２：測角手段、３：到着時間差計測手段、
４：追尾手段、５：初期値設定手段、６：平滑ベクトル用メモリ、
７：平滑誤差共分散行列用メモリ、８：予測手段、９：測角相関判定手段、
１０：測角平滑手段、１１：測角時間差選別手段、１２：時間差相関判定手段、
１３：測角値選択手段、１４：時間差平滑手段、１５：測定値選択手段、
１６：センサ選択手段、１７：不要観測値削除手段、１８：観測値選択手段
【図１】
2004301586.tif 000048
【図２】
2004301586.tif 000049
【図３】
2004301586.tif 000050
【図４】
2004301586.tif 000051
【図５】
2004301586.tif 000052
【図６】
2004301586.tif 000053
【図７】
2004301586.tif 000054
【図８】
2004301586.tif 000055