角速度推定装置及びコンピュータプログラム及び角速度推定方法
【課題】移動体の角速度を精度よく推定する。
【解決手段】周辺物体観測装置８１１（レーダ）は、移動体の周辺に存在する物体について、移動体を基準とした物体の相対位置を繰り返し観測する。相対位置取得部２１１は、周辺物体観測装置８１１が観測した観測結果を取得する。静止物体判定部２２０（停止物識別部）は、周辺物体観測装置８１１が相対位置を観測した物体が静止しているか否かを判定する。物体相関部２３０（停止物用追尾部）は、周辺物体観測装置８１１が観測した複数の相対位置のなかから、同一の物体について周辺物体観測装置８１１が観測した複数の相対位置を判定する。角速度推定部（状態推定部２４０、軌道推定部）は、移動体の角速度を推定する。
【解決手段】周辺物体観測装置８１１（レーダ）は、移動体の周辺に存在する物体について、移動体を基準とした物体の相対位置を繰り返し観測する。相対位置取得部２１１は、周辺物体観測装置８１１が観測した観測結果を取得する。静止物体判定部２２０（停止物識別部）は、周辺物体観測装置８１１が相対位置を観測した物体が静止しているか否かを判定する。物体相関部２３０（停止物用追尾部）は、周辺物体観測装置８１１が観測した複数の相対位置のなかから、同一の物体について周辺物体観測装置８１１が観測した複数の相対位置を判定する。角速度推定部（状態推定部２４０、軌道推定部）は、移動体の角速度を推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、移動体の角速度を推定する角速度推定装置に関する。
【背景技術】
【０００２】
ジャイロセンサなど移動体の角速度を観測する角速度観測装置には、バイアス誤差などの誤差がある。全地球測位システム（ＧＰＳ）などの位置観測装置を用いて、移動体の位置を観測し、観測結果から移動体の角速度を推定する方式がある。また、位置観測装置の観測結果から推定した角速度を用いて、角速度観測装置が観測した角速度を補正する方式がある。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平６−１４８３１９号公報
【特許文献２】特開平９−４９８７５号公報
【特許文献３】特開２００７−３３３３８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ＧＰＳのように外部のシステムに頼って移動体の位置を観測する位置観測装置は、電波状況などによっては使えない場合がある。
この発明は、例えば上記のような課題を解決するためになされたものであり、ＧＰＳのような位置観測装置に頼ることなく、移動体の角速度を精度よく推定することを目的とする。
【課題を解決するための手段】
【０００５】
この発明にかかる角速度推定装置は、
相対位置取得部と、静止物体判定部と、物体相関部と、角速度推定部とを有し、
上記相対位置取得部は、移動体の周辺に存在する物体について、上記移動体を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置が観測した観測結果を取得し、
上記静止物体判定部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が相対位置を観測した物体が静止しているか否かを判定し、
上記物体相関部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置が観測した複数の相対位置を判定し、
上記角速度推定部は、上記相対位置取得部が取得した観測結果と、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、上記移動体の角速度を推定することを特徴とする。
【発明の効果】
【０００６】
この発明にかかる角速度推定装置によれば、ＧＰＳのような位置観測装置がなくても、移動体の角速度を精度よく推定することができる。
【図面の簡単な説明】
【０００７】
【図１】実施の形態１におけるヨーレートバイアス補正装置８００の構成を示すブロック図。
【図２】時刻ｔ１〜ｔＮまで時間方向に連続して相関のとれた停止物の様子を表わす図。
【図３】実施の形態１における相関停止物選択部１４１の処理を示す処理フロー図。
【図４】停止物基準座標系への座標変換を示す図。
【図５】停止物基準座標系における自車軌道を示す図。
【図６】実施の形態１におけるバイアス補正部８４０の処理を示す処理フロー図。
【図７】時刻合わせを示す図。
【図８】実施の形態２におけるヨーレートバイアス補正装置８００の構成を示すブロック図。
【図９】複数停止物の相対位置の一例を示す図。
【図１０】タイムシーケンスの説明図。
【図１１】実施の形態３におけるヨーレートバイアス補正装置８００の構成を示すブロック図。
【図１２】実施の形態３における軌道統合部１５０の動作を示す処理フロー図。
【図１３】実施の形態４における停止物用追尾部１２０の構成を示すブロック図。
【図１４】実施の形態５における角速度推定装置２００のハードウェア構成の一例を示すハードウェア構成図。
【図１５】実施の形態５における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図。
【図１６】移動体８０１の移動速度と、物体７０１，７０２の移動体８０１に対する相対速度との関係を説明するための図。
【図１７】物体の移動体に対する相対速度の観測時刻７８１〜７８３と、移動体の移動速度の観測時刻７９１〜７９９との関係を説明するための図。
【図１８】移動体８０１の移動軌跡７５１と、物体７０３〜７０８の移動体８０１に対する相対位置の軌跡７６１〜７６６との関係を説明するための図。
【図１９】実施の形態５における物体相関部２３０の動作を説明するための図。
【図２０】移動体８０１を基準とした相対座標系と、静止点を基準とした絶対座標系との間の関係を説明するための図。
【図２１】実施の形態５における角速度推定処理Ｓ５００の流れの一例を示すフローチャート図。
【図２２】実施の形態５における観測結果取得工程Ｓ５１０の流れの一例を示すフローチャート図。
【図２３】実施の形態５における静止判定工程Ｓ５２０の流れの一例を示すフローチャート図。
【図２４】実施の形態５における物体相関工程Ｓ５３０の流れの一例を示すフローチャート図。
【図２５】実施の形態５における状態推定工程Ｓ５５０の流れの一例を示すフローチャート図。
【図２６】物体相関部２３０が相関処理に用いる平行移動量および回転角度と、移動体８０１の移動量の関係を説明するための図。
【図２７】実施の形態７における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図。
【図２８】実施の形態８における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図。
【発明を実施するための形態】
【０００８】
車両に搭載されているレーダ装置を用いて、車両前方を監視するに際して、車両前方の物体を検出する装置について、説明する。
【０００９】
車両に搭載し、光波・ミリ波などの送信波を車両前方の所定角度に照射して、その反射波を受信することによって、自車両の前方を走行する車両、自車両の前方の障害物を認識する、車両用前方監視装置がある。車両用前方監視装置においては、検出した物体の中から、直線路走行時、カーブ路走行時に依らず、自車両の進行方向に存在する車両を先行車両として認識する必要がある。このとき、自車進行方向に存在する先行車両を認識する方式として、車速とヨーレート（角速度）センサを用いて道路曲率を算出し、先行車両を認識する。
ただし、道路曲率を算出するために用いるヨーレートセンサには電圧オフセット分の零点バイアス誤差が発生し、さらに温度や時間変化に伴い零点バイアス誤差がドリフトするため、そのまま使用すると先行車判定を誤る可能性が高い。そのため、センサの零点バイアス補正を行う必要がある。
【００１０】
自車のヨーレート（以下、ヨー角速度）を算出するために、自車両のレーダで検出した複数の停止物の前時刻における角度と現時刻における角度の相関をとる方式がある。相関方式としては、前時刻で検出された停止物の角度をΔθだけずらし、現時刻で検出された停止物の角度と最も相関がとれるΔθを角度変化量とみなして、ヨー角速度を算出する方式がある。しかし、実際の道路走行環境において停止物は時間方向に必ずしも連続して検出できるとは限らない。例えば、途中で失検出、再出現、もしくはマルチパス波による不要信号の混入などが起こるため、上記方法ではヨー角速度を正確に推定できない。
【００１１】
また、自車両の停止状態（速度が０の場合）を検出するために、検出物体との相対速度が０となる条件を用いて、低速時のヨー角速度をヨー角速度零点バイアス値とみなして、補正する方式がある。ただし、自車が低速時でなければバイアス補正できない。
【００１２】
また、ＧＰＳで得た自車の絶対位置とレーダで検出した停止物との相対位置より、ヨー角速度を含む自車両の運動を算出する方式がある。この方式では、自車の運動とは無関係の停止物も含めて自車軌道を計算することから、ＧＰＳで得られる絶対位置なしでヨー角速度を正確に算出することはできない。
【００１３】
このように、ヨーレートセンサには電圧オフセット分の零点バイアス誤差が発生し、さらに温度や時間変化に伴い零点バイアス誤差がドリフトするため、そのまま使用すると先行車判定を誤る可能性が高い。
このため、複数の停止物の中から追尾フィルタによって時間方向に相関のある停止物を抽出し、その停止物と自車両の相対位置の時系列データより自車の位置と運動（本文中では軌道と呼ぶ）に基づくヨー角速度推定値を算出する。さらにヨー角速度観測値と推定値との差よりバイアス誤差を計算し、ヨー角速度を補正する。
【００１４】
実施の形態１．
実施の形態１について、図１〜図７を用いて説明する。
【００１５】
図１は、この実施の形態におけるヨーレートバイアス補正装置８００の構成を示すブロック図である。
ヨーレートバイアス補正装置８００は、レーダ８１０と、車速センサ８２０と、ヨーレートセンサ８３０と、停止物識別部１１０と、停止物用追尾部１２０と、停止物記憶部１３０と、自車軌道計算部１４０と、バイアス補正部８４０とを備える。
レーダ８１０（周辺物体観測装置）は、車両前方の所定角度範囲内にレーダ波を照射し、物体に反射されたレーダ波を受信して、その物体の位置及び相対速度を検出する。
車速センサ８２０（移動速度観測装置）は、車両の速度を検出する。
ヨーレートセンサ８３０（角速度観測装置）は、ヨー角速度を検出する。
停止物識別部１１０（相対位置取得部、速度観測値取得部、静止物体判定部）は、上記レーダ８１０により検出された物体の相対速度と上記車速センサ８２０により検出された車両の速度に基づいて当該物体が停止物体であるか否かを識別する。
停止物用追尾部１２０（物体相関部）は、上記停止物識別部１１０により停止物体であると識別された複数の停止物の中から追尾フィルタによって時間方向に相関のある停止物を抽出する。停止物用追尾部１２０は、例えば、相関部１２１と、平滑部１２２と、予測部１２３と、遅延部１２４とを有する。
停止物記憶部１３０は、上記停止物用追尾部１２０で相関済みの停止物を記憶する。
自車軌道計算部１４０（角速度推定部）は、上記停止物記憶部１３０より得た停止物位置を入力として、前記停止物位置を停止物基準の座標系へ変換することで自車位置を得て、前記自車位置よりヨー角速度を含む自車軌道を推定する。自車軌道計算部１４０は、例えば、相関停止物選択部１４１と、停止物基準座標変換部１４２と、軌道推定部１４３とを有する。
バイアス補正部８４０は、ヨーレートセンサ８３０で検出するヨー角速度と前記自車軌道計算部１４０より得たヨー角速度推定値を用いて、ヨー角速度のバイアス誤差を算出し、補正する。
【００１６】
図２は、時刻ｔ１〜ｔＮまで時間方向に連続して相関のとれた停止物の様子を表わす図である。
停止物を追尾すると、自車両の動きに応じて移動することがわかる。
【００１７】
停止物用追尾部１２０では、この図のように追尾フィルタ（カルマンフィルタやα−β（−γ）フィルタなどの公知のフィルタ）によって複数の停止物の中から時間方向に相関のとれる有効な停止物を抽出する。この図の白丸は、時間方向に相関がとれた同一停止物、斜線付き丸は、前記停止物（白丸）の最新時刻の位置、黒丸は時間方向に連続性がなく、停止物用追尾部１２０で相関のとれない停止物を意味する。停止物用追尾部１２０からは、相関のとれた停止物の位置（ドップラ速度があればドップラ速度も含める）と当該停止物番号（同一停止物を表す番号）を出力する。
停止物記憶部１３０では、停止物番号とその相関済み停止物の時系列データを記録し、記録された相関停止物の時系列データと当該停止物番号を相関停止物選択部１４１へ出力する。
相関停止物選択部１４１では、同一停止物ごとの相関停止物時系列データを入力とし、前記相関停止物時系列データの中から、あらかじめ設定した距離Ｒｍｉｎ（例えば３０ｍ）以内に入った相関停止物のみを抽出し、相関回数Ｎｃｏｒをカウントする。さらに前記相関回数Ｎｃｏｒが相関回数閾値Ｎｍｉｎ以上の場合は、前記相関停止物の位置を出力する。
【００１８】
図３は、この実施の形態における相関停止物選択部１４１の処理を示す処理フロー図である。
ステップ１において、相関停止物選択部１４１は、距離閾値以内に入る相関停止物を抽出する。
ステップ２において、相関停止物選択部１４１は、相関停止物数が閾値以上であるか否かを判定する。
相関停止物数が閾値以上である場合、ステップ３において、相関停止物選択部１４１は、上記相関停止物を出力する。
【００１９】
ここで、相関回数閾値Ｎｍｉｎは、自車速ＶＭ、レーダ観測レートＴ、距離Ｒｍｉｎを用いて次の式のように上限を定める。
【数１１】
例えば、Ｒｍｉｎ＝３０［ｍ］、ＶＭ＝１６［ｍ／ｓ］、Ｔ＝０．２［ｓ］とすると、レーダで９回程度観測は可能であるが、それ以上は観測されない。そのため、数１１の条件を満たす閾値Ｎｍｉｎを設定することで、以降の処理を確実に実行する。
【００２０】
停止物基準座標変換部１４２では、レーダ位置を原点とした座標系からある時刻の停止物を原点とした座標系へ変換する。本来、停止物の位置はレーダを原点とした座標系においてレーダとの相対位置で得られる。そこで、停止物のある時刻の位置を座標系の原点として、自車両の位置を座標変換することで、停止物から見た自車の位置を用いて、自車両の軌道を算出する。以降、例を用いて説明する。
【００２１】
図４は、停止物基準座標系への座標変換を示す図である。
この図のように、自車進行方向をＹ、車軸方向をＸとした自車両基準ＸＹ座標系で停止物位置を定義した場合、時刻ｔ１、ｔ２で観測したある停止物の位置をそれぞれ（ｘ１，ｙ１）、（ｘ２，ｙ２）とする（本来レーダでは距離と角度で停止物位置が得られるが、座標変換をすれば、ｘｙ座標系の位置に変換することは容易であるため、その手順は省略する）。次に、時刻ｔ１における前記停止物の位置を原点とした座標系で時刻ｔ１における自車の位置ベクトルｚｏ１および時刻ｔ２における位置ベクトルｚｏ２は、次の式のようになる。
【数１２】
停止物基準座標変換部１４２では、このようにして自車位置を軌道推定部１４３へ出力する。
【００２２】
図５は、停止物基準座標系における自車軌道を示す図である。
軌道推定部１４３では、前記自車位置の時系列データを入力とし、協調回転モデル（ｃｏｏｒｄｉｎａｔｅｄ ｔｕｒｎ ｍｏｄｅｌ）などの旋回モデルに基づく拡張カルマンフィルタを用いて自車の軌道推定値を計算する。ここで、軌道推定値Ｘハットは、次の式のように位置（ｘ，ｙ）、速度（ｘドット，ｙドット）、ヨー角速度ωで構成される。
【数１３】
軌道推定部１４３は、軌道推定値と推定誤差共分散行列をバイアス補正部８４０へ出力する。
【００２３】
バイアス補正部８４０では、ヨー角速度観測値のバイアス誤差を自車軌道計算部１４０から入力されるヨー角速度によって計算し、バイアス補正した後のヨー角速度を出力する。
【００２４】
図６は、この実施の形態におけるバイアス補正部８４０の処理を示す処理フロー図である。
まず、ステップ１では、自車軌道計算部１４０から出力された軌道推定値と推定誤差共分散行列がバイアス補正部８４０へ入力される。また、ヨーレートセンサ８３０からヨー角速度観測値がバイアス補正部８４０へ入力される。この時、ヨー角速度観測値を追尾フィルタなどで平滑処理した後、そのヨー角速度観測値を入力してもよいものとする。ただし、自車軌道計算部１４０から常にヨー角速度推定値が出力されることは無いため、この処理フローのようにヨー角速度推定値が得られないサンプリング時刻では、後述のバイアス補正値メモリから保存したバイアス補正値を用いてヨー角速度観測値を補正する（ステップ２）。ここで、バイアス補正値の初期値は０とする。
次に、ステップ３では、自車軌道計算部１４０から出力された軌道推定値と推定誤差共分散行列を用いて、ヨー角速度観測値による旋回モデルに基づいた予測値と予測誤差共分散およびヨー角速度推定値による旋回モデルに基づいた予測値と予測誤差共分散を算出する。ただし、通常、レーダ８１０で停止物を検出するサンプリングレートと自車のヨーレートセンサ８３０からヨー角速度を検出する時刻は異なり、ヨーレートセンサ８３０のサンプリングレートのほうが細かいことから、ヨー角速度の観測時刻ｔとヨー角速度推定値の更新時刻ｔ’が異なる場合には時刻合わせをする必要がある。
【００２５】
図７は、時刻合わせを示す図である。
そのため、時刻ｔまでの時間外挿処理によって、時刻ｔにおける推定値と推定誤差共分散行列を算出する。
【数１４】
ここで、Ｆ（ωｅｓｔ（ｔ’），ｔ−ｔ’）はヨー角速度を時刻ｔ’におけるヨー角速度推定値ωｅｓｔとした場合の旋回モデルの時刻ｔ’から時刻ｔへの状態推移行列を意味する。また、Ｑ（ｔ−ｔ’）はシステム雑音の共分散行列である。
【００２６】
上記の推定値と推定誤差共分散を用いて、時刻ｔからΔＴ秒後のヨー角速度観測値による旋回モデルに基づいた予測値ＸハットＯＢＳ，ｔ＋ΔＴと予測誤差共分散ＰＯＢＳ，ｔ＋ΔＴを算出する。さらにヨー角速度推定値による旋回モデルに基づいたΔＴ秒後の予測値ＸハットＥＳＴ，ｔ＋ΔＴと予測誤差共分散ＰＥＳＴ，ｔ＋ΔＴを算出する。
【００２７】
そして、ステップ４では、カイ二乗検定による次の判定式を満たさない場合は、ヨー角速度観測値による予測値とヨー角速度推定値による予測値が異なるため、ヨー角速度観測値にはバイアス誤差が含まれていることとする。
【数１５】
ここで、εｔｈはカイ二乗分布表から求めることとする。
【００２８】
ここで、ヨー角速度観測値の観測レートはヨー角速度推定値の出力レート（または、レーダの検出レート）より細かいため、ヨー角速度観測値が得られる毎時刻で、判定式を用いた判定を行い、上記判定式をＮ回中Ｍ回満たさない場合は、以下のバイアス誤差推定処理を実施する。満たす場合は、ヨー角速度観測値にはバイアス誤差が含まれていないこととみなして、バイアス補正値メモリに保存してあるバイアス誤差を用いて補正する（ステップ２）。
次に、ステップ５では、上記ヨー角速度推定値とヨー角速度観測値を用いて、ヨー角速度観測値のバイアス誤差を推定する。ここでは、例えばカルマンフィルタなど用いてバイアス誤差を推定する。
バイアス補正値メモリにバイアス誤差推定値をバイアス補正値として保存する（ステップ６）。最後にバイアス補正値を用いて補正したヨー角速度が出力される（ステップ７）。
【００２９】
以上説明したヨーレートバイアス補正装置８００において、自車軌道計算部１４０は、自車に搭載されたレーダ８１０から得た停止物の位置を停止物基準の座標系へ変換することにより、停止物基準座標における自車位置を算出し、前記自車位置を用いて自車軌道を推定する。
【００３０】
自車軌道計算部１４０は、レーダ８１０から所定の距離以内に存在する停止物のみを抽出し、前記停止物が所定数以上得られる場合、自車軌道を推定する。
【００３１】
自車軌道計算部１４０は、旋回モデルに基づく拡張カルマンフィルタを用いて、停止物基準座標における自車位置より自車軌道を推定する。
【００３２】
バイアス補正部８４０は、ヨーレートセンサ８３０から得られるヨー角速度観測値および自車軌道計算部１４０で算出したヨー角速度推定値のそれぞれで時間外挿して算出した予測値の差分を予測誤差共分散行列を用いたカイ二乗判定によって、同一とみなされない場合はヨー角速度観測値に含まれるバイアス誤差推定を行う。
【００３３】
バイアス補正部８４０は、ヨー角速度観測値の観測時刻と自車軌道計算部１４０で算出したヨー角速度推定値の推定時刻が異なる場合、観測時刻と推定時刻の差を時間外挿することで、ヨー角速度観測値の観測時刻にヨー角速度推定値の推定時刻を合わせる。
【００３４】
このように、不要信号を含む停止物の中から時間方向に連続性のある確かな停止物を抽出し、その停止物と自車との位置を用いてヨー角速度を含んだ自車軌道を推定することで、正確にヨー角速度を算出することができる。さらに前記ヨー角速度推定値を用いて、ヨーレートセンサで検出されるヨー角速度観測値のバイアス誤差を補正することにより、高サンプリングレートで得られるヨー角速度観測値を利用し、正確な先行車判定をすることが可能となる。
【００３５】
実施の形態２．
実施の形態２について、図８〜図１０を用いて説明する。
なお、実施の形態１と共通する部分については、同一の符号を付し、説明を省略する。
【００３６】
図８は、この実施の形態におけるヨーレートバイアス補正装置８００の構成を示すブロック図である。
実施の形態１では、ある１つの停止物を用いてヨー角速度を補正していたが、実際には複数の別々の停止物（デリニエータであれば、複数のデリニエータなど）が観測されるため、この実施の形態２では複数の停止物を用いたヨーレートバイアス補正装置８００を示す。
【００３７】
自車軌道計算部１４０では、複数の停止物を入力として自車軌道を推定する。また、停止物用追尾部１２０から相関済み停止物を自車軌道計算部１４０へ入力する時刻は、停止物ごとに異なる。
【００３８】
図９は、複数停止物の相対位置の一例を示す図である。
図１０は、タイムシーケンスの説明図である。
【００３９】
例えば、図９のように、停止物１と停止物２の２つの停止物の位置が得られる場合、図１０のようなタイムシーケンスに従い、停止物１の追尾（図１０の停止物用追尾１に相当）と停止物２の追尾（図１０の停止物用追尾２に相当）の処理が行われる。
まず、図３のフローに従い、図１０のように停止物用追尾１から時刻ｔ４において相関済み停止物が自車軌道計算部１４０へ出力される。停止物用追尾２では時刻ｔ５において出力されるため、先に自車軌道計算部１４０へ出力した停止物１を基準とした停止物基準座標系で自車軌道計算手段の処理を行う。前記自車軌道計算部１４０の処理は実施の形態１と同様のため省略する。
時刻ｔ５より、停止物用追尾２から相関済み停止物が出力されるため、前記停止物を停止物１を基準とした座標系へ変換する。この時には時刻ｔ５における停止物１と停止物２の相対位置ベクトルを用いて座標変換すればよい（図９）。この相対位置ベクトルは、停止物用追尾部１２０で出力される停止物１推定位置と停止物２推定位置との相対位置とすればよい。
時刻ｔ６にて停止物１の停止物が欠落した場合でも、停止物２の停止物を用いて更新するため精度向上が見込まれる。
停止物１が時刻ｔ９でレーダ覆域外へ出る場合、それ以降は他の停止物を基準とした座標系で自車軌道計算部１４０の処理を継続することとする。時刻ｔ９以降の自車軌道計算部１４０による自車両の位置は停止物１を基準とした座標系から停止物２を基準とした座標系に変換する。
以降、他の停止物が検出される場合も同様の処理を繰り返すことで、停止物１つでは観測回数が少なく、自車軌道の推定精度が良くない場合でも複数の停止物を用いて、推定を継続することで推定精度を向上させる効果がある。
その他の処理は実施の形態１と同様のため省略する。
【００４０】
さらに図８を用いて、ヨーレートバイアス補正装置８００の入出力について説明する。
停止物用追尾からは相関済み停止物の位置（ドップラ速度が観測されれば、ドップラ速度も含める）、停止物番号、および停止物の推定値（位置と速度）を出力する。停止物基準座標変換では、各停止物位置（およびドップラ速度）の時系列データを各停止物の推定位置を用いて基準停止物（上記の例では、停止物１に相当）を原点とした座標系に変換する。軌道推定部では、順次入力された停止物位置を用いてヨー角度推定値を含む軌道を推定し、バイアス補正手段へ出力する。
【００４１】
以上説明したヨーレートバイアス補正装置８００において、停止物用追尾部１２０は、当該停止物の推定位置を停止物記憶部１３０へ出力し、前記自車軌道計算部１４０は、複数の停止物から得た位置の時系列データを用いて、ある基準となる停止物の座標系において複数の停止物から自車位置を算出し、前記自車位置を用いて自車軌道を推定する。
【００４２】
実施の形態３．
実施の形態３について、図１１〜図１２を用いて説明する。
なお、実施の形態１〜実施の形態２と共通する部分については、同一の符号を付し、説明を省略する。
【００４３】
図１１は、この実施の形態におけるヨーレートバイアス補正装置８００の構成を示すブロック図である。
自車軌道計算部１４０ａ〜１４０ｃでは、各停止物の軌道推定値と推定誤差分散行列を算出し、各停止物の推定値（停止物用追尾で算出した推定値）と共に軌道統合部１５０へ出力する。軌道統合部１５０では、各停止物の推定値を用いて座標を合わせ、軌道推定値を推定誤差共分散行列を用いて重み付け統合する。
【００４４】
図１２は、この実施の形態における軌道統合部１５０の動作を示す処理フロー図である。
【００４５】
ステップＰ１で軌道統合部１５０へ入力される軌道の数ＮＴＲＫが２以上である場合は、座標系の基準となる停止物を選択するステップＰ２へ進む。相関回数Ｎｃｏｒの多い軌道を基準停止物と選択し、ステップＰ３では他の軌道の位置を基準停止物の座標系へ変換する。
ステップＰ４の軌道を統合する方式としては、既存の航跡統合方式を用いても良いとする。例えば、有色性に考慮して共分散交差法を用いて統合をする。あるいは有色性は考慮していないが、最小二乗統合法を使うこともある。
【００４６】
演算負荷を抑えるために、次の式を用いて、あらかじめ定めたパラメータａ（０≦ａ≦１）で重み付け統合してもよい。
【数１６】
【００４７】
あるいは、次の式を用いて、航跡の更新状況に応じて重み付け統合してもよい。
【数１７】
ここで、停止物１と２からそれぞれ軌道ｘＴＲＫ１、ｘＴＲＫ２が得られたとする。また、ｘＳＹＳは統合後の軌道、ｎＴＲＫ１は停止物１の相関回数Ｎｃｏｒ、ｎＴＲＫ２は停止物２の相関回数Ｎｃｏｒに相当する。
【００４８】
その他の処理は実施の形態１もしくは２と同様のため省略する。
【００４９】
以上説明したヨーレートバイアス補正装置８００は、レーダ８１０と、車速センサ８２０と、ヨーレートセンサ８３０と、停止物識別部１１０と、停止物用追尾部１２０と、停止物記憶部１３０と、自車軌道計算部１４０と、軌道統合部１５０と、バイアス補正部８４０とを備える。
レーダ８１０は、車両前方の所定角度範囲内にレーダ波を照射し、物体に反射されたレーダ波を受信して、その物体の位置及び相対速度を検出する。
車速センサ８２０は、車両の速度を検出する。
ヨーレートセンサ８３０は、ヨー角速度を検出する。
停止物識別部１１０は、上記レーダ８１０により検出された物体の相対速度と上記車速センサ８２０により検出された車両の速度に基づいて当該物体が停止物体であるか否かを識別する。
停止物用追尾部１２０は、上記停止物識別部１１０により停止物体であると識別された複数の停止物の中から追尾フィルタによって時間方向に相関のある停止物を抽出し、当該停止物推定位置を算出する。
停止物記憶部１３０は、上記停止物用追尾部１２０から得られる当該停止物の推定位置および相関済み停止物を記憶する。
自車軌道計算部１４０ａ〜１４０ｃは、上記停止物記憶部１３０より得た相関済み停止物位置の時系列データを入力として、前記停止物位置を停止物基準の座標系へ変換することで自車位置の時系列データを得て、前記自車位置よりヨー角速度を含む自車軌道を推定する。
軌道統合部１５０は、上記自車軌道計算部１４０ａ〜１４０ｃのそれぞれより出力された複数の軌道推定値を重み付け統合して自車軌道を算出する。
バイアス補正部８４０は、ヨーレートセンサ８３０で検出するヨー角速度と前記自車軌道計算部１４０ａ〜１４０ｃより得たヨー角速度推定値を用いて、ヨー角速度のバイアス誤差を算出し、補正する。
【００５０】
軌道統合部１５０は、複数停止物の各軌道推定値の中から相関停止物の最も多い停止物を基準とした座標系において軌道推定値を統合して自車軌道を算出する。、
【００５１】
実施の形態４．
実施の形態４について、図１３を用いて説明する。
なお、実施の形態１〜実施の形態３と共通する部分については、同一の符号を付し、説明を省略する。
【００５２】
図１３は、この実施の形態における停止物用追尾部１２０の構成を示すブロック図である。
停止物用追尾部１２０は、更に、停止物をクラスタリングする停止物クラスタリング部を有する。相関部・平滑部・予測部・遅延部からなる追尾フィルタは、停止物クラスタリング部がクラスタリングしたクラスタを追尾する。
【００５３】
実際の道路走行環境下では視線誘導標（デリニエータ）、ガードレール、壁などの停止物からの反射がある。特にガードレールや壁などの表面積の広い停止物からは複数の反射点が得られ、反射点１点を追尾することが困難な場合がある。そのため、停止物用追尾部１２０の追尾フィルタの前段で停止物のクラスタリングを実施することで、表面積の大きい停止物から得られる複数反射点をまとめ、そのクラスタを追尾することで停止物用追尾手段の相関性能を高める。
【００５４】
その他の処理は実施の形態１〜３と同様のため省略する。
【００５５】
以上説明したヨーレートバイアス補正装置８００において、停止物用追尾部１２０は、停止物をクラスタリングし、クラスタを追尾する。
【００５６】
実施の形態５．
実施の形態５について、図１４〜図２５を用いて説明する。
なお、実施の形態１〜実施の形態４と共通する部分については、同一の符号を付し、説明を省略する。
【００５７】
図１４は、この実施の形態における角速度推定装置２００のハードウェア構成の一例を示すハードウェア構成図である。
角速度推定装置２００は、例えば、処理装置９１１と、記憶装置９１４と、入力装置９０２と、出力装置９０１とを有するコンピュータである。
処理装置９１１は、コンピュータプログラムを実行することにより、データを処理し、角速度推定装置２００全体を制御する。
記憶装置９１４は、処理装置９１１が実行するコンピュータプログラムや、処理装置９１１が処理するデータなどを記憶する。記憶装置９１４は、例えば、揮発性メモリ、不揮発性メモリ、フラッシュメモリ、磁気ディスク装置、光学ディスク装置などである。
入力装置９０２は、角速度推定装置２００の外部から信号や情報を入力し、処理装置９１１が処理できる形式のデータに変換する。入力装置９０２が変換したデータは、処理装置９１１が直接処理してもよいし、記憶装置９１４が一時的に記憶してもよい。入力装置９０２は、例えば、キーボードやマウスなど利用者の操作を入力する操作入力装置、アナログ信号をデジタルデータに変換するアナログデジタル変換装置などの変換装置、他の装置が送信した信号を受信する受信装置などのインターフェイス装置などがある。
出力装置９０１は、処理装置９１１が処理したデータや記憶装置９１４が記憶したデータを、角速度推定装置２００の外部に出力できる形式に変換して出力する。出力装置９０１は、例えば、画像を表示する表示装置やスピーカーなど人の五感で感知できる形式にデータを変換して出力する装置、デジタルデータをアナログ信号に変換するデジタルアナログ変換装置などの変換装置や、他の装置に対して信号を送信する送信装置などのインターフェイス装置など、他の装置に入力できる形式にデータを変換して出力する装置などがある。
【００５８】
角速度推定装置２００の機能ブロックは、記憶装置９１４が記憶したコンピュータプログラムを処理装置９１１が実行することにより実現される。なお、角速度推定装置２００の機能ブロックは、必ずしもコンピュータにより実現される構成でなくてもよく、デジタル回路やアナログ回路などの電子回路によって実現される構成であってもよいし、機械的構成など電気的構成以外の構成によって実現される構成であってもよい。
また、実施の形態１〜実施の形態４で説明したヨーレートバイアス補正装置８００の機能ブロックも、同様に、コンピュータにより実現される構成であってもよいし、その他の構成により実現される構成であってもよい。
【００５９】
図１５は、この実施の形態における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図である。
角速度推定装置２００は、自動車などの移動体に搭載されている。移動体には、角速度推定装置２００のほか、周辺物体観測装置８１１、移動速度観測装置８２１、角速度観測装置８３１などが搭載されている。
【００６０】
周辺物体観測装置８１１は、移動体の周辺に存在する物体の位置や速度などを観測する。周辺物体観測装置８１１は、例えば、レーダである。例えば、周辺物体観測装置８１１は、電波やレーザ光などの放射波を放射して物体に当たって反射した反射波を検知することにより、伝搬時間を測定し、物体までの距離を算出する。周辺物体観測装置８１１は、放射波を放射する方向もしくは反射波を受信する方向を走査するなどして、物体が存在する方向を判定する。周辺物体観測装置８１１が放射波を１回走査するのにかかる時間は、例えば０．２秒である。したがって、移動体と物体との間の相対位置関係が変化しない場合、周辺物体観測装置８１１は、同一の物体を、例えば０．２秒ごとに繰り返し観測する。周辺物体観測装置８１１は、算出した距離と、判定した方向とに基づいて、移動体に対する物体の相対位置を算出する。また、周辺物体観測装置８１１は、例えば、ドップラー効果による放射波と反射波との間の波長のずれを検出することにより、移動体に対する物体の相対速度を検出する。周辺物体観測装置８１１は、物体の相対位置や相対速度などの観測結果を表わす信号を出力する。
【００６１】
移動速度観測装置８２１は、移動体の移動速度を観測する。移動速度観測装置８２１は、例えば、自動車の車軸に取り付けた回転数計により車軸の回転数を測定し、移動速度を算出する。移動速度観測装置８２１は、例えば０．１秒の間の車軸の回転数を計数する。移動速度観測装置８２１は、計数した回転数にタイヤの一周の長さを乗じることにより、０．１秒の間に移動体が進んだ距離を算出する。移動速度観測装置８２１は、算出した距離を１０倍することにより、移動体の秒速を算出する。この例の場合、移動速度観測装置８２１は、０．１秒に１回ずつ繰り返し移動体の移動速度を観測する。移動速度観測装置８２１は、移動体の移動速度を観測した観測結果を表わす信号を出力する。
なお、移動速度観測装置８２１は、移動体の移動速度を算出せず、測定した車軸の回転数をそのまま表わす信号を出力する構成であってもよい。その場合、後述する速度観測値取得部２１２が、移動速度観測装置８２１に代わって、上述した計算を実行し、移動体の移動速度を算出する。
【００６２】
角速度観測装置８３１は、移動体の角速度を観測する。移動速度観測装置８２１は、例えば、ヨーレートセンサやジャイロセンサなどである。角速度観測装置８３１は、移動体の三次元方向（水平方向・前後方向・ねじれ方向）の角速度を観測する構成であってもよいし、水平方向の角速度のみを観測する構成であってもよい。角速度観測装置８３１もまた、移動体の角速度を繰り返し観測する。観測間隔は、例えば１０ミリ秒である。角速度観測装置８３１は、移動体の角速度を観測した観測結果を表わす信号を出力する。
【００６３】
角速度推定装置２００は、周辺物体観測装置８１１、移動速度観測装置８２１、角速度観測装置８３１などの観測結果に基づいて、移動体の角速度を推定する。角速度観測装置８３１が観測する角速度には、バイアス誤差やその他の誤差が含まれるからである。角速度推定装置２００は、周辺物体観測装置８１１、移動速度観測装置８２１、角速度観測装置８３１などの観測結果を総合することにより、より正確な角速度を推定する。なお、角速度推定装置２００は、移動体の角速度だけでなく、移動体の速度や移動方向、位置などを推定する構成であってよい。
また、角速度推定装置２００は、推定した角速度と、角速度観測装置８３１が観測した角速度とに基づいて、角速度観測装置８３１が観測した角速度の誤差を算出する構成であってもよい。角速度推定装置２００は、算出した角速度の誤差を記憶しておき、周辺物体観測装置８１１などの観測結果が得られない場合や、得られた観測結果の信頼性が低い場合に、記憶しておいた角速度の誤差を用いて、角速度観測装置８３１が観測した角速度を補正することにより、移動体の角速度を推定する構成であってもよい。
【００６４】
角速度推定装置２００は、相対位置取得部２１１と、速度観測値取得部２１２と、角速度観測値取得部２１３と、静止物体判定部２２０と、物体相関部２３０と、状態推定部２４０とを有する。
【００６５】
相対位置取得部２１１は、入力装置９０２を用いて、周辺物体観測装置８１１が出力した信号を入力し、周辺物体観測装置８１１が観測した観測結果を取得する。
相対位置取得部２１１は、処理装置９１１を用いて、周辺物体観測装置８１１が出力した信号を入力した時刻を判定し、周辺物体観測装置８１１が物体を観測した観測時刻を算出する。相対位置取得部２１１は、信号を入力した時刻をそのまま観測時刻とみなす構成であってもよいし、周辺物体観測装置８１１における遅延時間を考慮して、信号を入力した時刻よりも遅延時間だけ戻った時刻を観測時刻とする構成であってもよい。なお、周辺物体観測装置８１１が出力する信号に、周辺物体観測装置８１１が物体を観測した観測時刻を表わす情報が含まれる構成であってもよい。その場合、相対位置取得部２１１は、処理装置９１１を用いて、入力した信号から観測時刻を取得する。
相対位置取得部２１１は、記憶装置９１４を用いて、周辺物体観測装置８１１が物体を観測した観測時刻や、観測された物体の相対位置および相対速度を表わすデータを記憶する。相対位置取得部２１１が記憶するデータを「周辺物体観測データ」と呼ぶ。「周辺物体観測データ」のうち、観測時刻を表わすデータを「物体観測時刻データ」、物体の相対位置を表わすデータを「相対位置観測値データ」、物体の相対速度を表わすデータを「相対速度観測値データ」と呼ぶ。
【００６６】
速度観測値取得部２１２は、入力装置９０２を用いて、移動速度観測装置８２１が出力した信号を入力し、移動速度観測装置８２１が観測した観測結果を取得する。速度観測値取得部２１２は、処理装置９１１を用いて、相対位置取得部２１１と同様の処理をして、移動速度観測装置８２１が移動体の移動速度を観測した観測時刻を求める。速度観測値取得部２１２は、記憶装置９１４を用いて、移動速度観測装置８２１が移動体の移動速度を観測した観測時刻や、観測された移動体の移動速度を表わすデータを記憶する。速度観測値取得部２１２が記憶するデータを「移動速度データ」と呼ぶ。「移動速度データ」のうち、観測時刻を表わすデータを「移動速度観測時刻データ」、移動体の移動速度を表わすデータを「移動速度観測値データ」と呼ぶ。
【００６７】
角速度観測値取得部２１３は、入力装置９０２を用いて、角速度観測装置８３１が出力した信号を入力し、角速度観測装置８３１が観測した観測結果を取得する。角速度観測値取得部２１３は、処理装置９１１を用いて、相対位置取得部２１１や速度観測値取得部２１２と同様の処理をして、角速度観測装置８３１が移動体の角速度を観測した観測時刻を求める。角速度観測値取得部２１３は、記憶装置９１４を用いて、角速度観測装置８３１が移動体の角速度を観測した観測時刻や、観測された移動体の角速度を表わすデータを記憶する。角速度観測値取得部２１３が記憶するデータを「移動体角速度データ」と呼ぶ。「移動体角速度データ」のうち、観測時刻を表わすデータを「角速度観測時刻データ」、移動体の角速度を表わすデータを「角速度観測値データ」と呼ぶ。
【００６８】
静止物体判定部２２０は、処理装置９１１を用いて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。静止物体判定部２２０は、記憶装置９１４を用いて、判定した判定結果を表わすデータを記憶する。静止物体判定部２２０が記憶するデータを「静止判定結果データ」と呼ぶ。
例えば、静止物体判定部２２０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データと、速度観測値取得部２１２が記憶した移動速度データとを入力する。静止物体判定部２２０は、処理装置９１１を用いて、入力した周辺物体観測データから、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度を表わす相対速度観測値データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、入力した移動速度データから、移動体の移動速度を表わす移動速度観測値データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、相対速度観測値データが表わす相対速度と、移動速度観測値データが表わす移動速度とに基づいて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。
【００６９】
図１６は、移動体８０１の移動速度と、物体７０１，７０２の移動体８０１に対する相対速度との関係を説明するための図である。
矢印７１１，７１３は、それぞれ、移動体８０１，物体７０２の移動速度を表わす。物体７０１は、静止しているものとする。矢印７２１，７２２は、それぞれ、物体７０１，７０２の移動体８０１に対する相対速度を表わす。
物体７０１は静止しているので、移動体８０１を基準にすると、物体７０１は、移動体８０１の移動速度とちょうど逆向きの方向に、移動体８０１の移動速度とちょうど同じ速度で移動しているように見える。これに対して、物体７０２は移動しているので、移動体８０１を基準にすると、物体７０２は、移動体８０１の移動による見かけの速度（矢印７２３）と、物体７０２の移動による実際の速度（矢印７１３）とを合成した速度で移動しているように見える。したがって、物体の移動体に対する相対速度（見かけの速度）は、次の式で表わすことができる。
【数１８】
ただし、ｖｒは、物体の移動体に対する相対速度を表わすベクトルである。ｖｔは、物体の移動速度を表わすベクトルである。ｖｓは、移動体の移動速度を表わすベクトルである。
【００７０】
例えば、静止物体判定部２２０は、処理装置９１１を用いて、物体の移動体に対する相対速度を表わすベクトルｖｒと、移動体の移動速度を表わすベクトルｖｓとを加算して、物体の移動速度を表わすベクトルｖｔを算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出したベクトルｖｔの絶対値を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した絶対値を所定の閾値と比較する。閾値は、物体の移動体に対する相対速度や移動体の移動速度の観測誤差を考慮してあらかじめ設定しておく。ベクトルｖｔの絶対値が閾値より小さい場合、静止物体判定部２２０は、処理装置９１１を用いて、その物体が静止していると判定する。
【００７１】
なお、ドップラー効果を利用して物体の移動体に対する相対速度を観測する場合、観測される相対速度は、物体の見かけの速度（矢印７２１，７２２）のうち、物体と移動体との距離方向の成分（矢印７２５，７２６）だけであり、移動体８０１を軸とする回転方向の成分（矢印７２７，７２８）は観測されない。観測される相対速度は、次の式で表わすことができる。
【数１９】
ただし、ｖｄは、物体の移動体に対する相対速度の距離方向成分を表わす実数である。αは、物体の移動体に対する移動速度の方向と、物体から見た移動体が見える方向との間の角度を表わす実数である。
物体の移動体に対する相対速度のうち、移動体８０１を軸とする回転方向の成分（矢印７２７，７２８）が観測されないので、角度αは未知である。しかし、物体７０１は静止しているので、物体７０１の角度α（７３３）は、移動体８０１の移動方向と、移動体８０１から見た物体７０１が見える方向との間の角度７３１と一致する。これに対し、物体７０２は移動しているので、物体７０２の角度α（７３４）は、移動体８０１の移動方向と、移動体８０１から見た物体７０１が見える方向との間の角度７３２と必ずしも一致しない。
【００７２】
例えば、静止物体判定部２２０は、処理装置９１１を用いて、移動体の移動方向と、移動体から見た物体が見える方向との間の角度の余弦（コサイン）を算出する。静止物体判定部２２０は、処理装置９１１を用いて、移動体の移動速度を表わすベクトルｖｓの絶対値と、算出した余弦との積を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した積から、物体の移動体に対する相対速度の距離方向成分を表わす実数ｖｄを差し引いた差を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した差を、所定の閾値と比較する。閾値は、物体の移動体に対する相対速度や移動体の移動速度の観測誤差を考慮してあらかじめ設定しておく。差が閾値より小さい場合、静止物体判定部２２０は、処理装置９１１を用いて、その物体が静止していると判定する。
【００７３】
図１７は、物体の移動体に対する相対速度の観測時刻７８１〜７８３と、移動体の移動速度の観測時刻７９１〜７９９との関係を説明するための図である。
上述したように、周辺物体観測装置８１１が物体の移動体に対する相対位置や相対速度を観測する観測周期７７１と、移動速度観測装置８２１が移動体の移動速度を観測する観測周期７７２とは、異なっている。また、周辺物体観測装置８１１が物体の移動体に対する相対位置や相対速度を観測する観測時刻７８１〜７８３は、物体が存在する方向を走査した時刻であるから、物体が存在する方向が変われば、１回の観測周期のなかでのタイミングが変化し、不定期である。これに対し、移動速度観測装置８２１が移動体の移動速度を観測する観測時刻７９１〜７９９の間隔は、一定であり、観測周期７７２と等しい。
したがって、物体の移動体に対する相対速度の観測時刻７８１〜７８３と、移動体の移動速度の観測時刻７９１〜７９９とが一致する可能性はほとんどない。しかし、物体が静止しているか否かを静止物体判定部２２０が判定するために用いる物体の移動体に対する相対速度と、移動体の移動速度とは、同じ時刻に観測したものである必要がある。
【００７４】
車軸の回転数を計数することにより移動体の移動速度を観測する場合、観測される移動速度は、計数を開始した時刻から計数を終了した時刻までの移動速度の平均値である。例えば、時刻７９３に観測した移動速度は、時刻７９２から時刻７９３までの移動速度の平均値である。したがって、静止物体判定部２２０は、時刻７８１に観測した物体の移動体に対する相対速度を、時刻７９３に観測した移動体の移動速度と比較すればよい。
【００７５】
例えば、静止物体判定部２２０は、処理装置９１１を用いて、移動速度観測装置８２１の観測結果を速度観測値取得部２１２が取得するたびに処理を実行する。静止物体判定部２２０は、処理装置９１１を用いて、速度観測値取得部２１２が記憶した今回の移動速度データと、１つ前の移動速度データとを入力する。静止物体判定部２２０は、処理装置９１１を用いて、入力した２つの移動速度データから、移動速度観測装置８２１が移動体の移動速度を観測した今回の観測時刻と前回の観測時刻とをそれぞれ表わす２つの移動速度観測時刻データを取得する。
次に、静止物体判定部２２０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、未処理の周辺物体観測データを入力する。静止物体判定部２２０は、処理装置９１１を用いて、入力した周辺物体観測データから、周辺物体観測装置８１１が物体を観測した観測時刻を表わす物体観測時刻データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した物体観測時刻データが表わす観測時刻と、取得した２つの移動速度観測時刻データが表わす観測時刻とを比較する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した前回の観測時刻より前である場合は、移動速度観測装置８２１が前回観測した移動体の移動速度を使って、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。静止物体判定部２２０は、処理装置９１１を用いて、入力した１つ前の移動速度データから、移動速度観測装置８２１が観測した移動速度を表わす移動速度観測値データを取得する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した前回の観測時刻より後で、かつ、今回の観測時刻より前である場合は、移動速度観測装置８２１が今回観測した移動体の移動速度を使って、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。静止物体判定部２２０は、処理装置９１１を用いて、入力した今回の移動速度データから、移動速度観測装置８２１が観測した移動速度を表わす移動速度観測値データを取得する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した今回の観測時刻より後である場合、移動速度観測装置８２１が次回観測する移動体の移動速度を使って、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。移動体の移動速度の次回の観測値はまだ得られていないので、静止物体判定部２２０は、その周辺物体観測データについて今回は処理をしない。
【００７６】
例えばこのようにして、静止物体判定部２２０は、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定するために使用する移動体の移動速度を、移動速度観測装置８２１が観測した観測結果のなかから選択する。あるいは、静止物体判定部２２０は、移動速度観測装置８２１が観測した観測結果に基づいて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定するために使用する移動体の移動速度を算出する構成であってもよい。
【００７７】
例えば、静止物体判定部２２０は、処理装置９１１を用いて、速度観測値取得部２１２が記憶した移動速度データが表わす観測時刻と移動速度とに基づいて、平滑処理をする。周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも前である場合、静止物体判定部２２０は、処理装置９１１を用いて、平滑処理の結果に基づいて、周辺物体観測装置８１１が物体を観測した観測時刻における移動体の移動速度を推定する。あるいは、周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも前である場合、静止物体判定部２２０は、処理装置９１１を用いて、平滑処理の結果に基づいて、周辺物体観測装置８１１が物体を観測した観測時刻における移動体の移動速度を予測する。静止物体判定部２２０は、処理装置９１１を用いて、推定あるいは予測した移動体の移動速度を用いて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。
【００７８】
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が観測した物体のうち、静止物体判定部２２０が静止していると判定した物体について、相関処理をする。すなわち、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が物体を観測した観測結果のなかから、静止している同じ物体を複数回観測した観測結果を判定する。物体相関部２３０は、記憶装置９１４を用いて、判定した判定結果を表わすデータを記憶する。物体相関部２３０が記憶するデータを「相関結果データ」と呼ぶ。
例えば、物体相関部２３０は、処理装置９１１を用いて、静止物体判定部２２０が記憶した静止判定結果データを入力する。物体相関部２３０は、処理装置９１１を用いて、入力した静止判定結果データが表わす判定結果に基づいて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、静止物体判定部２２０が静止していると判定した物体についての周辺物体観測データであって、未処理の周辺物体観測データを入力する。物体相関部２３０は、処理装置９１１を用いて、入力した周辺物体観測データから、周辺物体観測装置８１１が物体を観測した観測時刻を表わす物体観測時刻データと、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置を表わす相対位置観測値データとを取得する。
処理済の周辺物体観測データから推定した物体の移動体に対する相対位置の軌跡がある場合、物体相関部２３０は、処理装置９１１を用いて、取得したデータが表わす観測時刻及び相対位置がその軌跡に適合するか否かを判定する。適合する軌跡が複数ある場合、物体相関部２３０は、処理装置９１１を用いて、取得したデータが表わす観測時刻及び相対位置が最もよく適合する軌跡を判定する。物体相関部２３０は、処理装置９１１を用いて、取得したデータが表わす観測時刻及び相対位置を、（最もよく）適合する軌跡に組み入れて、軌跡を更新する。
取得したデータが表わす観測時刻及び相対位置が適合する軌跡がない場合、物体相関部２３０は、処理装置９１１を用いて、適合する軌跡がない他の周辺物体観測データのなかに、新たな軌跡となり得る周辺物体観測データがあるか否かを判定する。新たな軌跡となり得る周辺物体観測データがある場合、物体相関部２３０は、処理装置９１１を用いて、新たな軌跡を生成する。
物体相関部２３０は、記憶装置９１４を用いて、生成あるいは更新した軌跡を表わすデータを記憶する。
【００７９】
なお、この時点では、移動体の位置や向きがまだわかっていないので、物体相関部２３０における相関処理は、物体の移動体に対する相対位置（見かけの位置）に基づいて行う。
【００８０】
図１８は、移動体８０１の移動軌跡７５１と、物体７０３〜７０８の移動体８０１に対する相対位置の軌跡７６１〜７６６との関係を説明するための図である。
例えば、移動体８０１が、移動軌跡７５１に示すように蛇行して移動したものとする。また、物体７０３〜７０８は、静止しているものとする。
【００８１】
時刻７８５において、周辺物体観測装置８１１は、物体７０３〜７０８について、移動体８０１に対する相対位置７４１ａ〜７４１ｆを観測する。
時刻７８６において、周辺物体観測装置８１１は、物体７０３〜７０８について、移動体８０１に対する相対位置７４２ａ〜７４２ｆを観測する。
時刻７８７において、周辺物体観測装置８１１は、物体７０３〜７０８について、移動体８０１に対する相対位置７４３ａ〜７４３ｆを観測する。
物体相関部２３０は、これらの観測結果に基づいて、相関処理を行い、軌跡７６１〜７６６を生成する。
【００８２】
このように、静止している物体７０３〜７０８の移動体８０１に対する相対位置は、移動体８０１の移動に伴って変化する。特に、移動体８０１が旋回した場合、物体７０３〜７０８が見える方向が大きく変化するので、物体７０３〜７０８の移動体８０１に対する相対位置も大きく変化する。
このため、静止している物体７０３〜７０８の移動体８０１に対する相対位置の軌跡７６１〜７６６は、このように複雑な形状になる場合がある。
【００８３】
周辺物体観測装置８１１が複数の静止している物体７０３〜７０８を観測した場合、それぞれの物体７０３〜７０８の移動体８０１に対する相対位置は、移動体８０１の移動や旋回に伴って大きく変化するが、物体７０３〜７０８間の位置関係は変わらない。物体７０３〜７０８の移動体８０１に対する相対位置は、移動体８０１の旋回に伴って方向が変化するだけでなく、移動体８０１の移動に伴って方向や距離が変化する。これに対して、物体７０３〜７０８は静止しているので、移動体８０１が移動や旋回をしても、物体７０３〜７０８間の距離は変わらず、移動体８０１の旋回に伴って物体７０３〜７０８間の方向が変化するだけである。
【００８４】
物体相関部２３０は、このことを利用し、処理装置９１１を用いて、周辺物体観測装置８１１が観測する物体の移動体８０１に対する相対位置を予測する。物体相関部２３０は、処理装置９１１を用いて、予測した結果を使って相関処理をする。
【００８５】
例えば、物体相関部２３０は、処理装置９１１を用いて、１回の走査周期内に周辺物体観測装置８１１が観測した複数の物体のなかから、静止している物体を１つ選択する。物体相関部２３０は、処理装置９１１を用いて、選択した物体の座標が原点に一致するよう、周辺物体観測装置８１１が今回の走査周期内に観測した複数の静止している物体の移動体８０１に対する相対位置の座標を平行移動する。また、物体相関部２３０は、処理装置９１１を用いて、前回の走査周期内に周辺物体観測装置８１１が観測した複数の物体のなかからも同様に、静止している物体を１つ選択する。物体相関部２３０は、処理装置９１１を用いて、選択した物体の座標が原点に一致するよう、周辺物体観測装置８１１が前回の走査周期内に観測した複数の静止している物体の移動体８０１に対する相対位置の座標を平行移動する。
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が今回の走査周期内に観測した物体の相対位置を平行移動した座標を、原点を中心に回転する。物体相関部２３０は、処理装置９１１を用いて、回転した座標それぞれと、周辺物体観測装置８１１が前回の走査周期内に観測した物体の相対位置を平行移動した座標それぞれとの間の距離を算出する。算出した距離が所定の閾値より小さい場合、物体相関部２３０は、処理装置９１１を用いて、その２つの座標に対応する物体が同一の物体であるとみなし、同一の物体であるとみなせる座標の組の数を数える。
物体相関部２３０は、処理装置９１１を用いて、回転する角度を変えてこれを繰り返し、同一の物体であるとみなせる座標の組の数が最も多い角度を求める。
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が今回の走査周期内に観測した複数の物体のなかから選択する物体と、周辺物体観測装置８１１が前回の走査周期内に観測した複数の物体のなかから選択する物体との組み合わせを変えてこれを繰り返し、同一の物体であるとみなせる座標の組の数が最も多い組み合わせと回転角度とを求める。
【００８６】
図１９は、この実施の形態における物体相関部２３０の動作を説明するための図である。
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が前回の走査周期内に観測した静止している物体の移動体８０１に対する相対位置７４１ａ〜７４１ｆのなかから、例えば相対位置７４１ｃを選択する。物体相関部２３０は、処理装置９１１を用いて、相対位置７４１ｃの座標が原点に一致するよう、相対位置７４１ａ〜７４１ｆの座標を平行移動する。
次に、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が今回の走査周期内に観測した静止している物体の移動体８０１に対する相対位置７４２ａ〜７４２ｆのなかから、例えば相対位置７４２ｃを選択する。物体相関部２３０は、処理装置９１１を用いて、相対位置７４２ｃの座標が原点に一致するよう、相対位置７４２ａ〜７４２ｆの座標を平行移動する。
物体相関部２３０は、処理装置９１１を用いて、平行移動した相対位置７４２ａ〜７４２ｆの座標を、原点を中心として回転する。物体相関部２３０は、処理装置９１１を用いて、回転した相対位置７４２ａ〜７４２ｆの座標と、平行移動した相対位置７４１ａ〜７４１ｆの座標とを比較して、同一の物体であるとみなせる座標の組の数を数える。
【００８７】
物体相関部２３０が選択した相対位置の組み合わせが、周辺物体観測装置８１１が実際に同一の物体を観測した相対位置の組であり、かつ、回転した角度が、移動体８０１が旋回した角度と一致する場合、同一の物体であるとみなせる座標の組の数が最も多くなる。
物体相関部２３０は、処理装置９１１を用いて、同一の物体であるとみなせる座標の組の数が最も多い相対位置の組み合わせを、周辺物体観測装置８１１が同一の物体を観測した相対位置であると判定する。また、そのときに同一の物体であるとみなされた座標の組に対応する相対位置の組み合わせについても、物体相関部２３０は、周辺物体観測装置８１１が同一の物体を観測した相対位置であると判定する。
【００８８】
このように、静止物体判定部２２０が静止していると判定した物体についてのみ、物体相関部２３０が相関処理をするので、移動体８０１の移動軌跡７５１がわからなくても、周辺物体観測装置８１１が観測した物体を相関追尾することができる。
【００８９】
なお、物体相関部２３０は、相対位置の組み合わせを総当たりですべて試してみる構成であってもよいし、移動速度観測装置８２１が観測した移動体８０１の移動速度などの情報を用いて、試してみる相対位置の組み合わせを絞り込む構成であってもよい。
また、物体相関部２３０は、平行移動した座標を回転する角度の範囲を限定せずに試してみる構成であってもよいし、角速度観測装置８３１が観測した移動体８０１の角速度などの情報を用いて、試してみる角度を絞り込む構成であってもよい。
【００９０】
また、物体相関部２３０は、同一の物体であるとみなせる座標の組の数を比較するのではなく、同一の物体であるとみなせる座標の組を頂点とする凸多角形の面積を比較して、面積が最も広い相対位置の組み合わせや回転角度を判定する構成であってもよい。同一の物体であるとみなせる座標の組が狭い範囲に集中している場合、１つの物体の細部である可能性があるからである。
【００９１】
状態推定部２４０（角速度推定部）は、処理装置９１１を用いて、移動体の位置、速度、移動方向、角速度や、移動速度観測装置８２１が観測した移動速度のゲイン誤差、角速度観測装置８３１が観測した角速度のバイアス誤差などの状態量を推定する。状態推定部２４０は、記憶装置９１４を用いて、推定した結果を表わすデータを記憶する。状態推定部２４０は、出力装置９０１を用いて、推定した結果を外部に出力する。
状態推定部２４０は、状態量の推定にあたり、同一の静止している物体を周辺物体観測装置８１１が観測した観測結果であると物体相関部２３０が判定した物体の移動体８０１に対する相対位置を利用する。
なお、１つの物体が複数の走査周期にわたって観測された場合において、その物体が観測された走査周期の数が多いほど、周辺物体観測装置８１１がその物体を観測した移動体８０１に対する相対位置を利用する価値が高い。このため、その物体が観測された走査周期の数が所定の閾値より多い場合のみ、周辺物体観測装置８１１がその物体を観測した移動体８０１に対する相対位置を利用する構成であってもよい。あるいは、その物体が連続して観測された走査周期の数が所定の閾値より多い場合のみ、周辺物体観測装置８１１がその物体を観測した移動体８０１に対する相対位置を利用する構成であってもよい。例えば、状態推定部２４０は、処理装置９１１を用いて、物体相関部２３０が判定した判定結果に基づいて、その物体が（連続して）観測された走査周期の数を算出し、算出した走査周期の数を閾値と比較して、その物体が（連続して）観測された走査周期の数が閾値より多い場合のみ、周辺物体観測装置８１１が観測したその物体の移動体８０１に対する相対位置を利用する。
【００９２】
周辺物体観測装置８１１が観測した物体の移動体に対する相対位置は、移動体の絶対位置を原点とし、移動体の移動方向を基準とした相対座標系における座標と考えることができる。
【００９３】
図２０は、移動体８０１を基準とした相対座標系と、静止点を基準とした絶対座標系との間の関係を説明するための図である。
Ｘ軸及びＹ軸は、移動体８０１を基準とした相対座標系における座標軸である。ｘ軸及びｙ軸は、絶対座標系における座標軸である。なお、絶対座標系の原点は、静止していればどこでもよい。
絶対座標系における移動体８０１の位置を（ｘｓ，ｙｓ）、移動体８０１の移動方向が絶対座標系のｙ軸となす角度をθとすると、相対座標系における座標（Ｘ，Ｙ）と、絶対座標系における座標（ｘ，ｙ）との間には、次の関係がある。
【数２０】
ただし、Ｒθは、角度θの回転変換を表わす行列である。
【００９４】
静止している物体の絶対座標系における座標を（ｘｔ，ｙｔ）、その物体を周辺物体観測装置８１１が観測した移動体に対する相対位置の座標を（ｘｒ，ｙｒ）とし、観測誤差を考慮しなければ、数２０の座標変換をそのまま当てはめることができるから、次の式が成り立つ。
【数２１】
【００９５】
状態推定部２４０は、例えば、拡張カルマンフィルタを用いて、次の式で表されるモデルを使って推定を行う。
【数２２】
ただし、ｘ１は、状態量を表わすベクトルである。ｘｔは、周辺物体観測装置８１１が観測した静止物体の絶対座標系におけるｘ座標である。ｙｔは、周辺物体観測装置８１１が観測した静止物体の絶対座標系におけるｙ座標である。ｘｓは、絶対座標系における移動体のｘ座標である。ｙｓは、絶対座標系における移動体のｙ座標である。ｖは、移動体の移動速度である。θは、移動体の移動方向である。ωは、移動体の角速度である。ｆ１は、状態遷移モデルを表わす関数である。Δｔは、経過時間である。ｚ１は、観測量を表わすベクトルである。ｘｒは、周辺物体観測装置８１１が観測した静止物体の移動体に対する相対位置のＸ座標である。ｙｒは、周辺物体観測装置８１１が観測した静止物体の移動体に対する相対位置のＹ座標である。ｈ１は、観測モデルを表わす関数である。
【００９６】
なお、状態推定部２４０は、拡張カルマンフィルタの観測値として、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の座標（ｘｒ，ｙｒ）だけでなく、移動速度観測装置８２１が観測した移動体の移動速度や角速度観測装置８３１が観測した移動体の角速度を使う構成であってもよい。その場合、状態推定部２４０は、例えば、次の式で表されるモデルを使って推定を行う。
【数２３】
ただし、ｘは、状態量を表わすベクトルである。εｖは、移動速度観測装置８２１が観測する移動体の移動速度のゲイン誤差である。εωは、角速度観測装置８３１が観測する移動体の角速度のバイアス誤差である。ｆは、状態遷移モデルを表わす関数である。ｚは、観測量を表わすベクトルである。ｖｏは、移動速度観測装置８２１が観測した移動体の移動速度である。ωｏは、角速度観測装置８３１が観測した移動体の角速度である。ｘ１、ｆ１、ｚ１、ｈ１は、数２２における定義にしたがう。
【００９７】
なお、周辺物体観測装置８１１が物体を観測する時刻と、移動速度観測装置８２１が移動体の移動速度を観測する時刻と、角速度観測装置８３１が移動体の角速度を観測する時刻とは、それぞれ異なるが、それぞれの観測装置が観測する観測値の誤差の間には相関がない。このため、例えば、それぞれの観測装置が観測する観測値ごとに観測関数を分け、状態推定部２４０は、いずれかの観測装置が観測をするたびに、更新処理をする。
【００９８】
なお、複数の物体の移動体に対する相対位置を、状態量の推定に利用する場合、状態推定部２４０は、複数の物体の絶対座標系における座標を含めるよう拡張した状態量ベクトル（ｘまたはｘ１）を使う構成であってもよい。
しかし、拡張カルマンフィルタで計算する行列の次数が高くなると、計算量が増えるので、処理装置９１１の処理能力が低い場合、リアルタイムでの計算が困難になる場合がある。
そこで、状態推定部２４０は、それぞれの物体について並行して拡張カルマンフィルタを実行する構成としてもよい。その場合、移動体の絶対座標や角速度などの状態量は、複数の拡張カルマンフィルタがそれぞれ推定するので、複数の推定値が得られる。例えば、状態推定部２４０は、処理装置９１１を用いて、複数の拡張カルマンフィルタにより推定された複数の推定値を平均して、移動体の絶対座標や角速度などの状態量の推定値とする。状態推定部２４０は、推定値を平均するにあたり、複数の拡張カルマンフィルタにより算出された複数の誤差共分散行列を用いて、推定精度に応じた重み付けをする構成であってもよい。
【００９９】
図２１は、この実施の形態における角速度推定処理Ｓ５００の流れの一例を示すフローチャート図である。
角速度推定処理Ｓ５００において、角速度推定装置２００は、移動体の角速度などを推定する。角速度推定処理Ｓ５００は、例えば、観測結果取得工程Ｓ５１０と、静止判定工程Ｓ５２０と、物体相関工程Ｓ５３０と、状態推定工程Ｓ５５０とを有する。
【０１００】
観測結果取得工程Ｓ５１０において、相対位置取得部２１１及び速度観測値取得部２１２及び角速度観測値取得部２１３は、それぞれ、周辺物体観測装置８１１及び移動速度観測装置８２１及び角速度観測装置８３１が観測した観測結果を取得する。
静止判定工程Ｓ５２０において、静止物体判定部２２０は、観測結果取得工程Ｓ５１０で取得した観測結果に基づいて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。
物体相関工程Ｓ５３０において、物体相関部２３０は、静止判定工程Ｓ５２０で静止物体判定部２２０が静止していると判定した物体について、相関処理をする。
状態推定工程Ｓ５５０において、状態推定部２４０は、観測結果取得工程Ｓ５１０で取得した観測結果や物体相関工程Ｓ５３０で物体相関部２３０が相関処理した結果に基づいて、移動体の角速度などを推定する。
角速度推定装置２００は、観測結果取得工程Ｓ５１０に処理を戻し、処理を繰り返す。
【０１０１】
図２２は、この実施の形態における観測結果取得工程Ｓ５１０の流れの一例を示すフローチャート図である。
観測結果取得工程Ｓ５１０は、例えば、相対位置取得工程Ｓ５１１と、移動速度取得工程Ｓ５１２と、角速度取得工程Ｓ５１３とを有する。
【０１０２】
相対位置取得工程Ｓ５１１において、相対位置取得部２１１は、処理装置９１１を用いて、周辺物体観測装置８１１が観測結果を表わす信号を出力しているか否かを判定する。周辺物体観測装置８１１が信号を出力していると判定した場合、相対位置取得部２１１は、入力装置９０２を用いて、周辺物体観測装置８１１が出力した信号を入力する。相対位置取得部２１１は、処理装置９１１を用いて、入力した信号が表わす観測結果を取得する。相対位置取得部２１１は、処理装置９１１を用いて、取得した観測結果を表わす周辺物体観測データを生成する。相対位置取得部２１１が生成する周辺物体観測データは、例えば、物体観測時刻データ、相対位置観測値データ、相対速度観測値データを含む。
物体観測時刻データは、周辺物体観測装置８１１がその物体を観測した時刻を表わす。例えば、物体観測時刻データは、所定の時刻からの経過時間をミリ秒単位で表わす整数値データである。あるいは、物体観測時刻データは、フレームの番号を表わす整数値データと、そのフレームの開始時刻からの経過時間をミリ秒単位で表わす整数値データとの組である。なお、１つのフレームは、周辺物体観測装置８１１による１回の走査周期である。１つのフレームの長さは、例えば０．２秒である。
相対位置観測値データは、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置を表わす。例えば、相対位置観測値データは、移動体を原点、移動体の進行方向をＹ軸、移動体の進行方向に対して直角右方向をＸ軸とする移動体基準の相対座標系において、周辺物体観測装置８１１が観測した物体の座標をメートル単位で表わす２つの実数値データの組である。あるいは、相対位置観測値データは、周辺物体観測装置８１１が観測した物体と移動体との間の距離をメートル単位で表わす実数値データと、移動体から見て周辺物体観測装置８１１が観測した物体が見える方向と移動体の進行方向とがなす角度をラジアン単位で表わす実数値データとの組である。
相対速度観測値データは、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度を表わす。例えば、相対速度観測値データは、移動体基準の相対座標系において、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度ベクトルの成分をメートル毎秒単位で表わす２つの実数値データの組である。あるいは、相対速度観測値データは、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度の物体と移動体との距離方向成分をメートル毎秒単位で表わす実数値データである。
相対位置取得部２１１は、記憶装置９１４を用いて、生成した周辺物体観測データを記憶する。
【０１０３】
移動速度取得工程Ｓ５１２において、速度観測値取得部２１２は、処理装置９１１を用いて、移動速度観測装置８２１が観測結果を表わす信号を出力しているか否かを判定する。移動速度観測装置８２１が信号を出力している場合、速度観測値取得部２１２は、入力装置９０２を用いて、移動速度観測装置８２１が出力した信号を入力する。速度観測値取得部２１２は、処理装置９１１を用いて、入力した信号が表わす観測結果を取得する。速度観測値取得部２１２は、処理装置９１１を用いて、取得した観測結果を表わす移動速度データを生成する。速度観測値取得部２１２が生成する移動速度データは、例えば、移動速度観測時刻データと、移動速度観測値データとを含む。
移動速度観測時刻データは、移動速度観測装置８２１が移動体の移動速度を観測した時刻を表わす。例えば、移動速度観測時刻データは、所定の時刻からの経過時間をミリ秒単位で表わす整数値データである。
移動速度観測値データは、移動速度観測装置８２１が観測した移動体の移動速度を表わす。例えば、移動速度観測値データは、移動速度観測装置８２１が観測した移動体の移動速度をメートル毎秒単位で表わす実数値データである。
速度観測値取得部２１２は、記憶装置９１４を用いて、生成した移動速度データを記憶する。
【０１０４】
角速度取得工程Ｓ５１３において、角速度観測値取得部２１３は、処理装置９１１を用いて、角速度観測装置８３１が観測結果を表わす信号を出力しているか否かを判定する。角速度観測装置８３１が信号を出力している場合、角速度観測値取得部２１３は、入力装置９０２を用いて、角速度観測装置８３１が出力した信号を入力する。角速度観測値取得部２１３は、処理装置９１１を用いて、入力した信号が表わす観測結果を取得する。角速度観測値取得部２１３は、処理装置９１１を用いて、取得した観測結果を表わす角速度データを生成する。角速度観測値取得部２１３が生成する角速度データは、例えば、角速度観測時刻データと、角速度観測値データとを含む。
角速度観測時刻データは、角速度観測値取得部２１３が移動体の角速度を観測した時刻を表わす。例えば、角速度観測時刻データは、所定の時刻からの経過時間をミリ秒単位で表わす整数値データである。
角速度観測値データは、角速度観測値取得部２１３が観測した移動体の角速度を表わす。例えば、角速度観測値データは、角速度観測装置８３１が観測した移動体の角速度をラジアン毎秒単位で表わす実数値データである。
角速度観測値取得部２１３は、記憶装置９１４を用いて、生成した角速度データを記憶する。
【０１０５】
なお、物体観測時刻データと、移動速度観測時刻データと、角速度観測時刻データとにおける観測時刻の表現形式は、相互に変換が可能であれば、異なる形式であってもよい。
【０１０６】
図２３は、この実施の形態における静止判定工程Ｓ５２０の流れの一例を示すフローチャート図である。
静止判定工程Ｓ５２０は、例えば、移動速度観測時刻取得工程Ｓ５２１と、物体選択工程Ｓ５２２と、観測時刻比較工程Ｓ５２３と、速度比較工程Ｓ５２４とを有する。
【０１０７】
移動速度観測時刻取得工程Ｓ５２１において、静止物体判定部２２０は、処理装置９１１を用いて、移動速度取得工程Ｓ５１２で速度観測値取得部２１２が記憶した移動速度データのうち、観測時刻が最も新しい移動速度データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した移動速度データに含まれる移動速度観測時刻データから、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻を取得する。また、静止物体判定部２２０は、処理装置９１１を用いて、取得した移動速度データに含まれる移動速度観測値データから、移動速度観測装置８２１が観測した移動体の移動速度を取得する。
なお、静止判定工程Ｓ５２０を前回実行した後に移動速度観測装置８２１が新しい移動速度を観測していない場合、静止物体判定部２２０は、以降の処理をせず、静止判定工程Ｓ５２０を終了する構成であってもよい。例えば、静止物体判定部２２０は、処理装置９１１を用いて、移動速度観測時刻取得工程Ｓ５２１を前回実行したときに取得した最新の観測時刻と、今回取得した最新の観測時刻とを比較する。観測時刻が同じ場合、静止物体判定部２２０は、静止判定工程Ｓ５２０を終了する。
【０１０８】
物体選択工程Ｓ５２２において、静止物体判定部２２０は、処理装置９１１を用いて、相対位置取得工程Ｓ５１１で相対位置取得部２１１が記憶した周辺物体観測データのなかから、その物体が静止しているか否かをまだ判定していない周辺物体観測データを選択する。
例えば、後述する速度比較工程Ｓ５２４において、静止物体判定部２２０は、記憶装置９１４を用いて、ある周辺物体観測データについて静止しているか否かを判定した場合に、その判定結果を表わす静止判定結果データを、その周辺物体観測データに対応づけて記憶しておく。
静止物体判定部２２０は、処理装置９１１を用いて、ある周辺物体観測データについて、対応づけられた静止判定結果データを記憶しているか否かを判定することにより、その周辺物体観測データについて静止しているか否かを判定済である否かを判定する。
選択すべき周辺物体観測データが存在しない場合、静止物体判定部２２０は、処理装置９１１を用いて、静止判定工程Ｓ５２０を終了する。
選択すべき周辺物体観測データが１つ以上存在する場合、静止物体判定部２２０は、処理装置９１１を用いて、そのなかから周辺物体観測データを１つ選択する。静止物体判定部２２０は、処理装置９１１を用いて、観測時刻比較工程Ｓ５２３へ処理を進める。
【０１０９】
観測時刻比較工程Ｓ５２３において、静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２で選択した周辺物体観測データに含まれる物体観測時刻データから、周辺物体観測装置８１１がその物体を観測した観測時刻を取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した観測時刻と、移動速度観測時刻取得工程Ｓ５２１で取得した観測時刻とを比較する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも後である場合、静止物体判定部２２０は、まだ、その周辺物体観測データについて静止しているか否かの判定をしない。静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２に処理を戻し、次の周辺物体観測データを選択する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも前である場合、静止物体判定部２２０は、その周辺物体観測データについて静止しているか否かの判定をする。静止物体判定部２２０は、処理装置９１１を用いて、速度比較工程Ｓ５２４へ処理を進める。
【０１１０】
速度比較工程Ｓ５２４において、静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２で選択した周辺物体観測データに含まれる相対速度データから、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度を取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した相対速度と、移動速度観測時刻取得工程Ｓ５２１で取得した移動体の移動速度とに基づいて、その物体が静止しているか否かを判定する。静止物体判定部２２０は、処理装置９１１を用いて、判定した判定結果を表わす静止判定データを生成する。静止物体判定部２２０は、記憶装置９１４を用いて、生成した静止判定データを、物体選択工程Ｓ５２２で選択した周辺物体観測データに対応づけて記憶する。
静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２に処理を戻し、次の周辺物体観測データを選択する。
【０１１１】
図２４は、この実施の形態における物体相関工程Ｓ５３０の流れの一例を示すフローチャート図である。
物体相関工程Ｓ５３０は、前回静止物体取得工程Ｓ５３１と、今回静止物体取得工程Ｓ５３２と、前回物体選択工程Ｓ５３３と、前回物体平行移動工程Ｓ５３４と、今回物体選択工程Ｓ５３５と、今回物体平行移動工程Ｓ５３６と、回転角度選択工程Ｓ５３７と、今回物体回転工程Ｓ５３８と、距離算出工程Ｓ５３９と、同一物体判定工程Ｓ５４０と、閾値判定工程Ｓ５４１と、同一物体記憶工程Ｓ５４２とを有する。
【０１１２】
前回静止物体取得工程Ｓ５３１において、物体相関部２３０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、１つ前のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データであって、その物体が静止していると静止判定工程Ｓ５２０で静止物体判定部２２０が判定した周辺物体観測データをすべて取得する。
物体相関部２３０は、処理装置９１１を用いて、取得した周辺物体観測データそれぞれについて、２つ以上前のフレームにおける周辺物体観測データとの間で、周辺物体観測装置８１１が同一の物体を観測したものであると物体相関部２３０が判定した周辺物体観測データがあるフレームの数を求める。
例えば、後述する同一物体記憶工程Ｓ５４２において、物体相関部２３０は、処理装置９１１を用いて、あるフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データが、それより前のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データと同一の物体を観測したものであると判定した場合、相関回数を表わす相関回数データを生成する。相関回数は、同一の物体と判定したフレームの数である。物体相関部２３０は、記憶装置９１４を用いて、その周辺物体観測データに対応づけて相関回数データを記憶する。同一の物体を観測したものであると判定した相手の周辺物体観測データに対応づけられた相関回数データがない場合、物体相関部２３０は、相関回数を「１」とした相関回数データを生成する。同一の物体を観測したものであると判定した相手の周辺物体観測データに対応づけられた相関回数データがある場合、物体相関部２３０は、その相関回数データが表わす相関回数に「１」を加えたものを相関回数とする相関回数データを生成する。
物体相関部２３０は、処理装置９１１を用いて、取得した周辺物体観測データに対応づけて記憶した相関回数データが表わす相関回数を取得することにより、同一の物体と判定したフレームの数を求める。
物体相関部２３０は、処理装置９１１を用いて、取得したすべての周辺物体観測データを、取得したフレームの数が多い順に並べる。相関の取れたフレームの数が多いほど、その周辺物体観測データがノイズなどによる誤検出である可能性が低く、また、移動している物体を誤って静止していると認識したものである可能性も低い。このため、今回のフレームにおける周辺物体観測データとも相関が取れる可能性が高い。あらかじめ相関が取れる可能性が高い順に周辺物体観測データを並べておくことにより、物体相関工程Ｓ５３０の計算量を抑えることができる。
【０１１３】
今回静止物体取得工程Ｓ５３２において、物体相関部２３０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、最新のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データであって、その物体が静止していると静止判定工程Ｓ５２０で静止物体判定部２２０が判定した周辺物体観測データをすべて取得する。
【０１１４】
前回物体選択工程Ｓ５３３において、物体相関部２３０は、処理装置９１１を用いて、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データのなかから、前回静止物体取得工程Ｓ５３１で並べだ順に、周辺物体観測データを１つ選択する。
前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データがすべて選択済であり、選択すべき周辺物体観測データが存在しない場合、物体相関部２３０は、処理装置９１１を用いて、同一物体記憶工程Ｓ５４２へ処理を進める。
【０１１５】
未選択の周辺物体観測データが存在する場合、物体相関部２３０は、処理装置９１１を用いて、未選択の周辺物体観測データを１つ選択する。物体相関部２３０は、処理装置９１１を用いて、選択した周辺物体観測データと同一の物体を今回のフレームで周辺物体観測装置８１１が観測する相対位置を予測する。例えば、物体相関部２３０は、処理装置９１１を用いて、１つ前のフレームまでの間に相関の取れた物体の移動体に対する相対位置の軌跡や、状態推定部２４０が推定した移動体の角速度などの状態量に基づいて、予測を行う。
【０１１６】
前回物体平行移動工程Ｓ５３４において、物体相関部２３０は、処理装置９１１を用いて、前回静止物体取得工程Ｓ５３１で取得したすべての周辺物体観測データについて、その周辺物体観測データに含まれる相対位置観測値データが表わす周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の座標から、今回静止物体取得工程Ｓ５３２で選択した周辺物体観測データに含まれる相対位置観測値データが表わすその物体の移動体に対する相対位置の座標を差し引くことにより、物体の座標を平行移動する。物体相関部２３０は、記憶装置９１４を用いて、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データそれぞれについて、平行移動した座標を表わすデータを記憶する。
【０１１７】
今回物体選択工程Ｓ５３５において、物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データを、その周辺物体観測データに含まれる相対位置観測値データが表わす周辺物体観測装置８１１が観測した物体の移動体に対する相対位置が、前回物体選択工程Ｓ５３３で予測した相対位置に近い順に並べる。予測した相対位置との距離が短いほうが同一の物体を観測した観測結果である可能性が高いからである。なお、物体相関部２３０は、予測した相対位置との距離が所定の閾値より短いものだけを抽出して、近い順に並べる構成であってもよい。
物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データ（もしくは、前回物体選択工程Ｓ５３３で予測した相対位置との距離が所定の閾値より短いものだけを抽出した周辺物体観測データ）のなかから、前回物体選択工程Ｓ５３３で予測した相対位置との距離が近い順に、周辺物体観測データを１つ選択する。
今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データ（もしくは、前回物体選択工程Ｓ５３３で予測した相対位置との距離が所定の閾値より短いものだけを抽出した周辺物体観測データ）がすべて選択済であり、選択すべき周辺物体観測データが存在しない場合、物体相関部２３０は、処理装置９１１を用いて、前回物体選択工程Ｓ５３３に処理を戻し、前回静止物体取得工程Ｓ５３１で選択した周辺物体観測データのなかから次の周辺物体観測データを選択する。
未選択の周辺物体観測データが存在する場合、物体相関部２３０は、処理装置９１１を用いて、未選択の周辺物体観測データを１つ選択し、今回物体平行移動工程Ｓ５３６へ処理を進める。
【０１１８】
今回物体平行移動工程Ｓ５３６において、物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得したすべての周辺物体観測データについて、その周辺物体観測データに含まれる相対位置観測値データが表わす周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の座標から、今回物体選択工程Ｓ５３５で選択した周辺物体観測データに含まれる相対位置観測値データが表わすその物体の移動体に対する相対位置の座標を差し引くことにより、物体の座標を平行移動する。物体相関部２３０は、記憶装置９１４を用いて、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データそれぞれについて、平行移動した座標を表わすデータを記憶する。
【０１１９】
回転角度選択工程Ｓ５３７において、物体相関部２３０は、前回物体選択工程Ｓ５３３で選択した周辺物体観測データと、今回物体選択工程Ｓ５３５で選択した周辺物体観測データとが、同一の物体を観測した観測結果を表わす周辺物体観測データであると仮定して、処理装置９１１を用いて、移動体の旋回角度を推定する。物体相関部２３０は、状態推定部２４０が推定した状態量や、角速度観測装置８３１が観測した角速度などを、旋回角度の推定に利用する構成であってもよい。物体相関部２３０は、処理装置９１１を用いて、推定した旋回角度に基づいて、試してみる複数の回転角度を決定する。
物体相関部２３０は、処理装置９１１を用いて、決定した複数の回転角度のなかから、推定した旋回角度に近い順に、回転角度を１つ選択する。
決定した複数の回転角度をすべて選択済であり、選択すべき回転角度が存在しない場合、物体相関部２３０は、処理装置９１１を用いて、今回物体選択工程Ｓ５３５に処理を戻し、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データのなかから次の周辺物体観測データを選択する。
未選択の回転角度が存在する場合、物体相関部２３０は、処理装置９１１を用いて、未選択の回転角度を１つ選択し、今回物体回転工程Ｓ５３８へ処理を進める。
【０１２０】
今回物体回転工程Ｓ５３８において、物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得したすべての周辺物体観測データについて今回物体平行移動工程Ｓ５３６で平行移動した座標を、原点を中心として、回転角度選択工程Ｓ５３７で選択した回転角度だけ回転移動する。
【０１２１】
距離算出工程Ｓ５３９において、物体相関部２３０は、処理装置９１１を用いて、前回静止物体取得工程Ｓ５３１で取得したすべての周辺物体観測データと、今回静止物体取得工程Ｓ５３２で取得したすべての周辺物体観測データとのすべての組み合わせについて、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データについて前回物体平行移動工程Ｓ５３４で平行移動した座標と、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データについて今回物体回転工程Ｓ５３８で回転移動した座標との間の距離を算出する。物体相関部２３０は、処理装置９１１を用いて、算出した距離が近い順に、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データと、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データとのペアを生成する。
【０１２２】
同一物体判定工程Ｓ５４０において、物体相関部２３０は、処理装置９１１を用いて、距離算出工程Ｓ５３９で生成したペアのうち、算出した距離が所定の閾値より短いペアの数を数える。物体相関部２３０は、処理装置９１１を用いて、数えたペア数を、物体相関工程Ｓ５３０を今回実行したなかで、それまでに同一物体判定工程Ｓ５４０で数えたペア数の最大値と比較する。
今回数えたペア数がそれまでのペア数の最大値より小さい場合、物体相関部２３０は、回転角度選択工程Ｓ５３７に処理を戻し、次の回転角度を選択する。
今回数えたペア数がそれまでのペア数の最大値より大きい場合、物体相関部２３０は、記憶装置９１４を用いて、距離算出工程Ｓ５３９で生成したペアのうち算出した距離が閾値より短いペアと、数えたペア数とを表わすデータを記憶する。物体相関部２３０は、処理装置９１１を用いて、閾値判定工程Ｓ５４１へ処理を進める。
【０１２３】
閾値判定工程Ｓ５４１において、物体相関部２３０は、処理装置９１１を用いて、距離算出工程Ｓ５３９で数えたペア数を所定の閾値と比較する。
数えたペア数が閾値より小さい場合、物体相関部２３０は、処理装置９１１を用いて、回転角度選択工程Ｓ５３７に処理を戻し、次の回転角度を選択する。
数えたペア数が閾値より大きい場合、物体相関部２３０は、処理装置９１１を用いて、同一物体記憶工程Ｓ５４２へ処理を進める。相関が取れる可能性が高い順に試行するので、早い段階で正しい組み合わせと回転角度が見つかる可能性が高く、それ以降の試行をしないことにより、計算量を抑えるためである。
なお、比較の対象となる閾値は、あらかじめ定めた定数であってもよい。また、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データの数や、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データの数に基づいて、物体相関部２３０が閾値を算出する構成であってもよい。例えば、物体相関部２３０は、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データの数と、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データの数とを比較して小さいほうの数を求め、求めた数に所定の定数（例えば０．６）を乗じて、閾値を算出する。
【０１２４】
同一物体記憶工程Ｓ５４２において、物体相関部２３０は、処理装置９１１を用いて、同一物体判定工程Ｓ５４０で記憶したデータが表わすペアが、同一の物体を周辺物体観測装置８１１が観測した観測結果を表わす周辺物体観測データであると判定する。物体相関部２３０は、処理装置９１１を用いて、判定した結果を表わす相関結果データとして、上述した相関回数データを生成する。また、物体相関部２３０は、相関結果データとして、処理装置９１１を用いて、どの周辺物体観測データが、同一の物体を周辺物体観測装置８１１が観測した観測結果を表わす周辺物体観測データであるかを表わすデータを生成する。例えば、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が観測した物体に番号を振る。物体相関部２３０は、記憶装置９１４を用いて、振った番号を表わすデータを、周辺物体観測データに対応づけて記憶する。物体相関部２３０は、１つ前のフレームにおける周辺物体観測データと同一の物体を周辺物体観測装置８１１が観測した観測結果を表わすと判定した周辺物体観測データには、１つ前のフレームにおけるその周辺物体観測データと同一の番号を振る。物体相関部２３０は、それ以外の周辺物体観測データには、いずれの周辺物体観測データとも異なる番号を振る。
【０１２５】
図２５は、この実施の形態における状態推定工程Ｓ５５０の流れの一例を示すフローチャート図である。
状態推定工程Ｓ５５０は、相関物体選択工程Ｓ５５１と、相関回数判定工程Ｓ５５２と、予測工程Ｓ５５３と、更新工程Ｓ５５４と、平均工程Ｓ５５５とを有する。
【０１２６】
相関物体選択工程Ｓ５５１において、状態推定部２４０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、最新のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データであって、それより前のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データと同一の物体を観測した観測結果を表わすと物体相関工程Ｓ５３０で物体相関部２３０が判定した周辺物体観測データを１つ選択する。例えば、状態推定部２４０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、対応づけられた相関回数データを物体相関部２３０が記憶した周辺物体観測データを選択する。
相関が取れた周辺物体観測データがすべて選択済であり、選択すべき周辺物体観測データが存在しない場合、状態推定部２４０は、処理装置９１１を用いて、平均工程Ｓ５５５へ処理を進める。
相関が取れた周辺物体観測データのなかに未選択の周辺物体観測データが存在する場合、状態推定部２４０は、処理装置９１１を用いて、未選択の周辺物体観測データを１つ選択し、相関回数判定工程Ｓ５５２へ処理を進める。
【０１２７】
相関回数判定工程Ｓ５５２において、状態推定部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１で選択した周辺物体観測データの相関回数を所定の閾値と比較する。例えば、状態推定部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１で選択した周辺物体観測データに対応づけて物体相関部２３０が記憶した相関回数データを取得する、状態推定部２４０は、処理装置９１１を用いて、取得した相関回数データが表わす相関回数を閾値と比較する。
相関回数が閾値より小さい場合、状態推定部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１に処理を戻し、次の周辺物体観測データを選択する。
相関回数が閾値より大きい場合、状態推定部２４０は、処理装置９１１を用いて、予測工程Ｓ５５３へ処理を進める。
【０１２８】
予測工程Ｓ５５３において、状態推定部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１で選択した周辺物体観測データが表わす物体の絶対位置を状態量に含め、状態遷移関数を使って、相関物体選択工程Ｓ５５１で選択した周辺物体観測データに含まれる物体観測時刻データが表わす観測時刻における状態量を予測する。
【０１２９】
更新工程Ｓ５５４において、状態推定部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１で選択した周辺物体観測データに含まれる物体の移動体に対する相対位置を観測量として、例えば拡張カルマンフィルタを使って、状態量の推定値を更新する。状態推定部２４０は、記憶装置９１４を用いて、更新した状態量の推定値や誤差共分散行列を表わすデータを記憶する。
【０１３０】
平均工程Ｓ５５５において、状態推定部２４０は、処理装置９１１を用いて、相関の取れた周辺物体観測データそれぞれについて更新工程Ｓ５５４で推定した状態量の推定値を平均して、全体としての状態量の推定値を算出する。
【０１３１】
実施の形態６．
実施の形態６について、図２６を用いて説明する。
なお、実施の形態５と共通する部分については、同一の符号を付し、説明を省略する。
【０１３２】
この実施の形態における角速度推定装置２００のハードウェア構成や機能ブロックの構成は、実施の形態５と同様なので、ここでは説明を省略する。
【０１３３】
状態推定部２４０は、物体相関部２３０が相関処理をした物体の移動体に対する相対位置を観測値として入力するのではなく、物体相関部２３０が相関処理をする際に算出した平行移動量や回転角度を観測値として入力して、移動体の角速度などの状態量を推定する。
【０１３４】
図２６は、物体相関部２３０が相関処理に用いる平行移動量および回転角度と、移動体８０１の移動量の関係を説明するための図である。
物体相関部２３０は、あるフレームにおいて周辺物体観測装置８１１が観測した静止物体の移動体８０１に対する相対位置７４１ａ〜７４１ｆと、別のフレームにおいて周辺物体観測装置８１１が観測した静止物体の移動体８０１に対する相対位置７４２ａ〜７４２ｆとの相関を取ったとする。物体相関部２３０は、ベクトル７５５を用いて相対位置７４１ａ〜７４１ｆの座標を平行移動して、相対位置７４１ｃの座標を原点に合わせる。また、物体相関部２３０は、ベクトル７５６を用いて相対位置７４２ａ〜７４２ｆの座標を平行移動して、相対位置７４２ｃの座標を原点に合わせ、回転角度７５７だけ回転移動することにより、相関が取れたとする。
【０１３５】
この場合、ベクトル７５６を回転角度７５７だけ回転したベクトル７５８から、ベクトル７５５を差し引いた差ベクトル７５９が、その２つのフレームの間における移動体基準の相対座標系における移動体８０１の移動量である。
また、回転角度７５７は、その２つのフレームの間における移動体８０１の旋回角度である。
【０１３６】
例えば、物体相関部２３０は、図２４で説明した同一物体判定工程Ｓ５４０で、今回数えたペア数がそれまでのペア数の最大値より大きい場合、記憶装置９１４を用いて、前回物体選択工程Ｓ５３３で選択した周辺物体観測データと、今回物体選択工程Ｓ５３５で選択した周辺物体観測データと、回転角度選択工程Ｓ５３７で選択した回転角度を表わすデータとを記憶する。
状態推定部２４０は、物体相関部２３０が記憶した２つの周辺物体観測データと、回転角度を表わすデータとに基づいて、移動体の移動量および旋回角度の観測値を算出する。例えば、状態推定部２４０は、処理装置９１１を用いて、今回物体選択工程Ｓ５３５で物体相関部２３０が選択した周辺物体観測データから、最新のフレームにおいて周辺物体観測装置８１１が観測したその物体の移動体に対する相対位置を取得する。状態推定部２４０は、処理装置９１１を用いて、取得した相対位置を表わす座標を、原点を中心として、回転角度選択工程Ｓ５３７で物体相関部２３０が選択した回転角度だけ回転移動する。状態推定部２４０は、処理装置９１１を用いて、前回物体選択工程Ｓ５３３で物体相関部２３０が選択した周辺物体観測データから、１つ前のフレームにおいて周辺物体観測装置８１１が観測したその物体の移動体に対する相対位置を取得する。状態推定部２４０は、処理装置９１１を用いて、取得した相対位置を表わす座標から、回転移動した座標を差し引いた差を算出して、移動体の移動量の観測値とする。また、状態推定部２４０は、処理装置９１１を用いて、回転角度選択工程Ｓ５３７で物体相関部２３０が選択した回転角度を、移動体の旋回角度の観測値とする。
【０１３７】
状態推定部２４０は、処理装置９１１を用いて、このようにして算出した移動体の移動量および旋回角度の観測値を、例えば拡張カルマンフィルタの入力として推定値の更新処理を行う。その場合、状態推定部２４０は、例えば、次の式で表されるモデルを使って推定を行う。
【数２４】
ただし、ｘ２は、状態量を表わすベクトルである。ｆ２は、状態遷移モデルを表わす関数である。ｚ２は、観測量を表わすベクトルである。Δｘは、状態推定部２４０が算出した移動体の移動量の観測値の横方向成分である。Δｙは、状態推定部２４０が算出した移動体の移動量の観測値の縦方向成分である。Δθは、状態推定部２４０が算出した移動体の旋回角度の観測値である。ｈ２は、観測モデルを表わす関数である。
【０１３８】
ここで、状態量を表わすベクトルｘ２は、静止物体の絶対座標系における座標を含まない。したがって、状態推定部２４０は、静止物体ごとに推定をする必要はなく、全体で１つの推定を行えばよい。
【０１３９】
なお、状態推定部２４０は、数２３のｘ１、ｆ１、ｚ１、ｈ１を、数２４のｘ２、ｆ２、ｚ２、ｈ２で置き換えたモデルを使って推定をする構成であってもよい。
【０１４０】
実施の形態７．
実施の形態７について、図２７を用いて説明する。
なお、実施の形態５〜実施の形態６と共通する部分については、同一の符号を付し、説明を省略する。
【０１４１】
図２７は、この実施の形態における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図である。
【０１４２】
周辺物体観測装置８１１は、物体の移動体に対する相対速度を観測せず、物体の移動体に対する相対位置のみを観測する。
実施の形態５では、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度と、移動速度観測装置８２１が観測した移動体の移動速度とに基づいて、静止物体判定部２２０は、物体が静止しているか否かを判定する。これに対し、この実施の形態の周辺物体観測装置８１１は物体の移動体に対する相対速度を観測しないので、静止物体判定部２２０は、処理装置９１１を用いて、物体相関部２３０が相関を取った結果として得られる物体の移動体に対する相対位置の軌跡に基づいて物体の移動体に対する相対速度を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した物体の移動体に対する相対速度と、移動速度観測装置８２１が観測した移動体の移動速度とに基づいて、物体が静止しているか否かを判定する。
【０１４３】
また、物体が静止しているか否かを静止物体判定部２２０が判定する前に、物体相関部２３０が相関処理をする必要があるので、物体相関部２３０は、少なくとも初期の段階において、静止物体判定部２２０の判定結果を利用することができない。このため、物体相関部２３０は、処理装置９１１を用いて、とりあえず、周辺物体観測装置８１１が観測したすべての物体について、相関処理を実施する。なお、物体相関部２３０による相関処理の結果に基づいて、物体が静止しているか否かを静止物体判定部２２０が判定したのち、静止していないと静止物体判定部２２０が判定した物体について、物体相関部２３０は、相関処理を打ち切る構成であってもよい。
物体相関部２３０は、静止しているか否か不明である物体について相関処理を実施するため、状態推定部２４０が推定した移動体の位置や向きを利用する。例えば、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が物体を観測した観測時刻における移動体の位置や向きに基づいて、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の座標を、絶対座標系における座標に変換する。物体相関部２３０は、処理装置９１１を用いて、変換した座標に基づいて、物体の相関処理をする。
【０１４４】
これにより、周辺物体観測装置８１１が物体の移動体に対する相対速度を観測しない構成である場合でも、移動体の角速度などを推定することができる。
【０１４５】
実施の形態８．
実施の形態８について、図２８を用いて説明する。
なお、実施の形態１〜実施の形態７と共通する部分については、同一の符号を付し、説明を省略する。
【０１４６】
図２８は、この実施の形態における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図である。
角速度推定装置２００は、実施の形態６で説明した機能ブロックに加えて、更に、角速度誤差記憶部２５１と、角速度補正部２５２と、角速度検定部２５３と、角速度誤差算出部２５４とを有する。
【０１４７】
状態推定部２４０は、角速度観測装置８３１が観測した移動体の角速度を使わず、それ以外の観測値に基づいて、状態量を推定する。
【０１４８】
角速度誤差記憶部２５１は、記憶装置９１４を用いて、バイアス誤差など角速度観測装置８３１が観測する角速度の誤差を表わすデータを記憶する。角速度誤差記憶部２５１が記憶するデータを「角速度誤差データ」と呼ぶ。最初の段階では、角速度観測装置８３１が観測する角速度の誤差は不明であるから、例えば、誤差がないものと仮定し、角速度誤差記憶部２５１は、記憶装置９１４を用いて、「０」を表わすデータを、角速度誤差データとして記憶しておく構成であってもよい。
【０１４９】
角速度補正部２５２は、処理装置９１１を用いて、角速度観測装置８３１が観測した移動体の角速度の誤差を補正する。角速度補正部２５２は、記憶装置９１４を用いて、算出した角速度を表わすデータを記憶する。角速度補正部２５２が記憶するデータを「補正済角速度データ」と呼ぶ。
例えば、角速度補正部２５２は、処理装置９１１を用いて、角速度観測値取得部２１３が記憶した移動体角速度データと、角速度誤差記憶部２５１が記憶した角速度誤差データとを入力する。角速度補正部２５２は、処理装置９１１を用いて、入力した移動体角速度データから、角速度観測値データを取得する。角速度補正部２５２は、処理装置９１１を用いて、取得した角速度観測値データが表わす角速度から、入力した角速度誤差データが表わす誤差を差し引いた差を計算して、補正した角速度を算出する。
【０１５０】
角速度検定部２５３は、処理装置９１１を用いて、角速度補正部２５２が補正した角速度が正しいか否かを検定する。角速度検定部２５３は、記憶装置９１４を用いて、検定した結果を表わすデータを記憶する。角速度検定部２５３が記憶するデータを「角速度検定結果データ」と呼ぶ。
例えば、角速度検定部２５３は、処理装置９１１を用いて、状態推定部２４０が推定した状態量に基づいて、角速度補正部２５２が補正した角速度が正しいか否かを検定する。
【０１５１】
角速度誤差算出部２５４は、処理装置９１１を用いて、角速度観測装置８３１が観測した移動体の角速度の誤差を算出する。角速度誤差算出部２５４は、記憶装置９１４を用いて、算出した誤差を表わすデータを記憶する。角速度誤差算出部２５４が記憶するデータを「角速度誤差推定データ」と呼ぶ。
例えば、角速度誤差算出部２５４は、処理装置９１１を用いて、状態推定部２４０が推定した状態量を表わすデータと、角速度観測値取得部２１３が記憶した移動体角速度データとを入力する。角速度誤差算出部２５４は、処理装置９１１を用いて、入力したデータから、状態推定部２４０が推定した移動体の角速度を表わすデータを取得する。角速度誤差算出部２５４は、処理装置９１１を用いて、入力した移動体角速度データから、角速度観測値データを取得する。角速度誤差算出部２５４は、処理装置９１１を用いて、取得したデータが表わす移動体の角速度の推定値から、取得した角速度観測値データが表わす角速度の観測値を差し引いた差を計算して、角速度の誤差を算出する。
【０１５２】
角速度補正部２５２が補正した角速度が正しくないと角速度検定部２５３が判定した場合、角速度誤差記憶部２５１は、記憶した角速度誤差データを更新し、角速度誤差算出部２５４が記憶した角速度誤差推定データを、新たな角速度誤差データとして、記憶装置９１４を用いて記憶する。
【０１５３】
次に、角速度検定部２５３による検定処理について説明する。
【０１５４】
例えば、角速度検定部２５３は、状態推定部２４０が推定した状態量を使って、物体の位置を予測する。このとき、移動体の角速度だけは、状態推定部２４０が推定した状態量ではなく、角速度補正部２５２が補正した角速度を使う。これにより予測精度が落ちた場合、角速度検定部２５３は、角速度補正部２５２が補正した角速度が正しくないと判定する。
【０１５５】
状態推定部２４０が推定した角速度を、角速度補正部２５２が補正した角速度で置き換えるためには、状態推定部２４０が推定した状態量の時刻と、角速度観測装置８３１が移動体の角速度を観測した時刻とが一致する必要がある。状態推定部２４０は、周辺物体観測装置８１１が物体を観測した場合や、移動速度観測装置８２１が移動体の移動速度を観測した場合に、状態量の推定値を更新する。したがって、周辺物体観測装置８１１が物体を観測した時刻や、移動速度観測装置８２１が移動体の移動速度を観測した時刻が、状態推定部２４０が推定した状態量の時刻になる。
ここで、角速度観測装置８３１が移動体の角速度を観測した時刻をｔ１とする。周辺物体観測装置８１１が物体を観測した時刻または移動速度観測装置８２１が移動体の移動速度を観測した時刻をｔ２とする。なお、時刻ｔ２よりも時刻ｔ１のほうが、後の時刻であるものとする。
【０１５６】
角速度検定部２５３は、例えば、時刻ｔ２における状態量の推定値に基づいて外挿処理を行い、時刻ｔ１における状態量の推定値を算出する。
例えば、角速度検定部２５３は、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔ１における状態量の推定値を算出する。
【数２５】
ただし、山型付きのｘ（ｘハット。以下「ｘ＾」と表記する。）は、推定値ベクトルである。推定値ベクトルｘ＾は、ｋ次の列ベクトルである。推定値ベクトルｘ＾は、時刻ｔの関数である。推定値ベクトルｘ＾の要素は、時刻ｔにおける状態量の推定値である。推定値ベクトルｘ＾の次数ｋは、推定値の数である。
Ｆは、状態遷移行列である。状態遷移行列Ｆは、ｋ次の正方行列である。状態遷移行列Ｆは、時間Δｔの関数である。状態遷移行列Ｆは、所定の運動モデルにおいて、ある時刻における移動体の位置などを表わすベクトルを、時間Δｔが経過した後における移動体の位置などを表わすベクトルへ写す写像を表わす。
すなわち、この式は、時刻ｔ２における推定値ベクトルｘ＾（ｔ２）に、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）を作用させて、時刻ｔ１における推定値ベクトルｘ＾（ｔ１）を算出することを意味している。
なお、状態遷移行列Ｆの運動モデルは、例えば、移動体がある角速度で運動していると仮定する旋回運動モデルなどである。
【０１５７】
角速度検定部２５３は、算出した時刻ｔ１における状態量の推定値に基づいて、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルにしたがい、時刻ｔ１から所定の時間ΔＴが経過した時刻ｔ３（すなわち、ｔ３＝ｔ１＋ΔＴ）における状態量を予測する。
例えば、角速度検定部２５３は、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔ３における状態量の予測値を算出する。
【数２６】
ただし、ｘ＾Ｊは、角速度補正部２５２が補正した時刻ｔ１における角速度で、状態推定部２４０が推定した角速度を置き換えた推定値ベクトルである。ＦＪは、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルに基づく状態遷移行列である。
すなわち、この式は、時刻ｔ１における推定値ベクトルｘ＾Ｊ（ｔ１）に、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪを作用させて、時刻ｔ３における推定値ベクトルｘ＾Ｊ（ｔ３）を算出することを意味している。
【０１５８】
また、角速度検定部２５３は、状態推定部２４０が算出した誤差分散や誤差共分散に基づいて、予測した状態量の誤差分散や誤差共分散を推定する。
【０１５９】
状態量の推定値と同様、誤差分散などの時刻と、補正した角速度の時刻とを一致させるため、角速度検定部２５３は、例えば、時刻ｔ２における誤差分散などに基づいて、時刻ｔ１における誤差分散などを算出する。
例えば、角速度検定部２５３は、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔ１における誤差分散などを算出する。
【数２７】
ただし、Ｐは、推定値ベクトルｘ＾の誤差の分散共分散行列である。分散共分散行列Ｐは、ｋ次の正方行列である。分散共分散行列Ｐは、時刻ｔの関数である。分散共分散行列Ｐの要素は、時刻ｔにおける推定値ベクトルの各要素の誤差の間の分散または共分散である。
上付きのＴは、行列の転置を表わす。
Ｑは、システム雑音の分散共分散行列である。分散共分散行列Ｑは、ｋ次の正方行列である。分散共分散行列Ｑは、時間Δｔの関数である。分散共分散行列Ｑの要素は、時間Δｔが経過する間に発生するシステム雑音の分散または共分散である。
すなわち、この式は、時刻ｔ２における推定誤差の分散共分散行列Ｐ（ｔ２）に、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）の転置行列を左から作用させ、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）を右から作用させたものに、時間（ｔ１−ｔ２）の経過により発生したシステム雑音の分散共分散行列Ｑ（ｔ１−ｔ２）を加えて、時刻ｔ１における推定誤差の分散共分散行列Ｐ（ｔ１）を算出することを意味している。
【０１６０】
角速度検定部２５３は、算出した時刻ｔ１における誤差分散などに基づいて、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルにしたがい、時刻ｔ３における誤差分散などを算出する。
例えば、角速度検定部２５３は、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔ３における誤差分散などを算出する。
【数２８】
ただし、ＰＪは、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルに基づく予測の誤差の分散共分散行列である。
この式は、時刻ｔ１における推定誤差の分散共分散行列Ｐ（ｔ１）に、移動体が角速度補正部２５２が補正した時刻ｔ１における角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪ（ΔＴ）の転置行列を左から作用させ、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪ（ΔＴ）を右から作用させたものに、時間ΔＴの経過により発生したシステム雑音の分散共分散行列Ｑ（ΔＴ）を加えて、時刻ｔ３についての予測誤差の分散共分散行列ＰＪ（ｔ３）を算出することを意味している。
【０１６１】
角速度検定部２５３は、例えば上記のようにして、時刻ｔ３における状態量や誤差分散などを予測する。
なお、角速度検定部２５３は、角速度補正部２５２が補正した角速度ではなく、角速度観測値取得部２１３が記憶した移動体角速度データが表わす角速度観測装置８３１が観測した補正前の角速度に基づいて、状態量や誤差分散などを予測する構成であってもよい。
【０１６２】
また、角速度検定部２５３は、角速度を置き換えずに、状態推定部２４０が推定した移動体の角速度をそのまま使って、状態量や誤差分散などを予測する。
【０１６３】
角速度検定部２５３は、例えば、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔ３における状態量や誤差分散などを算出する。
【数２９】
ただし、ｘ＾Ｔは、状態推定部２４０が推定した移動体の角速度をそのまま使った時刻ｔ２における推定値ベクトルである。ＦＴは、状態推定部２４０が推定した時刻ｔ２における角速度で移動体が旋回運動をしているという運動モデルに基づく状態遷移行列である。ＰＴは、状態推定部２４０が推定した時刻ｔ２における角速度で移動体が旋回運動をしているという運動モデルに基づく予測の誤差の分散共分散行列である。
【０１６４】
角速度検定部２５３は、例えば上記のようにして、状態量や誤差分散などを予測する。
【０１６５】
角速度検定部２５３は、予測した２種類の状態量や誤差分散などに基づいて、角速度補正部２５２が補正した角速度が正しいか否かを判定する。
【０１６６】
角速度検定部２５３は、例えば、予測した２種類の状態量の間の差が、予想される範囲内であるか否かを判定し、予想される範囲を超えている場合に、角速度補正部２５２が補正した角速度が正しくないと判定する。
この判定には、統計学上の手法を用いることができる。角速度検定部２５３は、例えばカイ二乗検定により、２種類の状態量の間の差が、予想される範囲内であるか否かを判定する。例えば、角速度検定部２５３は、次の式の右辺を計算することにより、検定値を算出する。
【数３０】
ただし、εは、検定値である。
角速度検定部２５３は、処理装置９１１を用いて、算出した検定値εを、所定の閾値εｔｈと比較する。閾値εｔｈは、所定の有意水準に基づいて、例えばカイ二乗分布表から求めたものである。検定値εが閾値εｔｈ以下である場合、角速度検定部２５３は、２種類の状態量の間の差が、予想される範囲内であると判定する。検定値εが閾値εｔｈより大きい場合、角速度検定部２５３５は、２種類の状態量の間の差が、予想される範囲を超えていると判定する。
なお、角速度検定部２５３は、２種類の状態量を比較するのではなく、角速度補正部２５２が補正した角速度に基づいて予測した状態量に基づいて観測値を予測し、予測した観測値と実際の観測値とを比較することにより、角速度補正部２５２が補正した角速度が正しいか否かを判定する構成であってもよい。
【０１６７】
以上、各実施の形態で説明した構成は、一例であり、異なる実施の形態で説明した構成を組み合わせた構成としてもよい。また、主要でない部分の構成を既存の技術など他の構成と置き換えるなどの変形をした構成としてもよい。
【０１６８】
以上説明した角速度推定装置（２００；ヨーレートバイアス補正装置８００）は、相対位置取得部（２１１；停止物識別部１１０）と、静止物体判定部（２２０；停止物識別部１１０）と、物体相関部（２３０；停止物用追尾部１２０）と、角速度推定部（状態推定部２４０；軌道推定部１４３；軌道統合部１５０）とを有する。
上記相対位置取得部（２１１；１１０）は、移動体（８０１）の周辺に存在する物体について、上記移動体（８０１）を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置（８１１；レーダ８１０）が観測した観測結果を取得する。
上記静止物体判定部（２２０；１１０）は、上記相対位置取得部（２１１；１１０）が取得した観測結果に基づいて、上記周辺物体観測装置（８１１；８１０）が相対位置を観測した物体が静止しているか否かを判定する。
上記物体相関部（２３０；１２０）は、上記相対位置取得部（２１１）が取得した観測結果に基づいて、上記周辺物体観測装置（８１１；８１０）が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置（８１１；８１０）が観測した複数の相対位置を判定する。
上記角速度推定部（２４０；１４３；１５０）は、上記相対位置取得部（２１１）が取得した観測結果と、上記静止物体判定部（２２０；１１０）が判定した判定結果と、上記物体相関部（２３０；１２０）が判定した判定結果とに基づいて、上記移動体（８０１）の角速度を推定する。
【０１６９】
これにより、ＧＰＳなど移動体の位置を観測する観測装置がなくても、移動体の角速度を推定することができる。また、推定した角速度を用いることにより、ジャイロセンサなど移動体の角速度を観測する装置の誤差を推定することができる。
【０１７０】
角速度推定装置（２００；８００）は、相対位置算出部（停止物基準座標変換部１４２）を有する。
上記相対位置算出部（１４２）は、上記相対位置取得部（停止物識別部１１０）が取得した観測結果と、上記静止物体判定部（１１０）が判定した判定結果と、上記物体相関部（停止物用追尾部１２０）が判定した判定結果とに基づいて、静止している物体を基準とした上記移動体（８０１）の相対位置を算出する。
上記角速度推定部（１４３；１５０）は、上記相対位置算出部（１４２）が算出した相対位置に基づいて、上記移動体（８０１）の角速度を推定する。
【０１７１】
静止していると判定した物体との相対位置関係から、移動体自身の動きを推定して、移動体の角速度を推定するので、ＧＰＳなどの観測装置がなくても、移動体の角速度を推定することができる。
【０１７２】
角速度推定装置（２００；８００）は、角速度観測値取得部（２１３）と、角速度観測誤差算出部（状態推定部２４０；バイアス補正部８４０）とを有する。
上記角速度観測値取得部（２１３）は、上記移動体（８０１）の角速度を観測する角速度観測装置（角速度観測装置８３１；８３０）が観測した観測結果を取得する。
上記角速度観測誤差算出部（２４０；８４０）は、上記角速度推定部（２４０；１４３；１５０）が推定した推定結果と、上記角速度観測値取得部（２１３）が取得した観測結果とに基づいて、上記角速度観測装置（８３１；８３０）が観測した角速度の誤差を算出する。
【０１７３】
角速度観測装置の誤差を算出することにより、周辺物体観測装置からの入力がない場合でも、角速度観測装置が観測した移動体の角速度を補正することができる。
【０１７４】
角速度推定装置（２００；８００）は、速度観測値取得部（２１２；停止物識別部１１０）を有する。
上記速度観測値取得部（２１２；１１０）は、上記移動体（８０１）の移動速度を観測する移動速度観測装置（８２１；車速センサ８２０）が観測した観測結果を取得する。
上記静止物体判定部（２２０；１１０）は、上記相対位置取得部（２１１；１１０）が取得した観測結果と、上記速度観測値取得部（２１２；１１０）が取得した観測結果とに基づいて、上記物体が静止しているか否かを判定する。
【０１７５】
物体が静止しているか否かを判定することにより、静止している物体を基準として、移動体の動きを把握することができる。
【０１７６】
上記周辺物体観測装置（８１１；８１０）は、上記移動体（８０１）を基準とした上記物体の相対速度を観測する。
上記静止物体判定部（２２０；１１０）は、上記移動速度観測装置（８２１；８２０）が観測した上記移動体（８０１）の移動速度と、上記周辺物体観測装置（８１１；８１０）が観測した上記物体の相対速度とに基づいて、上記物体が静止しているか否かを判定する。
【０１７７】
移動体の移動速度と、物体の移動体に対する相対速度とに基づいて、物体が静止しているか否かを判定するので、移動体の旋回角度や物体の軌跡などを用いることなく、物体が静止しているか否かを判定することができる。
【０１７８】
角速度推定装置（２００；８００）は、相対速度推定部（静止物体判定部２２０）を有する。
上記相対速度推定部（２２０）は、上記物体相関部（２３０）が判定した判定結果に基づいて、上記周辺物体観測装置（８１１）が複数回観測した物体について、上記移動体（８０１）に対する上記物体の相対速度を推定する。
上記静止物体判定部（２２０）は、上記移動速度観測装置（８２１）が観測した上記移動体（８０１）の移動速度と、上記相対速度推定部（２２０）が推定した上記物体の相対速度とに基づいて、上記物体が静止しているか否かを判定する。
【０１７９】
物体相関部の判定結果に基づいて、物体の移動体に対する相対速度を算出するので、物体の移動体に対する相対速度を観測する観測装置がなくても、物体が静止しているか否かを判定することができる。
【０１８０】
角速度推定装置（２００；８００）は、コンピュータを角速度推定装置として機能させるコンピュータプログラムをコンピュータが実行することにより、実現することができる。
【符号の説明】
【０１８１】
１１０ 停止物識別部、１２０ 停止物用追尾部、１２１ 相関部、１２２ 平滑部、１２３ 予測部、１２４ 遅延部、１３０ 停止物記憶部、１４０ 自車軌道計算部、１４１ 相関停止物選択部、１４２ 停止物基準座標変換部、１４３ 軌道推定部、１５０ 軌道統合部、２００ 角速度推定装置、２１１ 相対位置取得部、２１２ 速度観測値取得部、２１３ 角速度観測値取得部、２２０ 静止物体判定部、２３０ 物体相関部、２４０ 状態推定部、２５１ 角速度誤差記憶部、２５２ 角速度補正部、２５３ 角速度検定部、２５４ 角速度誤差算出部、８００ ヨーレートバイアス補正装置、８１０ レーダ、８１１ 周辺物体観測装置、８２０ 車速センサ、８２１ 移動速度観測装置、８３０ ヨーレートセンサ、８３１ 角速度観測装置、８４０ バイアス補正部、９０１ 出力装置、９０２ 入力装置、９１１ 処理装置、９１４ 記憶装置。
【技術分野】
【０００１】
この発明は、移動体の角速度を推定する角速度推定装置に関する。
【背景技術】
【０００２】
ジャイロセンサなど移動体の角速度を観測する角速度観測装置には、バイアス誤差などの誤差がある。全地球測位システム（ＧＰＳ）などの位置観測装置を用いて、移動体の位置を観測し、観測結果から移動体の角速度を推定する方式がある。また、位置観測装置の観測結果から推定した角速度を用いて、角速度観測装置が観測した角速度を補正する方式がある。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平６−１４８３１９号公報
【特許文献２】特開平９−４９８７５号公報
【特許文献３】特開２００７−３３３３８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ＧＰＳのように外部のシステムに頼って移動体の位置を観測する位置観測装置は、電波状況などによっては使えない場合がある。
この発明は、例えば上記のような課題を解決するためになされたものであり、ＧＰＳのような位置観測装置に頼ることなく、移動体の角速度を精度よく推定することを目的とする。
【課題を解決するための手段】
【０００５】
この発明にかかる角速度推定装置は、
相対位置取得部と、静止物体判定部と、物体相関部と、角速度推定部とを有し、
上記相対位置取得部は、移動体の周辺に存在する物体について、上記移動体を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置が観測した観測結果を取得し、
上記静止物体判定部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が相対位置を観測した物体が静止しているか否かを判定し、
上記物体相関部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置が観測した複数の相対位置を判定し、
上記角速度推定部は、上記相対位置取得部が取得した観測結果と、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、上記移動体の角速度を推定することを特徴とする。
【発明の効果】
【０００６】
この発明にかかる角速度推定装置によれば、ＧＰＳのような位置観測装置がなくても、移動体の角速度を精度よく推定することができる。
【図面の簡単な説明】
【０００７】
【図１】実施の形態１におけるヨーレートバイアス補正装置８００の構成を示すブロック図。
【図２】時刻ｔ１〜ｔＮまで時間方向に連続して相関のとれた停止物の様子を表わす図。
【図３】実施の形態１における相関停止物選択部１４１の処理を示す処理フロー図。
【図４】停止物基準座標系への座標変換を示す図。
【図５】停止物基準座標系における自車軌道を示す図。
【図６】実施の形態１におけるバイアス補正部８４０の処理を示す処理フロー図。
【図７】時刻合わせを示す図。
【図８】実施の形態２におけるヨーレートバイアス補正装置８００の構成を示すブロック図。
【図９】複数停止物の相対位置の一例を示す図。
【図１０】タイムシーケンスの説明図。
【図１１】実施の形態３におけるヨーレートバイアス補正装置８００の構成を示すブロック図。
【図１２】実施の形態３における軌道統合部１５０の動作を示す処理フロー図。
【図１３】実施の形態４における停止物用追尾部１２０の構成を示すブロック図。
【図１４】実施の形態５における角速度推定装置２００のハードウェア構成の一例を示すハードウェア構成図。
【図１５】実施の形態５における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図。
【図１６】移動体８０１の移動速度と、物体７０１，７０２の移動体８０１に対する相対速度との関係を説明するための図。
【図１７】物体の移動体に対する相対速度の観測時刻７８１〜７８３と、移動体の移動速度の観測時刻７９１〜７９９との関係を説明するための図。
【図１８】移動体８０１の移動軌跡７５１と、物体７０３〜７０８の移動体８０１に対する相対位置の軌跡７６１〜７６６との関係を説明するための図。
【図１９】実施の形態５における物体相関部２３０の動作を説明するための図。
【図２０】移動体８０１を基準とした相対座標系と、静止点を基準とした絶対座標系との間の関係を説明するための図。
【図２１】実施の形態５における角速度推定処理Ｓ５００の流れの一例を示すフローチャート図。
【図２２】実施の形態５における観測結果取得工程Ｓ５１０の流れの一例を示すフローチャート図。
【図２３】実施の形態５における静止判定工程Ｓ５２０の流れの一例を示すフローチャート図。
【図２４】実施の形態５における物体相関工程Ｓ５３０の流れの一例を示すフローチャート図。
【図２５】実施の形態５における状態推定工程Ｓ５５０の流れの一例を示すフローチャート図。
【図２６】物体相関部２３０が相関処理に用いる平行移動量および回転角度と、移動体８０１の移動量の関係を説明するための図。
【図２７】実施の形態７における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図。
【図２８】実施の形態８における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図。
【発明を実施するための形態】
【０００８】
車両に搭載されているレーダ装置を用いて、車両前方を監視するに際して、車両前方の物体を検出する装置について、説明する。
【０００９】
車両に搭載し、光波・ミリ波などの送信波を車両前方の所定角度に照射して、その反射波を受信することによって、自車両の前方を走行する車両、自車両の前方の障害物を認識する、車両用前方監視装置がある。車両用前方監視装置においては、検出した物体の中から、直線路走行時、カーブ路走行時に依らず、自車両の進行方向に存在する車両を先行車両として認識する必要がある。このとき、自車進行方向に存在する先行車両を認識する方式として、車速とヨーレート（角速度）センサを用いて道路曲率を算出し、先行車両を認識する。
ただし、道路曲率を算出するために用いるヨーレートセンサには電圧オフセット分の零点バイアス誤差が発生し、さらに温度や時間変化に伴い零点バイアス誤差がドリフトするため、そのまま使用すると先行車判定を誤る可能性が高い。そのため、センサの零点バイアス補正を行う必要がある。
【００１０】
自車のヨーレート（以下、ヨー角速度）を算出するために、自車両のレーダで検出した複数の停止物の前時刻における角度と現時刻における角度の相関をとる方式がある。相関方式としては、前時刻で検出された停止物の角度をΔθだけずらし、現時刻で検出された停止物の角度と最も相関がとれるΔθを角度変化量とみなして、ヨー角速度を算出する方式がある。しかし、実際の道路走行環境において停止物は時間方向に必ずしも連続して検出できるとは限らない。例えば、途中で失検出、再出現、もしくはマルチパス波による不要信号の混入などが起こるため、上記方法ではヨー角速度を正確に推定できない。
【００１１】
また、自車両の停止状態（速度が０の場合）を検出するために、検出物体との相対速度が０となる条件を用いて、低速時のヨー角速度をヨー角速度零点バイアス値とみなして、補正する方式がある。ただし、自車が低速時でなければバイアス補正できない。
【００１２】
また、ＧＰＳで得た自車の絶対位置とレーダで検出した停止物との相対位置より、ヨー角速度を含む自車両の運動を算出する方式がある。この方式では、自車の運動とは無関係の停止物も含めて自車軌道を計算することから、ＧＰＳで得られる絶対位置なしでヨー角速度を正確に算出することはできない。
【００１３】
このように、ヨーレートセンサには電圧オフセット分の零点バイアス誤差が発生し、さらに温度や時間変化に伴い零点バイアス誤差がドリフトするため、そのまま使用すると先行車判定を誤る可能性が高い。
このため、複数の停止物の中から追尾フィルタによって時間方向に相関のある停止物を抽出し、その停止物と自車両の相対位置の時系列データより自車の位置と運動（本文中では軌道と呼ぶ）に基づくヨー角速度推定値を算出する。さらにヨー角速度観測値と推定値との差よりバイアス誤差を計算し、ヨー角速度を補正する。
【００１４】
実施の形態１．
実施の形態１について、図１〜図７を用いて説明する。
【００１５】
図１は、この実施の形態におけるヨーレートバイアス補正装置８００の構成を示すブロック図である。
ヨーレートバイアス補正装置８００は、レーダ８１０と、車速センサ８２０と、ヨーレートセンサ８３０と、停止物識別部１１０と、停止物用追尾部１２０と、停止物記憶部１３０と、自車軌道計算部１４０と、バイアス補正部８４０とを備える。
レーダ８１０（周辺物体観測装置）は、車両前方の所定角度範囲内にレーダ波を照射し、物体に反射されたレーダ波を受信して、その物体の位置及び相対速度を検出する。
車速センサ８２０（移動速度観測装置）は、車両の速度を検出する。
ヨーレートセンサ８３０（角速度観測装置）は、ヨー角速度を検出する。
停止物識別部１１０（相対位置取得部、速度観測値取得部、静止物体判定部）は、上記レーダ８１０により検出された物体の相対速度と上記車速センサ８２０により検出された車両の速度に基づいて当該物体が停止物体であるか否かを識別する。
停止物用追尾部１２０（物体相関部）は、上記停止物識別部１１０により停止物体であると識別された複数の停止物の中から追尾フィルタによって時間方向に相関のある停止物を抽出する。停止物用追尾部１２０は、例えば、相関部１２１と、平滑部１２２と、予測部１２３と、遅延部１２４とを有する。
停止物記憶部１３０は、上記停止物用追尾部１２０で相関済みの停止物を記憶する。
自車軌道計算部１４０（角速度推定部）は、上記停止物記憶部１３０より得た停止物位置を入力として、前記停止物位置を停止物基準の座標系へ変換することで自車位置を得て、前記自車位置よりヨー角速度を含む自車軌道を推定する。自車軌道計算部１４０は、例えば、相関停止物選択部１４１と、停止物基準座標変換部１４２と、軌道推定部１４３とを有する。
バイアス補正部８４０は、ヨーレートセンサ８３０で検出するヨー角速度と前記自車軌道計算部１４０より得たヨー角速度推定値を用いて、ヨー角速度のバイアス誤差を算出し、補正する。
【００１６】
図２は、時刻ｔ１〜ｔＮまで時間方向に連続して相関のとれた停止物の様子を表わす図である。
停止物を追尾すると、自車両の動きに応じて移動することがわかる。
【００１７】
停止物用追尾部１２０では、この図のように追尾フィルタ（カルマンフィルタやα−β（−γ）フィルタなどの公知のフィルタ）によって複数の停止物の中から時間方向に相関のとれる有効な停止物を抽出する。この図の白丸は、時間方向に相関がとれた同一停止物、斜線付き丸は、前記停止物（白丸）の最新時刻の位置、黒丸は時間方向に連続性がなく、停止物用追尾部１２０で相関のとれない停止物を意味する。停止物用追尾部１２０からは、相関のとれた停止物の位置（ドップラ速度があればドップラ速度も含める）と当該停止物番号（同一停止物を表す番号）を出力する。
停止物記憶部１３０では、停止物番号とその相関済み停止物の時系列データを記録し、記録された相関停止物の時系列データと当該停止物番号を相関停止物選択部１４１へ出力する。
相関停止物選択部１４１では、同一停止物ごとの相関停止物時系列データを入力とし、前記相関停止物時系列データの中から、あらかじめ設定した距離Ｒｍｉｎ（例えば３０ｍ）以内に入った相関停止物のみを抽出し、相関回数Ｎｃｏｒをカウントする。さらに前記相関回数Ｎｃｏｒが相関回数閾値Ｎｍｉｎ以上の場合は、前記相関停止物の位置を出力する。
【００１８】
図３は、この実施の形態における相関停止物選択部１４１の処理を示す処理フロー図である。
ステップ１において、相関停止物選択部１４１は、距離閾値以内に入る相関停止物を抽出する。
ステップ２において、相関停止物選択部１４１は、相関停止物数が閾値以上であるか否かを判定する。
相関停止物数が閾値以上である場合、ステップ３において、相関停止物選択部１４１は、上記相関停止物を出力する。
【００１９】
ここで、相関回数閾値Ｎｍｉｎは、自車速ＶＭ、レーダ観測レートＴ、距離Ｒｍｉｎを用いて次の式のように上限を定める。
【数１１】
例えば、Ｒｍｉｎ＝３０［ｍ］、ＶＭ＝１６［ｍ／ｓ］、Ｔ＝０．２［ｓ］とすると、レーダで９回程度観測は可能であるが、それ以上は観測されない。そのため、数１１の条件を満たす閾値Ｎｍｉｎを設定することで、以降の処理を確実に実行する。
【００２０】
停止物基準座標変換部１４２では、レーダ位置を原点とした座標系からある時刻の停止物を原点とした座標系へ変換する。本来、停止物の位置はレーダを原点とした座標系においてレーダとの相対位置で得られる。そこで、停止物のある時刻の位置を座標系の原点として、自車両の位置を座標変換することで、停止物から見た自車の位置を用いて、自車両の軌道を算出する。以降、例を用いて説明する。
【００２１】
図４は、停止物基準座標系への座標変換を示す図である。
この図のように、自車進行方向をＹ、車軸方向をＸとした自車両基準ＸＹ座標系で停止物位置を定義した場合、時刻ｔ１、ｔ２で観測したある停止物の位置をそれぞれ（ｘ１，ｙ１）、（ｘ２，ｙ２）とする（本来レーダでは距離と角度で停止物位置が得られるが、座標変換をすれば、ｘｙ座標系の位置に変換することは容易であるため、その手順は省略する）。次に、時刻ｔ１における前記停止物の位置を原点とした座標系で時刻ｔ１における自車の位置ベクトルｚｏ１および時刻ｔ２における位置ベクトルｚｏ２は、次の式のようになる。
【数１２】
停止物基準座標変換部１４２では、このようにして自車位置を軌道推定部１４３へ出力する。
【００２２】
図５は、停止物基準座標系における自車軌道を示す図である。
軌道推定部１４３では、前記自車位置の時系列データを入力とし、協調回転モデル（ｃｏｏｒｄｉｎａｔｅｄ ｔｕｒｎ ｍｏｄｅｌ）などの旋回モデルに基づく拡張カルマンフィルタを用いて自車の軌道推定値を計算する。ここで、軌道推定値Ｘハットは、次の式のように位置（ｘ，ｙ）、速度（ｘドット，ｙドット）、ヨー角速度ωで構成される。
【数１３】
軌道推定部１４３は、軌道推定値と推定誤差共分散行列をバイアス補正部８４０へ出力する。
【００２３】
バイアス補正部８４０では、ヨー角速度観測値のバイアス誤差を自車軌道計算部１４０から入力されるヨー角速度によって計算し、バイアス補正した後のヨー角速度を出力する。
【００２４】
図６は、この実施の形態におけるバイアス補正部８４０の処理を示す処理フロー図である。
まず、ステップ１では、自車軌道計算部１４０から出力された軌道推定値と推定誤差共分散行列がバイアス補正部８４０へ入力される。また、ヨーレートセンサ８３０からヨー角速度観測値がバイアス補正部８４０へ入力される。この時、ヨー角速度観測値を追尾フィルタなどで平滑処理した後、そのヨー角速度観測値を入力してもよいものとする。ただし、自車軌道計算部１４０から常にヨー角速度推定値が出力されることは無いため、この処理フローのようにヨー角速度推定値が得られないサンプリング時刻では、後述のバイアス補正値メモリから保存したバイアス補正値を用いてヨー角速度観測値を補正する（ステップ２）。ここで、バイアス補正値の初期値は０とする。
次に、ステップ３では、自車軌道計算部１４０から出力された軌道推定値と推定誤差共分散行列を用いて、ヨー角速度観測値による旋回モデルに基づいた予測値と予測誤差共分散およびヨー角速度推定値による旋回モデルに基づいた予測値と予測誤差共分散を算出する。ただし、通常、レーダ８１０で停止物を検出するサンプリングレートと自車のヨーレートセンサ８３０からヨー角速度を検出する時刻は異なり、ヨーレートセンサ８３０のサンプリングレートのほうが細かいことから、ヨー角速度の観測時刻ｔとヨー角速度推定値の更新時刻ｔ’が異なる場合には時刻合わせをする必要がある。
【００２５】
図７は、時刻合わせを示す図である。
そのため、時刻ｔまでの時間外挿処理によって、時刻ｔにおける推定値と推定誤差共分散行列を算出する。
【数１４】
ここで、Ｆ（ωｅｓｔ（ｔ’），ｔ−ｔ’）はヨー角速度を時刻ｔ’におけるヨー角速度推定値ωｅｓｔとした場合の旋回モデルの時刻ｔ’から時刻ｔへの状態推移行列を意味する。また、Ｑ（ｔ−ｔ’）はシステム雑音の共分散行列である。
【００２６】
上記の推定値と推定誤差共分散を用いて、時刻ｔからΔＴ秒後のヨー角速度観測値による旋回モデルに基づいた予測値ＸハットＯＢＳ，ｔ＋ΔＴと予測誤差共分散ＰＯＢＳ，ｔ＋ΔＴを算出する。さらにヨー角速度推定値による旋回モデルに基づいたΔＴ秒後の予測値ＸハットＥＳＴ，ｔ＋ΔＴと予測誤差共分散ＰＥＳＴ，ｔ＋ΔＴを算出する。
【００２７】
そして、ステップ４では、カイ二乗検定による次の判定式を満たさない場合は、ヨー角速度観測値による予測値とヨー角速度推定値による予測値が異なるため、ヨー角速度観測値にはバイアス誤差が含まれていることとする。
【数１５】
ここで、εｔｈはカイ二乗分布表から求めることとする。
【００２８】
ここで、ヨー角速度観測値の観測レートはヨー角速度推定値の出力レート（または、レーダの検出レート）より細かいため、ヨー角速度観測値が得られる毎時刻で、判定式を用いた判定を行い、上記判定式をＮ回中Ｍ回満たさない場合は、以下のバイアス誤差推定処理を実施する。満たす場合は、ヨー角速度観測値にはバイアス誤差が含まれていないこととみなして、バイアス補正値メモリに保存してあるバイアス誤差を用いて補正する（ステップ２）。
次に、ステップ５では、上記ヨー角速度推定値とヨー角速度観測値を用いて、ヨー角速度観測値のバイアス誤差を推定する。ここでは、例えばカルマンフィルタなど用いてバイアス誤差を推定する。
バイアス補正値メモリにバイアス誤差推定値をバイアス補正値として保存する（ステップ６）。最後にバイアス補正値を用いて補正したヨー角速度が出力される（ステップ７）。
【００２９】
以上説明したヨーレートバイアス補正装置８００において、自車軌道計算部１４０は、自車に搭載されたレーダ８１０から得た停止物の位置を停止物基準の座標系へ変換することにより、停止物基準座標における自車位置を算出し、前記自車位置を用いて自車軌道を推定する。
【００３０】
自車軌道計算部１４０は、レーダ８１０から所定の距離以内に存在する停止物のみを抽出し、前記停止物が所定数以上得られる場合、自車軌道を推定する。
【００３１】
自車軌道計算部１４０は、旋回モデルに基づく拡張カルマンフィルタを用いて、停止物基準座標における自車位置より自車軌道を推定する。
【００３２】
バイアス補正部８４０は、ヨーレートセンサ８３０から得られるヨー角速度観測値および自車軌道計算部１４０で算出したヨー角速度推定値のそれぞれで時間外挿して算出した予測値の差分を予測誤差共分散行列を用いたカイ二乗判定によって、同一とみなされない場合はヨー角速度観測値に含まれるバイアス誤差推定を行う。
【００３３】
バイアス補正部８４０は、ヨー角速度観測値の観測時刻と自車軌道計算部１４０で算出したヨー角速度推定値の推定時刻が異なる場合、観測時刻と推定時刻の差を時間外挿することで、ヨー角速度観測値の観測時刻にヨー角速度推定値の推定時刻を合わせる。
【００３４】
このように、不要信号を含む停止物の中から時間方向に連続性のある確かな停止物を抽出し、その停止物と自車との位置を用いてヨー角速度を含んだ自車軌道を推定することで、正確にヨー角速度を算出することができる。さらに前記ヨー角速度推定値を用いて、ヨーレートセンサで検出されるヨー角速度観測値のバイアス誤差を補正することにより、高サンプリングレートで得られるヨー角速度観測値を利用し、正確な先行車判定をすることが可能となる。
【００３５】
実施の形態２．
実施の形態２について、図８〜図１０を用いて説明する。
なお、実施の形態１と共通する部分については、同一の符号を付し、説明を省略する。
【００３６】
図８は、この実施の形態におけるヨーレートバイアス補正装置８００の構成を示すブロック図である。
実施の形態１では、ある１つの停止物を用いてヨー角速度を補正していたが、実際には複数の別々の停止物（デリニエータであれば、複数のデリニエータなど）が観測されるため、この実施の形態２では複数の停止物を用いたヨーレートバイアス補正装置８００を示す。
【００３７】
自車軌道計算部１４０では、複数の停止物を入力として自車軌道を推定する。また、停止物用追尾部１２０から相関済み停止物を自車軌道計算部１４０へ入力する時刻は、停止物ごとに異なる。
【００３８】
図９は、複数停止物の相対位置の一例を示す図である。
図１０は、タイムシーケンスの説明図である。
【００３９】
例えば、図９のように、停止物１と停止物２の２つの停止物の位置が得られる場合、図１０のようなタイムシーケンスに従い、停止物１の追尾（図１０の停止物用追尾１に相当）と停止物２の追尾（図１０の停止物用追尾２に相当）の処理が行われる。
まず、図３のフローに従い、図１０のように停止物用追尾１から時刻ｔ４において相関済み停止物が自車軌道計算部１４０へ出力される。停止物用追尾２では時刻ｔ５において出力されるため、先に自車軌道計算部１４０へ出力した停止物１を基準とした停止物基準座標系で自車軌道計算手段の処理を行う。前記自車軌道計算部１４０の処理は実施の形態１と同様のため省略する。
時刻ｔ５より、停止物用追尾２から相関済み停止物が出力されるため、前記停止物を停止物１を基準とした座標系へ変換する。この時には時刻ｔ５における停止物１と停止物２の相対位置ベクトルを用いて座標変換すればよい（図９）。この相対位置ベクトルは、停止物用追尾部１２０で出力される停止物１推定位置と停止物２推定位置との相対位置とすればよい。
時刻ｔ６にて停止物１の停止物が欠落した場合でも、停止物２の停止物を用いて更新するため精度向上が見込まれる。
停止物１が時刻ｔ９でレーダ覆域外へ出る場合、それ以降は他の停止物を基準とした座標系で自車軌道計算部１４０の処理を継続することとする。時刻ｔ９以降の自車軌道計算部１４０による自車両の位置は停止物１を基準とした座標系から停止物２を基準とした座標系に変換する。
以降、他の停止物が検出される場合も同様の処理を繰り返すことで、停止物１つでは観測回数が少なく、自車軌道の推定精度が良くない場合でも複数の停止物を用いて、推定を継続することで推定精度を向上させる効果がある。
その他の処理は実施の形態１と同様のため省略する。
【００４０】
さらに図８を用いて、ヨーレートバイアス補正装置８００の入出力について説明する。
停止物用追尾からは相関済み停止物の位置（ドップラ速度が観測されれば、ドップラ速度も含める）、停止物番号、および停止物の推定値（位置と速度）を出力する。停止物基準座標変換では、各停止物位置（およびドップラ速度）の時系列データを各停止物の推定位置を用いて基準停止物（上記の例では、停止物１に相当）を原点とした座標系に変換する。軌道推定部では、順次入力された停止物位置を用いてヨー角度推定値を含む軌道を推定し、バイアス補正手段へ出力する。
【００４１】
以上説明したヨーレートバイアス補正装置８００において、停止物用追尾部１２０は、当該停止物の推定位置を停止物記憶部１３０へ出力し、前記自車軌道計算部１４０は、複数の停止物から得た位置の時系列データを用いて、ある基準となる停止物の座標系において複数の停止物から自車位置を算出し、前記自車位置を用いて自車軌道を推定する。
【００４２】
実施の形態３．
実施の形態３について、図１１〜図１２を用いて説明する。
なお、実施の形態１〜実施の形態２と共通する部分については、同一の符号を付し、説明を省略する。
【００４３】
図１１は、この実施の形態におけるヨーレートバイアス補正装置８００の構成を示すブロック図である。
自車軌道計算部１４０ａ〜１４０ｃでは、各停止物の軌道推定値と推定誤差分散行列を算出し、各停止物の推定値（停止物用追尾で算出した推定値）と共に軌道統合部１５０へ出力する。軌道統合部１５０では、各停止物の推定値を用いて座標を合わせ、軌道推定値を推定誤差共分散行列を用いて重み付け統合する。
【００４４】
図１２は、この実施の形態における軌道統合部１５０の動作を示す処理フロー図である。
【００４５】
ステップＰ１で軌道統合部１５０へ入力される軌道の数ＮＴＲＫが２以上である場合は、座標系の基準となる停止物を選択するステップＰ２へ進む。相関回数Ｎｃｏｒの多い軌道を基準停止物と選択し、ステップＰ３では他の軌道の位置を基準停止物の座標系へ変換する。
ステップＰ４の軌道を統合する方式としては、既存の航跡統合方式を用いても良いとする。例えば、有色性に考慮して共分散交差法を用いて統合をする。あるいは有色性は考慮していないが、最小二乗統合法を使うこともある。
【００４６】
演算負荷を抑えるために、次の式を用いて、あらかじめ定めたパラメータａ（０≦ａ≦１）で重み付け統合してもよい。
【数１６】
【００４７】
あるいは、次の式を用いて、航跡の更新状況に応じて重み付け統合してもよい。
【数１７】
ここで、停止物１と２からそれぞれ軌道ｘＴＲＫ１、ｘＴＲＫ２が得られたとする。また、ｘＳＹＳは統合後の軌道、ｎＴＲＫ１は停止物１の相関回数Ｎｃｏｒ、ｎＴＲＫ２は停止物２の相関回数Ｎｃｏｒに相当する。
【００４８】
その他の処理は実施の形態１もしくは２と同様のため省略する。
【００４９】
以上説明したヨーレートバイアス補正装置８００は、レーダ８１０と、車速センサ８２０と、ヨーレートセンサ８３０と、停止物識別部１１０と、停止物用追尾部１２０と、停止物記憶部１３０と、自車軌道計算部１４０と、軌道統合部１５０と、バイアス補正部８４０とを備える。
レーダ８１０は、車両前方の所定角度範囲内にレーダ波を照射し、物体に反射されたレーダ波を受信して、その物体の位置及び相対速度を検出する。
車速センサ８２０は、車両の速度を検出する。
ヨーレートセンサ８３０は、ヨー角速度を検出する。
停止物識別部１１０は、上記レーダ８１０により検出された物体の相対速度と上記車速センサ８２０により検出された車両の速度に基づいて当該物体が停止物体であるか否かを識別する。
停止物用追尾部１２０は、上記停止物識別部１１０により停止物体であると識別された複数の停止物の中から追尾フィルタによって時間方向に相関のある停止物を抽出し、当該停止物推定位置を算出する。
停止物記憶部１３０は、上記停止物用追尾部１２０から得られる当該停止物の推定位置および相関済み停止物を記憶する。
自車軌道計算部１４０ａ〜１４０ｃは、上記停止物記憶部１３０より得た相関済み停止物位置の時系列データを入力として、前記停止物位置を停止物基準の座標系へ変換することで自車位置の時系列データを得て、前記自車位置よりヨー角速度を含む自車軌道を推定する。
軌道統合部１５０は、上記自車軌道計算部１４０ａ〜１４０ｃのそれぞれより出力された複数の軌道推定値を重み付け統合して自車軌道を算出する。
バイアス補正部８４０は、ヨーレートセンサ８３０で検出するヨー角速度と前記自車軌道計算部１４０ａ〜１４０ｃより得たヨー角速度推定値を用いて、ヨー角速度のバイアス誤差を算出し、補正する。
【００５０】
軌道統合部１５０は、複数停止物の各軌道推定値の中から相関停止物の最も多い停止物を基準とした座標系において軌道推定値を統合して自車軌道を算出する。、
【００５１】
実施の形態４．
実施の形態４について、図１３を用いて説明する。
なお、実施の形態１〜実施の形態３と共通する部分については、同一の符号を付し、説明を省略する。
【００５２】
図１３は、この実施の形態における停止物用追尾部１２０の構成を示すブロック図である。
停止物用追尾部１２０は、更に、停止物をクラスタリングする停止物クラスタリング部を有する。相関部・平滑部・予測部・遅延部からなる追尾フィルタは、停止物クラスタリング部がクラスタリングしたクラスタを追尾する。
【００５３】
実際の道路走行環境下では視線誘導標（デリニエータ）、ガードレール、壁などの停止物からの反射がある。特にガードレールや壁などの表面積の広い停止物からは複数の反射点が得られ、反射点１点を追尾することが困難な場合がある。そのため、停止物用追尾部１２０の追尾フィルタの前段で停止物のクラスタリングを実施することで、表面積の大きい停止物から得られる複数反射点をまとめ、そのクラスタを追尾することで停止物用追尾手段の相関性能を高める。
【００５４】
その他の処理は実施の形態１〜３と同様のため省略する。
【００５５】
以上説明したヨーレートバイアス補正装置８００において、停止物用追尾部１２０は、停止物をクラスタリングし、クラスタを追尾する。
【００５６】
実施の形態５．
実施の形態５について、図１４〜図２５を用いて説明する。
なお、実施の形態１〜実施の形態４と共通する部分については、同一の符号を付し、説明を省略する。
【００５７】
図１４は、この実施の形態における角速度推定装置２００のハードウェア構成の一例を示すハードウェア構成図である。
角速度推定装置２００は、例えば、処理装置９１１と、記憶装置９１４と、入力装置９０２と、出力装置９０１とを有するコンピュータである。
処理装置９１１は、コンピュータプログラムを実行することにより、データを処理し、角速度推定装置２００全体を制御する。
記憶装置９１４は、処理装置９１１が実行するコンピュータプログラムや、処理装置９１１が処理するデータなどを記憶する。記憶装置９１４は、例えば、揮発性メモリ、不揮発性メモリ、フラッシュメモリ、磁気ディスク装置、光学ディスク装置などである。
入力装置９０２は、角速度推定装置２００の外部から信号や情報を入力し、処理装置９１１が処理できる形式のデータに変換する。入力装置９０２が変換したデータは、処理装置９１１が直接処理してもよいし、記憶装置９１４が一時的に記憶してもよい。入力装置９０２は、例えば、キーボードやマウスなど利用者の操作を入力する操作入力装置、アナログ信号をデジタルデータに変換するアナログデジタル変換装置などの変換装置、他の装置が送信した信号を受信する受信装置などのインターフェイス装置などがある。
出力装置９０１は、処理装置９１１が処理したデータや記憶装置９１４が記憶したデータを、角速度推定装置２００の外部に出力できる形式に変換して出力する。出力装置９０１は、例えば、画像を表示する表示装置やスピーカーなど人の五感で感知できる形式にデータを変換して出力する装置、デジタルデータをアナログ信号に変換するデジタルアナログ変換装置などの変換装置や、他の装置に対して信号を送信する送信装置などのインターフェイス装置など、他の装置に入力できる形式にデータを変換して出力する装置などがある。
【００５８】
角速度推定装置２００の機能ブロックは、記憶装置９１４が記憶したコンピュータプログラムを処理装置９１１が実行することにより実現される。なお、角速度推定装置２００の機能ブロックは、必ずしもコンピュータにより実現される構成でなくてもよく、デジタル回路やアナログ回路などの電子回路によって実現される構成であってもよいし、機械的構成など電気的構成以外の構成によって実現される構成であってもよい。
また、実施の形態１〜実施の形態４で説明したヨーレートバイアス補正装置８００の機能ブロックも、同様に、コンピュータにより実現される構成であってもよいし、その他の構成により実現される構成であってもよい。
【００５９】
図１５は、この実施の形態における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図である。
角速度推定装置２００は、自動車などの移動体に搭載されている。移動体には、角速度推定装置２００のほか、周辺物体観測装置８１１、移動速度観測装置８２１、角速度観測装置８３１などが搭載されている。
【００６０】
周辺物体観測装置８１１は、移動体の周辺に存在する物体の位置や速度などを観測する。周辺物体観測装置８１１は、例えば、レーダである。例えば、周辺物体観測装置８１１は、電波やレーザ光などの放射波を放射して物体に当たって反射した反射波を検知することにより、伝搬時間を測定し、物体までの距離を算出する。周辺物体観測装置８１１は、放射波を放射する方向もしくは反射波を受信する方向を走査するなどして、物体が存在する方向を判定する。周辺物体観測装置８１１が放射波を１回走査するのにかかる時間は、例えば０．２秒である。したがって、移動体と物体との間の相対位置関係が変化しない場合、周辺物体観測装置８１１は、同一の物体を、例えば０．２秒ごとに繰り返し観測する。周辺物体観測装置８１１は、算出した距離と、判定した方向とに基づいて、移動体に対する物体の相対位置を算出する。また、周辺物体観測装置８１１は、例えば、ドップラー効果による放射波と反射波との間の波長のずれを検出することにより、移動体に対する物体の相対速度を検出する。周辺物体観測装置８１１は、物体の相対位置や相対速度などの観測結果を表わす信号を出力する。
【００６１】
移動速度観測装置８２１は、移動体の移動速度を観測する。移動速度観測装置８２１は、例えば、自動車の車軸に取り付けた回転数計により車軸の回転数を測定し、移動速度を算出する。移動速度観測装置８２１は、例えば０．１秒の間の車軸の回転数を計数する。移動速度観測装置８２１は、計数した回転数にタイヤの一周の長さを乗じることにより、０．１秒の間に移動体が進んだ距離を算出する。移動速度観測装置８２１は、算出した距離を１０倍することにより、移動体の秒速を算出する。この例の場合、移動速度観測装置８２１は、０．１秒に１回ずつ繰り返し移動体の移動速度を観測する。移動速度観測装置８２１は、移動体の移動速度を観測した観測結果を表わす信号を出力する。
なお、移動速度観測装置８２１は、移動体の移動速度を算出せず、測定した車軸の回転数をそのまま表わす信号を出力する構成であってもよい。その場合、後述する速度観測値取得部２１２が、移動速度観測装置８２１に代わって、上述した計算を実行し、移動体の移動速度を算出する。
【００６２】
角速度観測装置８３１は、移動体の角速度を観測する。移動速度観測装置８２１は、例えば、ヨーレートセンサやジャイロセンサなどである。角速度観測装置８３１は、移動体の三次元方向（水平方向・前後方向・ねじれ方向）の角速度を観測する構成であってもよいし、水平方向の角速度のみを観測する構成であってもよい。角速度観測装置８３１もまた、移動体の角速度を繰り返し観測する。観測間隔は、例えば１０ミリ秒である。角速度観測装置８３１は、移動体の角速度を観測した観測結果を表わす信号を出力する。
【００６３】
角速度推定装置２００は、周辺物体観測装置８１１、移動速度観測装置８２１、角速度観測装置８３１などの観測結果に基づいて、移動体の角速度を推定する。角速度観測装置８３１が観測する角速度には、バイアス誤差やその他の誤差が含まれるからである。角速度推定装置２００は、周辺物体観測装置８１１、移動速度観測装置８２１、角速度観測装置８３１などの観測結果を総合することにより、より正確な角速度を推定する。なお、角速度推定装置２００は、移動体の角速度だけでなく、移動体の速度や移動方向、位置などを推定する構成であってよい。
また、角速度推定装置２００は、推定した角速度と、角速度観測装置８３１が観測した角速度とに基づいて、角速度観測装置８３１が観測した角速度の誤差を算出する構成であってもよい。角速度推定装置２００は、算出した角速度の誤差を記憶しておき、周辺物体観測装置８１１などの観測結果が得られない場合や、得られた観測結果の信頼性が低い場合に、記憶しておいた角速度の誤差を用いて、角速度観測装置８３１が観測した角速度を補正することにより、移動体の角速度を推定する構成であってもよい。
【００６４】
角速度推定装置２００は、相対位置取得部２１１と、速度観測値取得部２１２と、角速度観測値取得部２１３と、静止物体判定部２２０と、物体相関部２３０と、状態推定部２４０とを有する。
【００６５】
相対位置取得部２１１は、入力装置９０２を用いて、周辺物体観測装置８１１が出力した信号を入力し、周辺物体観測装置８１１が観測した観測結果を取得する。
相対位置取得部２１１は、処理装置９１１を用いて、周辺物体観測装置８１１が出力した信号を入力した時刻を判定し、周辺物体観測装置８１１が物体を観測した観測時刻を算出する。相対位置取得部２１１は、信号を入力した時刻をそのまま観測時刻とみなす構成であってもよいし、周辺物体観測装置８１１における遅延時間を考慮して、信号を入力した時刻よりも遅延時間だけ戻った時刻を観測時刻とする構成であってもよい。なお、周辺物体観測装置８１１が出力する信号に、周辺物体観測装置８１１が物体を観測した観測時刻を表わす情報が含まれる構成であってもよい。その場合、相対位置取得部２１１は、処理装置９１１を用いて、入力した信号から観測時刻を取得する。
相対位置取得部２１１は、記憶装置９１４を用いて、周辺物体観測装置８１１が物体を観測した観測時刻や、観測された物体の相対位置および相対速度を表わすデータを記憶する。相対位置取得部２１１が記憶するデータを「周辺物体観測データ」と呼ぶ。「周辺物体観測データ」のうち、観測時刻を表わすデータを「物体観測時刻データ」、物体の相対位置を表わすデータを「相対位置観測値データ」、物体の相対速度を表わすデータを「相対速度観測値データ」と呼ぶ。
【００６６】
速度観測値取得部２１２は、入力装置９０２を用いて、移動速度観測装置８２１が出力した信号を入力し、移動速度観測装置８２１が観測した観測結果を取得する。速度観測値取得部２１２は、処理装置９１１を用いて、相対位置取得部２１１と同様の処理をして、移動速度観測装置８２１が移動体の移動速度を観測した観測時刻を求める。速度観測値取得部２１２は、記憶装置９１４を用いて、移動速度観測装置８２１が移動体の移動速度を観測した観測時刻や、観測された移動体の移動速度を表わすデータを記憶する。速度観測値取得部２１２が記憶するデータを「移動速度データ」と呼ぶ。「移動速度データ」のうち、観測時刻を表わすデータを「移動速度観測時刻データ」、移動体の移動速度を表わすデータを「移動速度観測値データ」と呼ぶ。
【００６７】
角速度観測値取得部２１３は、入力装置９０２を用いて、角速度観測装置８３１が出力した信号を入力し、角速度観測装置８３１が観測した観測結果を取得する。角速度観測値取得部２１３は、処理装置９１１を用いて、相対位置取得部２１１や速度観測値取得部２１２と同様の処理をして、角速度観測装置８３１が移動体の角速度を観測した観測時刻を求める。角速度観測値取得部２１３は、記憶装置９１４を用いて、角速度観測装置８３１が移動体の角速度を観測した観測時刻や、観測された移動体の角速度を表わすデータを記憶する。角速度観測値取得部２１３が記憶するデータを「移動体角速度データ」と呼ぶ。「移動体角速度データ」のうち、観測時刻を表わすデータを「角速度観測時刻データ」、移動体の角速度を表わすデータを「角速度観測値データ」と呼ぶ。
【００６８】
静止物体判定部２２０は、処理装置９１１を用いて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。静止物体判定部２２０は、記憶装置９１４を用いて、判定した判定結果を表わすデータを記憶する。静止物体判定部２２０が記憶するデータを「静止判定結果データ」と呼ぶ。
例えば、静止物体判定部２２０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データと、速度観測値取得部２１２が記憶した移動速度データとを入力する。静止物体判定部２２０は、処理装置９１１を用いて、入力した周辺物体観測データから、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度を表わす相対速度観測値データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、入力した移動速度データから、移動体の移動速度を表わす移動速度観測値データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、相対速度観測値データが表わす相対速度と、移動速度観測値データが表わす移動速度とに基づいて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。
【００６９】
図１６は、移動体８０１の移動速度と、物体７０１，７０２の移動体８０１に対する相対速度との関係を説明するための図である。
矢印７１１，７１３は、それぞれ、移動体８０１，物体７０２の移動速度を表わす。物体７０１は、静止しているものとする。矢印７２１，７２２は、それぞれ、物体７０１，７０２の移動体８０１に対する相対速度を表わす。
物体７０１は静止しているので、移動体８０１を基準にすると、物体７０１は、移動体８０１の移動速度とちょうど逆向きの方向に、移動体８０１の移動速度とちょうど同じ速度で移動しているように見える。これに対して、物体７０２は移動しているので、移動体８０１を基準にすると、物体７０２は、移動体８０１の移動による見かけの速度（矢印７２３）と、物体７０２の移動による実際の速度（矢印７１３）とを合成した速度で移動しているように見える。したがって、物体の移動体に対する相対速度（見かけの速度）は、次の式で表わすことができる。
【数１８】
ただし、ｖｒは、物体の移動体に対する相対速度を表わすベクトルである。ｖｔは、物体の移動速度を表わすベクトルである。ｖｓは、移動体の移動速度を表わすベクトルである。
【００７０】
例えば、静止物体判定部２２０は、処理装置９１１を用いて、物体の移動体に対する相対速度を表わすベクトルｖｒと、移動体の移動速度を表わすベクトルｖｓとを加算して、物体の移動速度を表わすベクトルｖｔを算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出したベクトルｖｔの絶対値を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した絶対値を所定の閾値と比較する。閾値は、物体の移動体に対する相対速度や移動体の移動速度の観測誤差を考慮してあらかじめ設定しておく。ベクトルｖｔの絶対値が閾値より小さい場合、静止物体判定部２２０は、処理装置９１１を用いて、その物体が静止していると判定する。
【００７１】
なお、ドップラー効果を利用して物体の移動体に対する相対速度を観測する場合、観測される相対速度は、物体の見かけの速度（矢印７２１，７２２）のうち、物体と移動体との距離方向の成分（矢印７２５，７２６）だけであり、移動体８０１を軸とする回転方向の成分（矢印７２７，７２８）は観測されない。観測される相対速度は、次の式で表わすことができる。
【数１９】
ただし、ｖｄは、物体の移動体に対する相対速度の距離方向成分を表わす実数である。αは、物体の移動体に対する移動速度の方向と、物体から見た移動体が見える方向との間の角度を表わす実数である。
物体の移動体に対する相対速度のうち、移動体８０１を軸とする回転方向の成分（矢印７２７，７２８）が観測されないので、角度αは未知である。しかし、物体７０１は静止しているので、物体７０１の角度α（７３３）は、移動体８０１の移動方向と、移動体８０１から見た物体７０１が見える方向との間の角度７３１と一致する。これに対し、物体７０２は移動しているので、物体７０２の角度α（７３４）は、移動体８０１の移動方向と、移動体８０１から見た物体７０１が見える方向との間の角度７３２と必ずしも一致しない。
【００７２】
例えば、静止物体判定部２２０は、処理装置９１１を用いて、移動体の移動方向と、移動体から見た物体が見える方向との間の角度の余弦（コサイン）を算出する。静止物体判定部２２０は、処理装置９１１を用いて、移動体の移動速度を表わすベクトルｖｓの絶対値と、算出した余弦との積を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した積から、物体の移動体に対する相対速度の距離方向成分を表わす実数ｖｄを差し引いた差を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した差を、所定の閾値と比較する。閾値は、物体の移動体に対する相対速度や移動体の移動速度の観測誤差を考慮してあらかじめ設定しておく。差が閾値より小さい場合、静止物体判定部２２０は、処理装置９１１を用いて、その物体が静止していると判定する。
【００７３】
図１７は、物体の移動体に対する相対速度の観測時刻７８１〜７８３と、移動体の移動速度の観測時刻７９１〜７９９との関係を説明するための図である。
上述したように、周辺物体観測装置８１１が物体の移動体に対する相対位置や相対速度を観測する観測周期７７１と、移動速度観測装置８２１が移動体の移動速度を観測する観測周期７７２とは、異なっている。また、周辺物体観測装置８１１が物体の移動体に対する相対位置や相対速度を観測する観測時刻７８１〜７８３は、物体が存在する方向を走査した時刻であるから、物体が存在する方向が変われば、１回の観測周期のなかでのタイミングが変化し、不定期である。これに対し、移動速度観測装置８２１が移動体の移動速度を観測する観測時刻７９１〜７９９の間隔は、一定であり、観測周期７７２と等しい。
したがって、物体の移動体に対する相対速度の観測時刻７８１〜７８３と、移動体の移動速度の観測時刻７９１〜７９９とが一致する可能性はほとんどない。しかし、物体が静止しているか否かを静止物体判定部２２０が判定するために用いる物体の移動体に対する相対速度と、移動体の移動速度とは、同じ時刻に観測したものである必要がある。
【００７４】
車軸の回転数を計数することにより移動体の移動速度を観測する場合、観測される移動速度は、計数を開始した時刻から計数を終了した時刻までの移動速度の平均値である。例えば、時刻７９３に観測した移動速度は、時刻７９２から時刻７９３までの移動速度の平均値である。したがって、静止物体判定部２２０は、時刻７８１に観測した物体の移動体に対する相対速度を、時刻７９３に観測した移動体の移動速度と比較すればよい。
【００７５】
例えば、静止物体判定部２２０は、処理装置９１１を用いて、移動速度観測装置８２１の観測結果を速度観測値取得部２１２が取得するたびに処理を実行する。静止物体判定部２２０は、処理装置９１１を用いて、速度観測値取得部２１２が記憶した今回の移動速度データと、１つ前の移動速度データとを入力する。静止物体判定部２２０は、処理装置９１１を用いて、入力した２つの移動速度データから、移動速度観測装置８２１が移動体の移動速度を観測した今回の観測時刻と前回の観測時刻とをそれぞれ表わす２つの移動速度観測時刻データを取得する。
次に、静止物体判定部２２０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、未処理の周辺物体観測データを入力する。静止物体判定部２２０は、処理装置９１１を用いて、入力した周辺物体観測データから、周辺物体観測装置８１１が物体を観測した観測時刻を表わす物体観測時刻データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した物体観測時刻データが表わす観測時刻と、取得した２つの移動速度観測時刻データが表わす観測時刻とを比較する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した前回の観測時刻より前である場合は、移動速度観測装置８２１が前回観測した移動体の移動速度を使って、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。静止物体判定部２２０は、処理装置９１１を用いて、入力した１つ前の移動速度データから、移動速度観測装置８２１が観測した移動速度を表わす移動速度観測値データを取得する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した前回の観測時刻より後で、かつ、今回の観測時刻より前である場合は、移動速度観測装置８２１が今回観測した移動体の移動速度を使って、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。静止物体判定部２２０は、処理装置９１１を用いて、入力した今回の移動速度データから、移動速度観測装置８２１が観測した移動速度を表わす移動速度観測値データを取得する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した今回の観測時刻より後である場合、移動速度観測装置８２１が次回観測する移動体の移動速度を使って、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。移動体の移動速度の次回の観測値はまだ得られていないので、静止物体判定部２２０は、その周辺物体観測データについて今回は処理をしない。
【００７６】
例えばこのようにして、静止物体判定部２２０は、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定するために使用する移動体の移動速度を、移動速度観測装置８２１が観測した観測結果のなかから選択する。あるいは、静止物体判定部２２０は、移動速度観測装置８２１が観測した観測結果に基づいて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定するために使用する移動体の移動速度を算出する構成であってもよい。
【００７７】
例えば、静止物体判定部２２０は、処理装置９１１を用いて、速度観測値取得部２１２が記憶した移動速度データが表わす観測時刻と移動速度とに基づいて、平滑処理をする。周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも前である場合、静止物体判定部２２０は、処理装置９１１を用いて、平滑処理の結果に基づいて、周辺物体観測装置８１１が物体を観測した観測時刻における移動体の移動速度を推定する。あるいは、周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも前である場合、静止物体判定部２２０は、処理装置９１１を用いて、平滑処理の結果に基づいて、周辺物体観測装置８１１が物体を観測した観測時刻における移動体の移動速度を予測する。静止物体判定部２２０は、処理装置９１１を用いて、推定あるいは予測した移動体の移動速度を用いて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。
【００７８】
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が観測した物体のうち、静止物体判定部２２０が静止していると判定した物体について、相関処理をする。すなわち、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が物体を観測した観測結果のなかから、静止している同じ物体を複数回観測した観測結果を判定する。物体相関部２３０は、記憶装置９１４を用いて、判定した判定結果を表わすデータを記憶する。物体相関部２３０が記憶するデータを「相関結果データ」と呼ぶ。
例えば、物体相関部２３０は、処理装置９１１を用いて、静止物体判定部２２０が記憶した静止判定結果データを入力する。物体相関部２３０は、処理装置９１１を用いて、入力した静止判定結果データが表わす判定結果に基づいて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、静止物体判定部２２０が静止していると判定した物体についての周辺物体観測データであって、未処理の周辺物体観測データを入力する。物体相関部２３０は、処理装置９１１を用いて、入力した周辺物体観測データから、周辺物体観測装置８１１が物体を観測した観測時刻を表わす物体観測時刻データと、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置を表わす相対位置観測値データとを取得する。
処理済の周辺物体観測データから推定した物体の移動体に対する相対位置の軌跡がある場合、物体相関部２３０は、処理装置９１１を用いて、取得したデータが表わす観測時刻及び相対位置がその軌跡に適合するか否かを判定する。適合する軌跡が複数ある場合、物体相関部２３０は、処理装置９１１を用いて、取得したデータが表わす観測時刻及び相対位置が最もよく適合する軌跡を判定する。物体相関部２３０は、処理装置９１１を用いて、取得したデータが表わす観測時刻及び相対位置を、（最もよく）適合する軌跡に組み入れて、軌跡を更新する。
取得したデータが表わす観測時刻及び相対位置が適合する軌跡がない場合、物体相関部２３０は、処理装置９１１を用いて、適合する軌跡がない他の周辺物体観測データのなかに、新たな軌跡となり得る周辺物体観測データがあるか否かを判定する。新たな軌跡となり得る周辺物体観測データがある場合、物体相関部２３０は、処理装置９１１を用いて、新たな軌跡を生成する。
物体相関部２３０は、記憶装置９１４を用いて、生成あるいは更新した軌跡を表わすデータを記憶する。
【００７９】
なお、この時点では、移動体の位置や向きがまだわかっていないので、物体相関部２３０における相関処理は、物体の移動体に対する相対位置（見かけの位置）に基づいて行う。
【００８０】
図１８は、移動体８０１の移動軌跡７５１と、物体７０３〜７０８の移動体８０１に対する相対位置の軌跡７６１〜７６６との関係を説明するための図である。
例えば、移動体８０１が、移動軌跡７５１に示すように蛇行して移動したものとする。また、物体７０３〜７０８は、静止しているものとする。
【００８１】
時刻７８５において、周辺物体観測装置８１１は、物体７０３〜７０８について、移動体８０１に対する相対位置７４１ａ〜７４１ｆを観測する。
時刻７８６において、周辺物体観測装置８１１は、物体７０３〜７０８について、移動体８０１に対する相対位置７４２ａ〜７４２ｆを観測する。
時刻７８７において、周辺物体観測装置８１１は、物体７０３〜７０８について、移動体８０１に対する相対位置７４３ａ〜７４３ｆを観測する。
物体相関部２３０は、これらの観測結果に基づいて、相関処理を行い、軌跡７６１〜７６６を生成する。
【００８２】
このように、静止している物体７０３〜７０８の移動体８０１に対する相対位置は、移動体８０１の移動に伴って変化する。特に、移動体８０１が旋回した場合、物体７０３〜７０８が見える方向が大きく変化するので、物体７０３〜７０８の移動体８０１に対する相対位置も大きく変化する。
このため、静止している物体７０３〜７０８の移動体８０１に対する相対位置の軌跡７６１〜７６６は、このように複雑な形状になる場合がある。
【００８３】
周辺物体観測装置８１１が複数の静止している物体７０３〜７０８を観測した場合、それぞれの物体７０３〜７０８の移動体８０１に対する相対位置は、移動体８０１の移動や旋回に伴って大きく変化するが、物体７０３〜７０８間の位置関係は変わらない。物体７０３〜７０８の移動体８０１に対する相対位置は、移動体８０１の旋回に伴って方向が変化するだけでなく、移動体８０１の移動に伴って方向や距離が変化する。これに対して、物体７０３〜７０８は静止しているので、移動体８０１が移動や旋回をしても、物体７０３〜７０８間の距離は変わらず、移動体８０１の旋回に伴って物体７０３〜７０８間の方向が変化するだけである。
【００８４】
物体相関部２３０は、このことを利用し、処理装置９１１を用いて、周辺物体観測装置８１１が観測する物体の移動体８０１に対する相対位置を予測する。物体相関部２３０は、処理装置９１１を用いて、予測した結果を使って相関処理をする。
【００８５】
例えば、物体相関部２３０は、処理装置９１１を用いて、１回の走査周期内に周辺物体観測装置８１１が観測した複数の物体のなかから、静止している物体を１つ選択する。物体相関部２３０は、処理装置９１１を用いて、選択した物体の座標が原点に一致するよう、周辺物体観測装置８１１が今回の走査周期内に観測した複数の静止している物体の移動体８０１に対する相対位置の座標を平行移動する。また、物体相関部２３０は、処理装置９１１を用いて、前回の走査周期内に周辺物体観測装置８１１が観測した複数の物体のなかからも同様に、静止している物体を１つ選択する。物体相関部２３０は、処理装置９１１を用いて、選択した物体の座標が原点に一致するよう、周辺物体観測装置８１１が前回の走査周期内に観測した複数の静止している物体の移動体８０１に対する相対位置の座標を平行移動する。
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が今回の走査周期内に観測した物体の相対位置を平行移動した座標を、原点を中心に回転する。物体相関部２３０は、処理装置９１１を用いて、回転した座標それぞれと、周辺物体観測装置８１１が前回の走査周期内に観測した物体の相対位置を平行移動した座標それぞれとの間の距離を算出する。算出した距離が所定の閾値より小さい場合、物体相関部２３０は、処理装置９１１を用いて、その２つの座標に対応する物体が同一の物体であるとみなし、同一の物体であるとみなせる座標の組の数を数える。
物体相関部２３０は、処理装置９１１を用いて、回転する角度を変えてこれを繰り返し、同一の物体であるとみなせる座標の組の数が最も多い角度を求める。
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が今回の走査周期内に観測した複数の物体のなかから選択する物体と、周辺物体観測装置８１１が前回の走査周期内に観測した複数の物体のなかから選択する物体との組み合わせを変えてこれを繰り返し、同一の物体であるとみなせる座標の組の数が最も多い組み合わせと回転角度とを求める。
【００８６】
図１９は、この実施の形態における物体相関部２３０の動作を説明するための図である。
物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が前回の走査周期内に観測した静止している物体の移動体８０１に対する相対位置７４１ａ〜７４１ｆのなかから、例えば相対位置７４１ｃを選択する。物体相関部２３０は、処理装置９１１を用いて、相対位置７４１ｃの座標が原点に一致するよう、相対位置７４１ａ〜７４１ｆの座標を平行移動する。
次に、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が今回の走査周期内に観測した静止している物体の移動体８０１に対する相対位置７４２ａ〜７４２ｆのなかから、例えば相対位置７４２ｃを選択する。物体相関部２３０は、処理装置９１１を用いて、相対位置７４２ｃの座標が原点に一致するよう、相対位置７４２ａ〜７４２ｆの座標を平行移動する。
物体相関部２３０は、処理装置９１１を用いて、平行移動した相対位置７４２ａ〜７４２ｆの座標を、原点を中心として回転する。物体相関部２３０は、処理装置９１１を用いて、回転した相対位置７４２ａ〜７４２ｆの座標と、平行移動した相対位置７４１ａ〜７４１ｆの座標とを比較して、同一の物体であるとみなせる座標の組の数を数える。
【００８７】
物体相関部２３０が選択した相対位置の組み合わせが、周辺物体観測装置８１１が実際に同一の物体を観測した相対位置の組であり、かつ、回転した角度が、移動体８０１が旋回した角度と一致する場合、同一の物体であるとみなせる座標の組の数が最も多くなる。
物体相関部２３０は、処理装置９１１を用いて、同一の物体であるとみなせる座標の組の数が最も多い相対位置の組み合わせを、周辺物体観測装置８１１が同一の物体を観測した相対位置であると判定する。また、そのときに同一の物体であるとみなされた座標の組に対応する相対位置の組み合わせについても、物体相関部２３０は、周辺物体観測装置８１１が同一の物体を観測した相対位置であると判定する。
【００８８】
このように、静止物体判定部２２０が静止していると判定した物体についてのみ、物体相関部２３０が相関処理をするので、移動体８０１の移動軌跡７５１がわからなくても、周辺物体観測装置８１１が観測した物体を相関追尾することができる。
【００８９】
なお、物体相関部２３０は、相対位置の組み合わせを総当たりですべて試してみる構成であってもよいし、移動速度観測装置８２１が観測した移動体８０１の移動速度などの情報を用いて、試してみる相対位置の組み合わせを絞り込む構成であってもよい。
また、物体相関部２３０は、平行移動した座標を回転する角度の範囲を限定せずに試してみる構成であってもよいし、角速度観測装置８３１が観測した移動体８０１の角速度などの情報を用いて、試してみる角度を絞り込む構成であってもよい。
【００９０】
また、物体相関部２３０は、同一の物体であるとみなせる座標の組の数を比較するのではなく、同一の物体であるとみなせる座標の組を頂点とする凸多角形の面積を比較して、面積が最も広い相対位置の組み合わせや回転角度を判定する構成であってもよい。同一の物体であるとみなせる座標の組が狭い範囲に集中している場合、１つの物体の細部である可能性があるからである。
【００９１】
状態推定部２４０（角速度推定部）は、処理装置９１１を用いて、移動体の位置、速度、移動方向、角速度や、移動速度観測装置８２１が観測した移動速度のゲイン誤差、角速度観測装置８３１が観測した角速度のバイアス誤差などの状態量を推定する。状態推定部２４０は、記憶装置９１４を用いて、推定した結果を表わすデータを記憶する。状態推定部２４０は、出力装置９０１を用いて、推定した結果を外部に出力する。
状態推定部２４０は、状態量の推定にあたり、同一の静止している物体を周辺物体観測装置８１１が観測した観測結果であると物体相関部２３０が判定した物体の移動体８０１に対する相対位置を利用する。
なお、１つの物体が複数の走査周期にわたって観測された場合において、その物体が観測された走査周期の数が多いほど、周辺物体観測装置８１１がその物体を観測した移動体８０１に対する相対位置を利用する価値が高い。このため、その物体が観測された走査周期の数が所定の閾値より多い場合のみ、周辺物体観測装置８１１がその物体を観測した移動体８０１に対する相対位置を利用する構成であってもよい。あるいは、その物体が連続して観測された走査周期の数が所定の閾値より多い場合のみ、周辺物体観測装置８１１がその物体を観測した移動体８０１に対する相対位置を利用する構成であってもよい。例えば、状態推定部２４０は、処理装置９１１を用いて、物体相関部２３０が判定した判定結果に基づいて、その物体が（連続して）観測された走査周期の数を算出し、算出した走査周期の数を閾値と比較して、その物体が（連続して）観測された走査周期の数が閾値より多い場合のみ、周辺物体観測装置８１１が観測したその物体の移動体８０１に対する相対位置を利用する。
【００９２】
周辺物体観測装置８１１が観測した物体の移動体に対する相対位置は、移動体の絶対位置を原点とし、移動体の移動方向を基準とした相対座標系における座標と考えることができる。
【００９３】
図２０は、移動体８０１を基準とした相対座標系と、静止点を基準とした絶対座標系との間の関係を説明するための図である。
Ｘ軸及びＹ軸は、移動体８０１を基準とした相対座標系における座標軸である。ｘ軸及びｙ軸は、絶対座標系における座標軸である。なお、絶対座標系の原点は、静止していればどこでもよい。
絶対座標系における移動体８０１の位置を（ｘｓ，ｙｓ）、移動体８０１の移動方向が絶対座標系のｙ軸となす角度をθとすると、相対座標系における座標（Ｘ，Ｙ）と、絶対座標系における座標（ｘ，ｙ）との間には、次の関係がある。
【数２０】
ただし、Ｒθは、角度θの回転変換を表わす行列である。
【００９４】
静止している物体の絶対座標系における座標を（ｘｔ，ｙｔ）、その物体を周辺物体観測装置８１１が観測した移動体に対する相対位置の座標を（ｘｒ，ｙｒ）とし、観測誤差を考慮しなければ、数２０の座標変換をそのまま当てはめることができるから、次の式が成り立つ。
【数２１】
【００９５】
状態推定部２４０は、例えば、拡張カルマンフィルタを用いて、次の式で表されるモデルを使って推定を行う。
【数２２】
ただし、ｘ１は、状態量を表わすベクトルである。ｘｔは、周辺物体観測装置８１１が観測した静止物体の絶対座標系におけるｘ座標である。ｙｔは、周辺物体観測装置８１１が観測した静止物体の絶対座標系におけるｙ座標である。ｘｓは、絶対座標系における移動体のｘ座標である。ｙｓは、絶対座標系における移動体のｙ座標である。ｖは、移動体の移動速度である。θは、移動体の移動方向である。ωは、移動体の角速度である。ｆ１は、状態遷移モデルを表わす関数である。Δｔは、経過時間である。ｚ１は、観測量を表わすベクトルである。ｘｒは、周辺物体観測装置８１１が観測した静止物体の移動体に対する相対位置のＸ座標である。ｙｒは、周辺物体観測装置８１１が観測した静止物体の移動体に対する相対位置のＹ座標である。ｈ１は、観測モデルを表わす関数である。
【００９６】
なお、状態推定部２４０は、拡張カルマンフィルタの観測値として、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の座標（ｘｒ，ｙｒ）だけでなく、移動速度観測装置８２１が観測した移動体の移動速度や角速度観測装置８３１が観測した移動体の角速度を使う構成であってもよい。その場合、状態推定部２４０は、例えば、次の式で表されるモデルを使って推定を行う。
【数２３】
ただし、ｘは、状態量を表わすベクトルである。εｖは、移動速度観測装置８２１が観測する移動体の移動速度のゲイン誤差である。εωは、角速度観測装置８３１が観測する移動体の角速度のバイアス誤差である。ｆは、状態遷移モデルを表わす関数である。ｚは、観測量を表わすベクトルである。ｖｏは、移動速度観測装置８２１が観測した移動体の移動速度である。ωｏは、角速度観測装置８３１が観測した移動体の角速度である。ｘ１、ｆ１、ｚ１、ｈ１は、数２２における定義にしたがう。
【００９７】
なお、周辺物体観測装置８１１が物体を観測する時刻と、移動速度観測装置８２１が移動体の移動速度を観測する時刻と、角速度観測装置８３１が移動体の角速度を観測する時刻とは、それぞれ異なるが、それぞれの観測装置が観測する観測値の誤差の間には相関がない。このため、例えば、それぞれの観測装置が観測する観測値ごとに観測関数を分け、状態推定部２４０は、いずれかの観測装置が観測をするたびに、更新処理をする。
【００９８】
なお、複数の物体の移動体に対する相対位置を、状態量の推定に利用する場合、状態推定部２４０は、複数の物体の絶対座標系における座標を含めるよう拡張した状態量ベクトル（ｘまたはｘ１）を使う構成であってもよい。
しかし、拡張カルマンフィルタで計算する行列の次数が高くなると、計算量が増えるので、処理装置９１１の処理能力が低い場合、リアルタイムでの計算が困難になる場合がある。
そこで、状態推定部２４０は、それぞれの物体について並行して拡張カルマンフィルタを実行する構成としてもよい。その場合、移動体の絶対座標や角速度などの状態量は、複数の拡張カルマンフィルタがそれぞれ推定するので、複数の推定値が得られる。例えば、状態推定部２４０は、処理装置９１１を用いて、複数の拡張カルマンフィルタにより推定された複数の推定値を平均して、移動体の絶対座標や角速度などの状態量の推定値とする。状態推定部２４０は、推定値を平均するにあたり、複数の拡張カルマンフィルタにより算出された複数の誤差共分散行列を用いて、推定精度に応じた重み付けをする構成であってもよい。
【００９９】
図２１は、この実施の形態における角速度推定処理Ｓ５００の流れの一例を示すフローチャート図である。
角速度推定処理Ｓ５００において、角速度推定装置２００は、移動体の角速度などを推定する。角速度推定処理Ｓ５００は、例えば、観測結果取得工程Ｓ５１０と、静止判定工程Ｓ５２０と、物体相関工程Ｓ５３０と、状態推定工程Ｓ５５０とを有する。
【０１００】
観測結果取得工程Ｓ５１０において、相対位置取得部２１１及び速度観測値取得部２１２及び角速度観測値取得部２１３は、それぞれ、周辺物体観測装置８１１及び移動速度観測装置８２１及び角速度観測装置８３１が観測した観測結果を取得する。
静止判定工程Ｓ５２０において、静止物体判定部２２０は、観測結果取得工程Ｓ５１０で取得した観測結果に基づいて、周辺物体観測装置８１１が観測した物体が静止しているか否かを判定する。
物体相関工程Ｓ５３０において、物体相関部２３０は、静止判定工程Ｓ５２０で静止物体判定部２２０が静止していると判定した物体について、相関処理をする。
状態推定工程Ｓ５５０において、状態推定部２４０は、観測結果取得工程Ｓ５１０で取得した観測結果や物体相関工程Ｓ５３０で物体相関部２３０が相関処理した結果に基づいて、移動体の角速度などを推定する。
角速度推定装置２００は、観測結果取得工程Ｓ５１０に処理を戻し、処理を繰り返す。
【０１０１】
図２２は、この実施の形態における観測結果取得工程Ｓ５１０の流れの一例を示すフローチャート図である。
観測結果取得工程Ｓ５１０は、例えば、相対位置取得工程Ｓ５１１と、移動速度取得工程Ｓ５１２と、角速度取得工程Ｓ５１３とを有する。
【０１０２】
相対位置取得工程Ｓ５１１において、相対位置取得部２１１は、処理装置９１１を用いて、周辺物体観測装置８１１が観測結果を表わす信号を出力しているか否かを判定する。周辺物体観測装置８１１が信号を出力していると判定した場合、相対位置取得部２１１は、入力装置９０２を用いて、周辺物体観測装置８１１が出力した信号を入力する。相対位置取得部２１１は、処理装置９１１を用いて、入力した信号が表わす観測結果を取得する。相対位置取得部２１１は、処理装置９１１を用いて、取得した観測結果を表わす周辺物体観測データを生成する。相対位置取得部２１１が生成する周辺物体観測データは、例えば、物体観測時刻データ、相対位置観測値データ、相対速度観測値データを含む。
物体観測時刻データは、周辺物体観測装置８１１がその物体を観測した時刻を表わす。例えば、物体観測時刻データは、所定の時刻からの経過時間をミリ秒単位で表わす整数値データである。あるいは、物体観測時刻データは、フレームの番号を表わす整数値データと、そのフレームの開始時刻からの経過時間をミリ秒単位で表わす整数値データとの組である。なお、１つのフレームは、周辺物体観測装置８１１による１回の走査周期である。１つのフレームの長さは、例えば０．２秒である。
相対位置観測値データは、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置を表わす。例えば、相対位置観測値データは、移動体を原点、移動体の進行方向をＹ軸、移動体の進行方向に対して直角右方向をＸ軸とする移動体基準の相対座標系において、周辺物体観測装置８１１が観測した物体の座標をメートル単位で表わす２つの実数値データの組である。あるいは、相対位置観測値データは、周辺物体観測装置８１１が観測した物体と移動体との間の距離をメートル単位で表わす実数値データと、移動体から見て周辺物体観測装置８１１が観測した物体が見える方向と移動体の進行方向とがなす角度をラジアン単位で表わす実数値データとの組である。
相対速度観測値データは、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度を表わす。例えば、相対速度観測値データは、移動体基準の相対座標系において、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度ベクトルの成分をメートル毎秒単位で表わす２つの実数値データの組である。あるいは、相対速度観測値データは、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度の物体と移動体との距離方向成分をメートル毎秒単位で表わす実数値データである。
相対位置取得部２１１は、記憶装置９１４を用いて、生成した周辺物体観測データを記憶する。
【０１０３】
移動速度取得工程Ｓ５１２において、速度観測値取得部２１２は、処理装置９１１を用いて、移動速度観測装置８２１が観測結果を表わす信号を出力しているか否かを判定する。移動速度観測装置８２１が信号を出力している場合、速度観測値取得部２１２は、入力装置９０２を用いて、移動速度観測装置８２１が出力した信号を入力する。速度観測値取得部２１２は、処理装置９１１を用いて、入力した信号が表わす観測結果を取得する。速度観測値取得部２１２は、処理装置９１１を用いて、取得した観測結果を表わす移動速度データを生成する。速度観測値取得部２１２が生成する移動速度データは、例えば、移動速度観測時刻データと、移動速度観測値データとを含む。
移動速度観測時刻データは、移動速度観測装置８２１が移動体の移動速度を観測した時刻を表わす。例えば、移動速度観測時刻データは、所定の時刻からの経過時間をミリ秒単位で表わす整数値データである。
移動速度観測値データは、移動速度観測装置８２１が観測した移動体の移動速度を表わす。例えば、移動速度観測値データは、移動速度観測装置８２１が観測した移動体の移動速度をメートル毎秒単位で表わす実数値データである。
速度観測値取得部２１２は、記憶装置９１４を用いて、生成した移動速度データを記憶する。
【０１０４】
角速度取得工程Ｓ５１３において、角速度観測値取得部２１３は、処理装置９１１を用いて、角速度観測装置８３１が観測結果を表わす信号を出力しているか否かを判定する。角速度観測装置８３１が信号を出力している場合、角速度観測値取得部２１３は、入力装置９０２を用いて、角速度観測装置８３１が出力した信号を入力する。角速度観測値取得部２１３は、処理装置９１１を用いて、入力した信号が表わす観測結果を取得する。角速度観測値取得部２１３は、処理装置９１１を用いて、取得した観測結果を表わす角速度データを生成する。角速度観測値取得部２１３が生成する角速度データは、例えば、角速度観測時刻データと、角速度観測値データとを含む。
角速度観測時刻データは、角速度観測値取得部２１３が移動体の角速度を観測した時刻を表わす。例えば、角速度観測時刻データは、所定の時刻からの経過時間をミリ秒単位で表わす整数値データである。
角速度観測値データは、角速度観測値取得部２１３が観測した移動体の角速度を表わす。例えば、角速度観測値データは、角速度観測装置８３１が観測した移動体の角速度をラジアン毎秒単位で表わす実数値データである。
角速度観測値取得部２１３は、記憶装置９１４を用いて、生成した角速度データを記憶する。
【０１０５】
なお、物体観測時刻データと、移動速度観測時刻データと、角速度観測時刻データとにおける観測時刻の表現形式は、相互に変換が可能であれば、異なる形式であってもよい。
【０１０６】
図２３は、この実施の形態における静止判定工程Ｓ５２０の流れの一例を示すフローチャート図である。
静止判定工程Ｓ５２０は、例えば、移動速度観測時刻取得工程Ｓ５２１と、物体選択工程Ｓ５２２と、観測時刻比較工程Ｓ５２３と、速度比較工程Ｓ５２４とを有する。
【０１０７】
移動速度観測時刻取得工程Ｓ５２１において、静止物体判定部２２０は、処理装置９１１を用いて、移動速度取得工程Ｓ５１２で速度観測値取得部２１２が記憶した移動速度データのうち、観測時刻が最も新しい移動速度データを取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した移動速度データに含まれる移動速度観測時刻データから、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻を取得する。また、静止物体判定部２２０は、処理装置９１１を用いて、取得した移動速度データに含まれる移動速度観測値データから、移動速度観測装置８２１が観測した移動体の移動速度を取得する。
なお、静止判定工程Ｓ５２０を前回実行した後に移動速度観測装置８２１が新しい移動速度を観測していない場合、静止物体判定部２２０は、以降の処理をせず、静止判定工程Ｓ５２０を終了する構成であってもよい。例えば、静止物体判定部２２０は、処理装置９１１を用いて、移動速度観測時刻取得工程Ｓ５２１を前回実行したときに取得した最新の観測時刻と、今回取得した最新の観測時刻とを比較する。観測時刻が同じ場合、静止物体判定部２２０は、静止判定工程Ｓ５２０を終了する。
【０１０８】
物体選択工程Ｓ５２２において、静止物体判定部２２０は、処理装置９１１を用いて、相対位置取得工程Ｓ５１１で相対位置取得部２１１が記憶した周辺物体観測データのなかから、その物体が静止しているか否かをまだ判定していない周辺物体観測データを選択する。
例えば、後述する速度比較工程Ｓ５２４において、静止物体判定部２２０は、記憶装置９１４を用いて、ある周辺物体観測データについて静止しているか否かを判定した場合に、その判定結果を表わす静止判定結果データを、その周辺物体観測データに対応づけて記憶しておく。
静止物体判定部２２０は、処理装置９１１を用いて、ある周辺物体観測データについて、対応づけられた静止判定結果データを記憶しているか否かを判定することにより、その周辺物体観測データについて静止しているか否かを判定済である否かを判定する。
選択すべき周辺物体観測データが存在しない場合、静止物体判定部２２０は、処理装置９１１を用いて、静止判定工程Ｓ５２０を終了する。
選択すべき周辺物体観測データが１つ以上存在する場合、静止物体判定部２２０は、処理装置９１１を用いて、そのなかから周辺物体観測データを１つ選択する。静止物体判定部２２０は、処理装置９１１を用いて、観測時刻比較工程Ｓ５２３へ処理を進める。
【０１０９】
観測時刻比較工程Ｓ５２３において、静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２で選択した周辺物体観測データに含まれる物体観測時刻データから、周辺物体観測装置８１１がその物体を観測した観測時刻を取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した観測時刻と、移動速度観測時刻取得工程Ｓ５２１で取得した観測時刻とを比較する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも後である場合、静止物体判定部２２０は、まだ、その周辺物体観測データについて静止しているか否かの判定をしない。静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２に処理を戻し、次の周辺物体観測データを選択する。
周辺物体観測装置８１１が物体を観測した観測時刻が、移動速度観測装置８２１が移動体の移動速度を観測した最新の観測時刻よりも前である場合、静止物体判定部２２０は、その周辺物体観測データについて静止しているか否かの判定をする。静止物体判定部２２０は、処理装置９１１を用いて、速度比較工程Ｓ５２４へ処理を進める。
【０１１０】
速度比較工程Ｓ５２４において、静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２で選択した周辺物体観測データに含まれる相対速度データから、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度を取得する。静止物体判定部２２０は、処理装置９１１を用いて、取得した相対速度と、移動速度観測時刻取得工程Ｓ５２１で取得した移動体の移動速度とに基づいて、その物体が静止しているか否かを判定する。静止物体判定部２２０は、処理装置９１１を用いて、判定した判定結果を表わす静止判定データを生成する。静止物体判定部２２０は、記憶装置９１４を用いて、生成した静止判定データを、物体選択工程Ｓ５２２で選択した周辺物体観測データに対応づけて記憶する。
静止物体判定部２２０は、処理装置９１１を用いて、物体選択工程Ｓ５２２に処理を戻し、次の周辺物体観測データを選択する。
【０１１１】
図２４は、この実施の形態における物体相関工程Ｓ５３０の流れの一例を示すフローチャート図である。
物体相関工程Ｓ５３０は、前回静止物体取得工程Ｓ５３１と、今回静止物体取得工程Ｓ５３２と、前回物体選択工程Ｓ５３３と、前回物体平行移動工程Ｓ５３４と、今回物体選択工程Ｓ５３５と、今回物体平行移動工程Ｓ５３６と、回転角度選択工程Ｓ５３７と、今回物体回転工程Ｓ５３８と、距離算出工程Ｓ５３９と、同一物体判定工程Ｓ５４０と、閾値判定工程Ｓ５４１と、同一物体記憶工程Ｓ５４２とを有する。
【０１１２】
前回静止物体取得工程Ｓ５３１において、物体相関部２３０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、１つ前のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データであって、その物体が静止していると静止判定工程Ｓ５２０で静止物体判定部２２０が判定した周辺物体観測データをすべて取得する。
物体相関部２３０は、処理装置９１１を用いて、取得した周辺物体観測データそれぞれについて、２つ以上前のフレームにおける周辺物体観測データとの間で、周辺物体観測装置８１１が同一の物体を観測したものであると物体相関部２３０が判定した周辺物体観測データがあるフレームの数を求める。
例えば、後述する同一物体記憶工程Ｓ５４２において、物体相関部２３０は、処理装置９１１を用いて、あるフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データが、それより前のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データと同一の物体を観測したものであると判定した場合、相関回数を表わす相関回数データを生成する。相関回数は、同一の物体と判定したフレームの数である。物体相関部２３０は、記憶装置９１４を用いて、その周辺物体観測データに対応づけて相関回数データを記憶する。同一の物体を観測したものであると判定した相手の周辺物体観測データに対応づけられた相関回数データがない場合、物体相関部２３０は、相関回数を「１」とした相関回数データを生成する。同一の物体を観測したものであると判定した相手の周辺物体観測データに対応づけられた相関回数データがある場合、物体相関部２３０は、その相関回数データが表わす相関回数に「１」を加えたものを相関回数とする相関回数データを生成する。
物体相関部２３０は、処理装置９１１を用いて、取得した周辺物体観測データに対応づけて記憶した相関回数データが表わす相関回数を取得することにより、同一の物体と判定したフレームの数を求める。
物体相関部２３０は、処理装置９１１を用いて、取得したすべての周辺物体観測データを、取得したフレームの数が多い順に並べる。相関の取れたフレームの数が多いほど、その周辺物体観測データがノイズなどによる誤検出である可能性が低く、また、移動している物体を誤って静止していると認識したものである可能性も低い。このため、今回のフレームにおける周辺物体観測データとも相関が取れる可能性が高い。あらかじめ相関が取れる可能性が高い順に周辺物体観測データを並べておくことにより、物体相関工程Ｓ５３０の計算量を抑えることができる。
【０１１３】
今回静止物体取得工程Ｓ５３２において、物体相関部２３０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、最新のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データであって、その物体が静止していると静止判定工程Ｓ５２０で静止物体判定部２２０が判定した周辺物体観測データをすべて取得する。
【０１１４】
前回物体選択工程Ｓ５３３において、物体相関部２３０は、処理装置９１１を用いて、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データのなかから、前回静止物体取得工程Ｓ５３１で並べだ順に、周辺物体観測データを１つ選択する。
前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データがすべて選択済であり、選択すべき周辺物体観測データが存在しない場合、物体相関部２３０は、処理装置９１１を用いて、同一物体記憶工程Ｓ５４２へ処理を進める。
【０１１５】
未選択の周辺物体観測データが存在する場合、物体相関部２３０は、処理装置９１１を用いて、未選択の周辺物体観測データを１つ選択する。物体相関部２３０は、処理装置９１１を用いて、選択した周辺物体観測データと同一の物体を今回のフレームで周辺物体観測装置８１１が観測する相対位置を予測する。例えば、物体相関部２３０は、処理装置９１１を用いて、１つ前のフレームまでの間に相関の取れた物体の移動体に対する相対位置の軌跡や、状態推定部２４０が推定した移動体の角速度などの状態量に基づいて、予測を行う。
【０１１６】
前回物体平行移動工程Ｓ５３４において、物体相関部２３０は、処理装置９１１を用いて、前回静止物体取得工程Ｓ５３１で取得したすべての周辺物体観測データについて、その周辺物体観測データに含まれる相対位置観測値データが表わす周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の座標から、今回静止物体取得工程Ｓ５３２で選択した周辺物体観測データに含まれる相対位置観測値データが表わすその物体の移動体に対する相対位置の座標を差し引くことにより、物体の座標を平行移動する。物体相関部２３０は、記憶装置９１４を用いて、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データそれぞれについて、平行移動した座標を表わすデータを記憶する。
【０１１７】
今回物体選択工程Ｓ５３５において、物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データを、その周辺物体観測データに含まれる相対位置観測値データが表わす周辺物体観測装置８１１が観測した物体の移動体に対する相対位置が、前回物体選択工程Ｓ５３３で予測した相対位置に近い順に並べる。予測した相対位置との距離が短いほうが同一の物体を観測した観測結果である可能性が高いからである。なお、物体相関部２３０は、予測した相対位置との距離が所定の閾値より短いものだけを抽出して、近い順に並べる構成であってもよい。
物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データ（もしくは、前回物体選択工程Ｓ５３３で予測した相対位置との距離が所定の閾値より短いものだけを抽出した周辺物体観測データ）のなかから、前回物体選択工程Ｓ５３３で予測した相対位置との距離が近い順に、周辺物体観測データを１つ選択する。
今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データ（もしくは、前回物体選択工程Ｓ５３３で予測した相対位置との距離が所定の閾値より短いものだけを抽出した周辺物体観測データ）がすべて選択済であり、選択すべき周辺物体観測データが存在しない場合、物体相関部２３０は、処理装置９１１を用いて、前回物体選択工程Ｓ５３３に処理を戻し、前回静止物体取得工程Ｓ５３１で選択した周辺物体観測データのなかから次の周辺物体観測データを選択する。
未選択の周辺物体観測データが存在する場合、物体相関部２３０は、処理装置９１１を用いて、未選択の周辺物体観測データを１つ選択し、今回物体平行移動工程Ｓ５３６へ処理を進める。
【０１１８】
今回物体平行移動工程Ｓ５３６において、物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得したすべての周辺物体観測データについて、その周辺物体観測データに含まれる相対位置観測値データが表わす周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の座標から、今回物体選択工程Ｓ５３５で選択した周辺物体観測データに含まれる相対位置観測値データが表わすその物体の移動体に対する相対位置の座標を差し引くことにより、物体の座標を平行移動する。物体相関部２３０は、記憶装置９１４を用いて、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データそれぞれについて、平行移動した座標を表わすデータを記憶する。
【０１１９】
回転角度選択工程Ｓ５３７において、物体相関部２３０は、前回物体選択工程Ｓ５３３で選択した周辺物体観測データと、今回物体選択工程Ｓ５３５で選択した周辺物体観測データとが、同一の物体を観測した観測結果を表わす周辺物体観測データであると仮定して、処理装置９１１を用いて、移動体の旋回角度を推定する。物体相関部２３０は、状態推定部２４０が推定した状態量や、角速度観測装置８３１が観測した角速度などを、旋回角度の推定に利用する構成であってもよい。物体相関部２３０は、処理装置９１１を用いて、推定した旋回角度に基づいて、試してみる複数の回転角度を決定する。
物体相関部２３０は、処理装置９１１を用いて、決定した複数の回転角度のなかから、推定した旋回角度に近い順に、回転角度を１つ選択する。
決定した複数の回転角度をすべて選択済であり、選択すべき回転角度が存在しない場合、物体相関部２３０は、処理装置９１１を用いて、今回物体選択工程Ｓ５３５に処理を戻し、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データのなかから次の周辺物体観測データを選択する。
未選択の回転角度が存在する場合、物体相関部２３０は、処理装置９１１を用いて、未選択の回転角度を１つ選択し、今回物体回転工程Ｓ５３８へ処理を進める。
【０１２０】
今回物体回転工程Ｓ５３８において、物体相関部２３０は、処理装置９１１を用いて、今回静止物体取得工程Ｓ５３２で取得したすべての周辺物体観測データについて今回物体平行移動工程Ｓ５３６で平行移動した座標を、原点を中心として、回転角度選択工程Ｓ５３７で選択した回転角度だけ回転移動する。
【０１２１】
距離算出工程Ｓ５３９において、物体相関部２３０は、処理装置９１１を用いて、前回静止物体取得工程Ｓ５３１で取得したすべての周辺物体観測データと、今回静止物体取得工程Ｓ５３２で取得したすべての周辺物体観測データとのすべての組み合わせについて、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データについて前回物体平行移動工程Ｓ５３４で平行移動した座標と、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データについて今回物体回転工程Ｓ５３８で回転移動した座標との間の距離を算出する。物体相関部２３０は、処理装置９１１を用いて、算出した距離が近い順に、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データと、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データとのペアを生成する。
【０１２２】
同一物体判定工程Ｓ５４０において、物体相関部２３０は、処理装置９１１を用いて、距離算出工程Ｓ５３９で生成したペアのうち、算出した距離が所定の閾値より短いペアの数を数える。物体相関部２３０は、処理装置９１１を用いて、数えたペア数を、物体相関工程Ｓ５３０を今回実行したなかで、それまでに同一物体判定工程Ｓ５４０で数えたペア数の最大値と比較する。
今回数えたペア数がそれまでのペア数の最大値より小さい場合、物体相関部２３０は、回転角度選択工程Ｓ５３７に処理を戻し、次の回転角度を選択する。
今回数えたペア数がそれまでのペア数の最大値より大きい場合、物体相関部２３０は、記憶装置９１４を用いて、距離算出工程Ｓ５３９で生成したペアのうち算出した距離が閾値より短いペアと、数えたペア数とを表わすデータを記憶する。物体相関部２３０は、処理装置９１１を用いて、閾値判定工程Ｓ５４１へ処理を進める。
【０１２３】
閾値判定工程Ｓ５４１において、物体相関部２３０は、処理装置９１１を用いて、距離算出工程Ｓ５３９で数えたペア数を所定の閾値と比較する。
数えたペア数が閾値より小さい場合、物体相関部２３０は、処理装置９１１を用いて、回転角度選択工程Ｓ５３７に処理を戻し、次の回転角度を選択する。
数えたペア数が閾値より大きい場合、物体相関部２３０は、処理装置９１１を用いて、同一物体記憶工程Ｓ５４２へ処理を進める。相関が取れる可能性が高い順に試行するので、早い段階で正しい組み合わせと回転角度が見つかる可能性が高く、それ以降の試行をしないことにより、計算量を抑えるためである。
なお、比較の対象となる閾値は、あらかじめ定めた定数であってもよい。また、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データの数や、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データの数に基づいて、物体相関部２３０が閾値を算出する構成であってもよい。例えば、物体相関部２３０は、前回静止物体取得工程Ｓ５３１で取得した周辺物体観測データの数と、今回静止物体取得工程Ｓ５３２で取得した周辺物体観測データの数とを比較して小さいほうの数を求め、求めた数に所定の定数（例えば０．６）を乗じて、閾値を算出する。
【０１２４】
同一物体記憶工程Ｓ５４２において、物体相関部２３０は、処理装置９１１を用いて、同一物体判定工程Ｓ５４０で記憶したデータが表わすペアが、同一の物体を周辺物体観測装置８１１が観測した観測結果を表わす周辺物体観測データであると判定する。物体相関部２３０は、処理装置９１１を用いて、判定した結果を表わす相関結果データとして、上述した相関回数データを生成する。また、物体相関部２３０は、相関結果データとして、処理装置９１１を用いて、どの周辺物体観測データが、同一の物体を周辺物体観測装置８１１が観測した観測結果を表わす周辺物体観測データであるかを表わすデータを生成する。例えば、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が観測した物体に番号を振る。物体相関部２３０は、記憶装置９１４を用いて、振った番号を表わすデータを、周辺物体観測データに対応づけて記憶する。物体相関部２３０は、１つ前のフレームにおける周辺物体観測データと同一の物体を周辺物体観測装置８１１が観測した観測結果を表わすと判定した周辺物体観測データには、１つ前のフレームにおけるその周辺物体観測データと同一の番号を振る。物体相関部２３０は、それ以外の周辺物体観測データには、いずれの周辺物体観測データとも異なる番号を振る。
【０１２５】
図２５は、この実施の形態における状態推定工程Ｓ５５０の流れの一例を示すフローチャート図である。
状態推定工程Ｓ５５０は、相関物体選択工程Ｓ５５１と、相関回数判定工程Ｓ５５２と、予測工程Ｓ５５３と、更新工程Ｓ５５４と、平均工程Ｓ５５５とを有する。
【０１２６】
相関物体選択工程Ｓ５５１において、状態推定部２４０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、最新のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データであって、それより前のフレームにおいて周辺物体観測装置８１１が観測した物体についての周辺物体観測データと同一の物体を観測した観測結果を表わすと物体相関工程Ｓ５３０で物体相関部２３０が判定した周辺物体観測データを１つ選択する。例えば、状態推定部２４０は、処理装置９１１を用いて、相対位置取得部２１１が記憶した周辺物体観測データのなかから、対応づけられた相関回数データを物体相関部２３０が記憶した周辺物体観測データを選択する。
相関が取れた周辺物体観測データがすべて選択済であり、選択すべき周辺物体観測データが存在しない場合、状態推定部２４０は、処理装置９１１を用いて、平均工程Ｓ５５５へ処理を進める。
相関が取れた周辺物体観測データのなかに未選択の周辺物体観測データが存在する場合、状態推定部２４０は、処理装置９１１を用いて、未選択の周辺物体観測データを１つ選択し、相関回数判定工程Ｓ５５２へ処理を進める。
【０１２７】
相関回数判定工程Ｓ５５２において、状態推定部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１で選択した周辺物体観測データの相関回数を所定の閾値と比較する。例えば、状態推定部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１で選択した周辺物体観測データに対応づけて物体相関部２３０が記憶した相関回数データを取得する、状態推定部２４０は、処理装置９１１を用いて、取得した相関回数データが表わす相関回数を閾値と比較する。
相関回数が閾値より小さい場合、状態推定部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１に処理を戻し、次の周辺物体観測データを選択する。
相関回数が閾値より大きい場合、状態推定部２４０は、処理装置９１１を用いて、予測工程Ｓ５５３へ処理を進める。
【０１２８】
予測工程Ｓ５５３において、状態推定部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１で選択した周辺物体観測データが表わす物体の絶対位置を状態量に含め、状態遷移関数を使って、相関物体選択工程Ｓ５５１で選択した周辺物体観測データに含まれる物体観測時刻データが表わす観測時刻における状態量を予測する。
【０１２９】
更新工程Ｓ５５４において、状態推定部２４０は、処理装置９１１を用いて、相関物体選択工程Ｓ５５１で選択した周辺物体観測データに含まれる物体の移動体に対する相対位置を観測量として、例えば拡張カルマンフィルタを使って、状態量の推定値を更新する。状態推定部２４０は、記憶装置９１４を用いて、更新した状態量の推定値や誤差共分散行列を表わすデータを記憶する。
【０１３０】
平均工程Ｓ５５５において、状態推定部２４０は、処理装置９１１を用いて、相関の取れた周辺物体観測データそれぞれについて更新工程Ｓ５５４で推定した状態量の推定値を平均して、全体としての状態量の推定値を算出する。
【０１３１】
実施の形態６．
実施の形態６について、図２６を用いて説明する。
なお、実施の形態５と共通する部分については、同一の符号を付し、説明を省略する。
【０１３２】
この実施の形態における角速度推定装置２００のハードウェア構成や機能ブロックの構成は、実施の形態５と同様なので、ここでは説明を省略する。
【０１３３】
状態推定部２４０は、物体相関部２３０が相関処理をした物体の移動体に対する相対位置を観測値として入力するのではなく、物体相関部２３０が相関処理をする際に算出した平行移動量や回転角度を観測値として入力して、移動体の角速度などの状態量を推定する。
【０１３４】
図２６は、物体相関部２３０が相関処理に用いる平行移動量および回転角度と、移動体８０１の移動量の関係を説明するための図である。
物体相関部２３０は、あるフレームにおいて周辺物体観測装置８１１が観測した静止物体の移動体８０１に対する相対位置７４１ａ〜７４１ｆと、別のフレームにおいて周辺物体観測装置８１１が観測した静止物体の移動体８０１に対する相対位置７４２ａ〜７４２ｆとの相関を取ったとする。物体相関部２３０は、ベクトル７５５を用いて相対位置７４１ａ〜７４１ｆの座標を平行移動して、相対位置７４１ｃの座標を原点に合わせる。また、物体相関部２３０は、ベクトル７５６を用いて相対位置７４２ａ〜７４２ｆの座標を平行移動して、相対位置７４２ｃの座標を原点に合わせ、回転角度７５７だけ回転移動することにより、相関が取れたとする。
【０１３５】
この場合、ベクトル７５６を回転角度７５７だけ回転したベクトル７５８から、ベクトル７５５を差し引いた差ベクトル７５９が、その２つのフレームの間における移動体基準の相対座標系における移動体８０１の移動量である。
また、回転角度７５７は、その２つのフレームの間における移動体８０１の旋回角度である。
【０１３６】
例えば、物体相関部２３０は、図２４で説明した同一物体判定工程Ｓ５４０で、今回数えたペア数がそれまでのペア数の最大値より大きい場合、記憶装置９１４を用いて、前回物体選択工程Ｓ５３３で選択した周辺物体観測データと、今回物体選択工程Ｓ５３５で選択した周辺物体観測データと、回転角度選択工程Ｓ５３７で選択した回転角度を表わすデータとを記憶する。
状態推定部２４０は、物体相関部２３０が記憶した２つの周辺物体観測データと、回転角度を表わすデータとに基づいて、移動体の移動量および旋回角度の観測値を算出する。例えば、状態推定部２４０は、処理装置９１１を用いて、今回物体選択工程Ｓ５３５で物体相関部２３０が選択した周辺物体観測データから、最新のフレームにおいて周辺物体観測装置８１１が観測したその物体の移動体に対する相対位置を取得する。状態推定部２４０は、処理装置９１１を用いて、取得した相対位置を表わす座標を、原点を中心として、回転角度選択工程Ｓ５３７で物体相関部２３０が選択した回転角度だけ回転移動する。状態推定部２４０は、処理装置９１１を用いて、前回物体選択工程Ｓ５３３で物体相関部２３０が選択した周辺物体観測データから、１つ前のフレームにおいて周辺物体観測装置８１１が観測したその物体の移動体に対する相対位置を取得する。状態推定部２４０は、処理装置９１１を用いて、取得した相対位置を表わす座標から、回転移動した座標を差し引いた差を算出して、移動体の移動量の観測値とする。また、状態推定部２４０は、処理装置９１１を用いて、回転角度選択工程Ｓ５３７で物体相関部２３０が選択した回転角度を、移動体の旋回角度の観測値とする。
【０１３７】
状態推定部２４０は、処理装置９１１を用いて、このようにして算出した移動体の移動量および旋回角度の観測値を、例えば拡張カルマンフィルタの入力として推定値の更新処理を行う。その場合、状態推定部２４０は、例えば、次の式で表されるモデルを使って推定を行う。
【数２４】
ただし、ｘ２は、状態量を表わすベクトルである。ｆ２は、状態遷移モデルを表わす関数である。ｚ２は、観測量を表わすベクトルである。Δｘは、状態推定部２４０が算出した移動体の移動量の観測値の横方向成分である。Δｙは、状態推定部２４０が算出した移動体の移動量の観測値の縦方向成分である。Δθは、状態推定部２４０が算出した移動体の旋回角度の観測値である。ｈ２は、観測モデルを表わす関数である。
【０１３８】
ここで、状態量を表わすベクトルｘ２は、静止物体の絶対座標系における座標を含まない。したがって、状態推定部２４０は、静止物体ごとに推定をする必要はなく、全体で１つの推定を行えばよい。
【０１３９】
なお、状態推定部２４０は、数２３のｘ１、ｆ１、ｚ１、ｈ１を、数２４のｘ２、ｆ２、ｚ２、ｈ２で置き換えたモデルを使って推定をする構成であってもよい。
【０１４０】
実施の形態７．
実施の形態７について、図２７を用いて説明する。
なお、実施の形態５〜実施の形態６と共通する部分については、同一の符号を付し、説明を省略する。
【０１４１】
図２７は、この実施の形態における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図である。
【０１４２】
周辺物体観測装置８１１は、物体の移動体に対する相対速度を観測せず、物体の移動体に対する相対位置のみを観測する。
実施の形態５では、周辺物体観測装置８１１が観測した物体の移動体に対する相対速度と、移動速度観測装置８２１が観測した移動体の移動速度とに基づいて、静止物体判定部２２０は、物体が静止しているか否かを判定する。これに対し、この実施の形態の周辺物体観測装置８１１は物体の移動体に対する相対速度を観測しないので、静止物体判定部２２０は、処理装置９１１を用いて、物体相関部２３０が相関を取った結果として得られる物体の移動体に対する相対位置の軌跡に基づいて物体の移動体に対する相対速度を算出する。静止物体判定部２２０は、処理装置９１１を用いて、算出した物体の移動体に対する相対速度と、移動速度観測装置８２１が観測した移動体の移動速度とに基づいて、物体が静止しているか否かを判定する。
【０１４３】
また、物体が静止しているか否かを静止物体判定部２２０が判定する前に、物体相関部２３０が相関処理をする必要があるので、物体相関部２３０は、少なくとも初期の段階において、静止物体判定部２２０の判定結果を利用することができない。このため、物体相関部２３０は、処理装置９１１を用いて、とりあえず、周辺物体観測装置８１１が観測したすべての物体について、相関処理を実施する。なお、物体相関部２３０による相関処理の結果に基づいて、物体が静止しているか否かを静止物体判定部２２０が判定したのち、静止していないと静止物体判定部２２０が判定した物体について、物体相関部２３０は、相関処理を打ち切る構成であってもよい。
物体相関部２３０は、静止しているか否か不明である物体について相関処理を実施するため、状態推定部２４０が推定した移動体の位置や向きを利用する。例えば、物体相関部２３０は、処理装置９１１を用いて、周辺物体観測装置８１１が物体を観測した観測時刻における移動体の位置や向きに基づいて、周辺物体観測装置８１１が観測した物体の移動体に対する相対位置の座標を、絶対座標系における座標に変換する。物体相関部２３０は、処理装置９１１を用いて、変換した座標に基づいて、物体の相関処理をする。
【０１４４】
これにより、周辺物体観測装置８１１が物体の移動体に対する相対速度を観測しない構成である場合でも、移動体の角速度などを推定することができる。
【０１４５】
実施の形態８．
実施の形態８について、図２８を用いて説明する。
なお、実施の形態１〜実施の形態７と共通する部分については、同一の符号を付し、説明を省略する。
【０１４６】
図２８は、この実施の形態における角速度推定装置２００の機能ブロックの構成の一例を示すブロック構成図である。
角速度推定装置２００は、実施の形態６で説明した機能ブロックに加えて、更に、角速度誤差記憶部２５１と、角速度補正部２５２と、角速度検定部２５３と、角速度誤差算出部２５４とを有する。
【０１４７】
状態推定部２４０は、角速度観測装置８３１が観測した移動体の角速度を使わず、それ以外の観測値に基づいて、状態量を推定する。
【０１４８】
角速度誤差記憶部２５１は、記憶装置９１４を用いて、バイアス誤差など角速度観測装置８３１が観測する角速度の誤差を表わすデータを記憶する。角速度誤差記憶部２５１が記憶するデータを「角速度誤差データ」と呼ぶ。最初の段階では、角速度観測装置８３１が観測する角速度の誤差は不明であるから、例えば、誤差がないものと仮定し、角速度誤差記憶部２５１は、記憶装置９１４を用いて、「０」を表わすデータを、角速度誤差データとして記憶しておく構成であってもよい。
【０１４９】
角速度補正部２５２は、処理装置９１１を用いて、角速度観測装置８３１が観測した移動体の角速度の誤差を補正する。角速度補正部２５２は、記憶装置９１４を用いて、算出した角速度を表わすデータを記憶する。角速度補正部２５２が記憶するデータを「補正済角速度データ」と呼ぶ。
例えば、角速度補正部２５２は、処理装置９１１を用いて、角速度観測値取得部２１３が記憶した移動体角速度データと、角速度誤差記憶部２５１が記憶した角速度誤差データとを入力する。角速度補正部２５２は、処理装置９１１を用いて、入力した移動体角速度データから、角速度観測値データを取得する。角速度補正部２５２は、処理装置９１１を用いて、取得した角速度観測値データが表わす角速度から、入力した角速度誤差データが表わす誤差を差し引いた差を計算して、補正した角速度を算出する。
【０１５０】
角速度検定部２５３は、処理装置９１１を用いて、角速度補正部２５２が補正した角速度が正しいか否かを検定する。角速度検定部２５３は、記憶装置９１４を用いて、検定した結果を表わすデータを記憶する。角速度検定部２５３が記憶するデータを「角速度検定結果データ」と呼ぶ。
例えば、角速度検定部２５３は、処理装置９１１を用いて、状態推定部２４０が推定した状態量に基づいて、角速度補正部２５２が補正した角速度が正しいか否かを検定する。
【０１５１】
角速度誤差算出部２５４は、処理装置９１１を用いて、角速度観測装置８３１が観測した移動体の角速度の誤差を算出する。角速度誤差算出部２５４は、記憶装置９１４を用いて、算出した誤差を表わすデータを記憶する。角速度誤差算出部２５４が記憶するデータを「角速度誤差推定データ」と呼ぶ。
例えば、角速度誤差算出部２５４は、処理装置９１１を用いて、状態推定部２４０が推定した状態量を表わすデータと、角速度観測値取得部２１３が記憶した移動体角速度データとを入力する。角速度誤差算出部２５４は、処理装置９１１を用いて、入力したデータから、状態推定部２４０が推定した移動体の角速度を表わすデータを取得する。角速度誤差算出部２５４は、処理装置９１１を用いて、入力した移動体角速度データから、角速度観測値データを取得する。角速度誤差算出部２５４は、処理装置９１１を用いて、取得したデータが表わす移動体の角速度の推定値から、取得した角速度観測値データが表わす角速度の観測値を差し引いた差を計算して、角速度の誤差を算出する。
【０１５２】
角速度補正部２５２が補正した角速度が正しくないと角速度検定部２５３が判定した場合、角速度誤差記憶部２５１は、記憶した角速度誤差データを更新し、角速度誤差算出部２５４が記憶した角速度誤差推定データを、新たな角速度誤差データとして、記憶装置９１４を用いて記憶する。
【０１５３】
次に、角速度検定部２５３による検定処理について説明する。
【０１５４】
例えば、角速度検定部２５３は、状態推定部２４０が推定した状態量を使って、物体の位置を予測する。このとき、移動体の角速度だけは、状態推定部２４０が推定した状態量ではなく、角速度補正部２５２が補正した角速度を使う。これにより予測精度が落ちた場合、角速度検定部２５３は、角速度補正部２５２が補正した角速度が正しくないと判定する。
【０１５５】
状態推定部２４０が推定した角速度を、角速度補正部２５２が補正した角速度で置き換えるためには、状態推定部２４０が推定した状態量の時刻と、角速度観測装置８３１が移動体の角速度を観測した時刻とが一致する必要がある。状態推定部２４０は、周辺物体観測装置８１１が物体を観測した場合や、移動速度観測装置８２１が移動体の移動速度を観測した場合に、状態量の推定値を更新する。したがって、周辺物体観測装置８１１が物体を観測した時刻や、移動速度観測装置８２１が移動体の移動速度を観測した時刻が、状態推定部２４０が推定した状態量の時刻になる。
ここで、角速度観測装置８３１が移動体の角速度を観測した時刻をｔ１とする。周辺物体観測装置８１１が物体を観測した時刻または移動速度観測装置８２１が移動体の移動速度を観測した時刻をｔ２とする。なお、時刻ｔ２よりも時刻ｔ１のほうが、後の時刻であるものとする。
【０１５６】
角速度検定部２５３は、例えば、時刻ｔ２における状態量の推定値に基づいて外挿処理を行い、時刻ｔ１における状態量の推定値を算出する。
例えば、角速度検定部２５３は、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔ１における状態量の推定値を算出する。
【数２５】
ただし、山型付きのｘ（ｘハット。以下「ｘ＾」と表記する。）は、推定値ベクトルである。推定値ベクトルｘ＾は、ｋ次の列ベクトルである。推定値ベクトルｘ＾は、時刻ｔの関数である。推定値ベクトルｘ＾の要素は、時刻ｔにおける状態量の推定値である。推定値ベクトルｘ＾の次数ｋは、推定値の数である。
Ｆは、状態遷移行列である。状態遷移行列Ｆは、ｋ次の正方行列である。状態遷移行列Ｆは、時間Δｔの関数である。状態遷移行列Ｆは、所定の運動モデルにおいて、ある時刻における移動体の位置などを表わすベクトルを、時間Δｔが経過した後における移動体の位置などを表わすベクトルへ写す写像を表わす。
すなわち、この式は、時刻ｔ２における推定値ベクトルｘ＾（ｔ２）に、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）を作用させて、時刻ｔ１における推定値ベクトルｘ＾（ｔ１）を算出することを意味している。
なお、状態遷移行列Ｆの運動モデルは、例えば、移動体がある角速度で運動していると仮定する旋回運動モデルなどである。
【０１５７】
角速度検定部２５３は、算出した時刻ｔ１における状態量の推定値に基づいて、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルにしたがい、時刻ｔ１から所定の時間ΔＴが経過した時刻ｔ３（すなわち、ｔ３＝ｔ１＋ΔＴ）における状態量を予測する。
例えば、角速度検定部２５３は、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔ３における状態量の予測値を算出する。
【数２６】
ただし、ｘ＾Ｊは、角速度補正部２５２が補正した時刻ｔ１における角速度で、状態推定部２４０が推定した角速度を置き換えた推定値ベクトルである。ＦＪは、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルに基づく状態遷移行列である。
すなわち、この式は、時刻ｔ１における推定値ベクトルｘ＾Ｊ（ｔ１）に、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪを作用させて、時刻ｔ３における推定値ベクトルｘ＾Ｊ（ｔ３）を算出することを意味している。
【０１５８】
また、角速度検定部２５３は、状態推定部２４０が算出した誤差分散や誤差共分散に基づいて、予測した状態量の誤差分散や誤差共分散を推定する。
【０１５９】
状態量の推定値と同様、誤差分散などの時刻と、補正した角速度の時刻とを一致させるため、角速度検定部２５３は、例えば、時刻ｔ２における誤差分散などに基づいて、時刻ｔ１における誤差分散などを算出する。
例えば、角速度検定部２５３は、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔ１における誤差分散などを算出する。
【数２７】
ただし、Ｐは、推定値ベクトルｘ＾の誤差の分散共分散行列である。分散共分散行列Ｐは、ｋ次の正方行列である。分散共分散行列Ｐは、時刻ｔの関数である。分散共分散行列Ｐの要素は、時刻ｔにおける推定値ベクトルの各要素の誤差の間の分散または共分散である。
上付きのＴは、行列の転置を表わす。
Ｑは、システム雑音の分散共分散行列である。分散共分散行列Ｑは、ｋ次の正方行列である。分散共分散行列Ｑは、時間Δｔの関数である。分散共分散行列Ｑの要素は、時間Δｔが経過する間に発生するシステム雑音の分散または共分散である。
すなわち、この式は、時刻ｔ２における推定誤差の分散共分散行列Ｐ（ｔ２）に、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）の転置行列を左から作用させ、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）を右から作用させたものに、時間（ｔ１−ｔ２）の経過により発生したシステム雑音の分散共分散行列Ｑ（ｔ１−ｔ２）を加えて、時刻ｔ１における推定誤差の分散共分散行列Ｐ（ｔ１）を算出することを意味している。
【０１６０】
角速度検定部２５３は、算出した時刻ｔ１における誤差分散などに基づいて、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルにしたがい、時刻ｔ３における誤差分散などを算出する。
例えば、角速度検定部２５３は、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔ３における誤差分散などを算出する。
【数２８】
ただし、ＰＪは、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルに基づく予測の誤差の分散共分散行列である。
この式は、時刻ｔ１における推定誤差の分散共分散行列Ｐ（ｔ１）に、移動体が角速度補正部２５２が補正した時刻ｔ１における角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪ（ΔＴ）の転置行列を左から作用させ、角速度補正部２５２が補正した時刻ｔ１における角速度で移動体が旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪ（ΔＴ）を右から作用させたものに、時間ΔＴの経過により発生したシステム雑音の分散共分散行列Ｑ（ΔＴ）を加えて、時刻ｔ３についての予測誤差の分散共分散行列ＰＪ（ｔ３）を算出することを意味している。
【０１６１】
角速度検定部２５３は、例えば上記のようにして、時刻ｔ３における状態量や誤差分散などを予測する。
なお、角速度検定部２５３は、角速度補正部２５２が補正した角速度ではなく、角速度観測値取得部２１３が記憶した移動体角速度データが表わす角速度観測装置８３１が観測した補正前の角速度に基づいて、状態量や誤差分散などを予測する構成であってもよい。
【０１６２】
また、角速度検定部２５３は、角速度を置き換えずに、状態推定部２４０が推定した移動体の角速度をそのまま使って、状態量や誤差分散などを予測する。
【０１６３】
角速度検定部２５３は、例えば、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔ３における状態量や誤差分散などを算出する。
【数２９】
ただし、ｘ＾Ｔは、状態推定部２４０が推定した移動体の角速度をそのまま使った時刻ｔ２における推定値ベクトルである。ＦＴは、状態推定部２４０が推定した時刻ｔ２における角速度で移動体が旋回運動をしているという運動モデルに基づく状態遷移行列である。ＰＴは、状態推定部２４０が推定した時刻ｔ２における角速度で移動体が旋回運動をしているという運動モデルに基づく予測の誤差の分散共分散行列である。
【０１６４】
角速度検定部２５３は、例えば上記のようにして、状態量や誤差分散などを予測する。
【０１６５】
角速度検定部２５３は、予測した２種類の状態量や誤差分散などに基づいて、角速度補正部２５２が補正した角速度が正しいか否かを判定する。
【０１６６】
角速度検定部２５３は、例えば、予測した２種類の状態量の間の差が、予想される範囲内であるか否かを判定し、予想される範囲を超えている場合に、角速度補正部２５２が補正した角速度が正しくないと判定する。
この判定には、統計学上の手法を用いることができる。角速度検定部２５３は、例えばカイ二乗検定により、２種類の状態量の間の差が、予想される範囲内であるか否かを判定する。例えば、角速度検定部２５３は、次の式の右辺を計算することにより、検定値を算出する。
【数３０】
ただし、εは、検定値である。
角速度検定部２５３は、処理装置９１１を用いて、算出した検定値εを、所定の閾値εｔｈと比較する。閾値εｔｈは、所定の有意水準に基づいて、例えばカイ二乗分布表から求めたものである。検定値εが閾値εｔｈ以下である場合、角速度検定部２５３は、２種類の状態量の間の差が、予想される範囲内であると判定する。検定値εが閾値εｔｈより大きい場合、角速度検定部２５３５は、２種類の状態量の間の差が、予想される範囲を超えていると判定する。
なお、角速度検定部２５３は、２種類の状態量を比較するのではなく、角速度補正部２５２が補正した角速度に基づいて予測した状態量に基づいて観測値を予測し、予測した観測値と実際の観測値とを比較することにより、角速度補正部２５２が補正した角速度が正しいか否かを判定する構成であってもよい。
【０１６７】
以上、各実施の形態で説明した構成は、一例であり、異なる実施の形態で説明した構成を組み合わせた構成としてもよい。また、主要でない部分の構成を既存の技術など他の構成と置き換えるなどの変形をした構成としてもよい。
【０１６８】
以上説明した角速度推定装置（２００；ヨーレートバイアス補正装置８００）は、相対位置取得部（２１１；停止物識別部１１０）と、静止物体判定部（２２０；停止物識別部１１０）と、物体相関部（２３０；停止物用追尾部１２０）と、角速度推定部（状態推定部２４０；軌道推定部１４３；軌道統合部１５０）とを有する。
上記相対位置取得部（２１１；１１０）は、移動体（８０１）の周辺に存在する物体について、上記移動体（８０１）を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置（８１１；レーダ８１０）が観測した観測結果を取得する。
上記静止物体判定部（２２０；１１０）は、上記相対位置取得部（２１１；１１０）が取得した観測結果に基づいて、上記周辺物体観測装置（８１１；８１０）が相対位置を観測した物体が静止しているか否かを判定する。
上記物体相関部（２３０；１２０）は、上記相対位置取得部（２１１）が取得した観測結果に基づいて、上記周辺物体観測装置（８１１；８１０）が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置（８１１；８１０）が観測した複数の相対位置を判定する。
上記角速度推定部（２４０；１４３；１５０）は、上記相対位置取得部（２１１）が取得した観測結果と、上記静止物体判定部（２２０；１１０）が判定した判定結果と、上記物体相関部（２３０；１２０）が判定した判定結果とに基づいて、上記移動体（８０１）の角速度を推定する。
【０１６９】
これにより、ＧＰＳなど移動体の位置を観測する観測装置がなくても、移動体の角速度を推定することができる。また、推定した角速度を用いることにより、ジャイロセンサなど移動体の角速度を観測する装置の誤差を推定することができる。
【０１７０】
角速度推定装置（２００；８００）は、相対位置算出部（停止物基準座標変換部１４２）を有する。
上記相対位置算出部（１４２）は、上記相対位置取得部（停止物識別部１１０）が取得した観測結果と、上記静止物体判定部（１１０）が判定した判定結果と、上記物体相関部（停止物用追尾部１２０）が判定した判定結果とに基づいて、静止している物体を基準とした上記移動体（８０１）の相対位置を算出する。
上記角速度推定部（１４３；１５０）は、上記相対位置算出部（１４２）が算出した相対位置に基づいて、上記移動体（８０１）の角速度を推定する。
【０１７１】
静止していると判定した物体との相対位置関係から、移動体自身の動きを推定して、移動体の角速度を推定するので、ＧＰＳなどの観測装置がなくても、移動体の角速度を推定することができる。
【０１７２】
角速度推定装置（２００；８００）は、角速度観測値取得部（２１３）と、角速度観測誤差算出部（状態推定部２４０；バイアス補正部８４０）とを有する。
上記角速度観測値取得部（２１３）は、上記移動体（８０１）の角速度を観測する角速度観測装置（角速度観測装置８３１；８３０）が観測した観測結果を取得する。
上記角速度観測誤差算出部（２４０；８４０）は、上記角速度推定部（２４０；１４３；１５０）が推定した推定結果と、上記角速度観測値取得部（２１３）が取得した観測結果とに基づいて、上記角速度観測装置（８３１；８３０）が観測した角速度の誤差を算出する。
【０１７３】
角速度観測装置の誤差を算出することにより、周辺物体観測装置からの入力がない場合でも、角速度観測装置が観測した移動体の角速度を補正することができる。
【０１７４】
角速度推定装置（２００；８００）は、速度観測値取得部（２１２；停止物識別部１１０）を有する。
上記速度観測値取得部（２１２；１１０）は、上記移動体（８０１）の移動速度を観測する移動速度観測装置（８２１；車速センサ８２０）が観測した観測結果を取得する。
上記静止物体判定部（２２０；１１０）は、上記相対位置取得部（２１１；１１０）が取得した観測結果と、上記速度観測値取得部（２１２；１１０）が取得した観測結果とに基づいて、上記物体が静止しているか否かを判定する。
【０１７５】
物体が静止しているか否かを判定することにより、静止している物体を基準として、移動体の動きを把握することができる。
【０１７６】
上記周辺物体観測装置（８１１；８１０）は、上記移動体（８０１）を基準とした上記物体の相対速度を観測する。
上記静止物体判定部（２２０；１１０）は、上記移動速度観測装置（８２１；８２０）が観測した上記移動体（８０１）の移動速度と、上記周辺物体観測装置（８１１；８１０）が観測した上記物体の相対速度とに基づいて、上記物体が静止しているか否かを判定する。
【０１７７】
移動体の移動速度と、物体の移動体に対する相対速度とに基づいて、物体が静止しているか否かを判定するので、移動体の旋回角度や物体の軌跡などを用いることなく、物体が静止しているか否かを判定することができる。
【０１７８】
角速度推定装置（２００；８００）は、相対速度推定部（静止物体判定部２２０）を有する。
上記相対速度推定部（２２０）は、上記物体相関部（２３０）が判定した判定結果に基づいて、上記周辺物体観測装置（８１１）が複数回観測した物体について、上記移動体（８０１）に対する上記物体の相対速度を推定する。
上記静止物体判定部（２２０）は、上記移動速度観測装置（８２１）が観測した上記移動体（８０１）の移動速度と、上記相対速度推定部（２２０）が推定した上記物体の相対速度とに基づいて、上記物体が静止しているか否かを判定する。
【０１７９】
物体相関部の判定結果に基づいて、物体の移動体に対する相対速度を算出するので、物体の移動体に対する相対速度を観測する観測装置がなくても、物体が静止しているか否かを判定することができる。
【０１８０】
角速度推定装置（２００；８００）は、コンピュータを角速度推定装置として機能させるコンピュータプログラムをコンピュータが実行することにより、実現することができる。
【符号の説明】
【０１８１】
１１０ 停止物識別部、１２０ 停止物用追尾部、１２１ 相関部、１２２ 平滑部、１２３ 予測部、１２４ 遅延部、１３０ 停止物記憶部、１４０ 自車軌道計算部、１４１ 相関停止物選択部、１４２ 停止物基準座標変換部、１４３ 軌道推定部、１５０ 軌道統合部、２００ 角速度推定装置、２１１ 相対位置取得部、２１２ 速度観測値取得部、２１３ 角速度観測値取得部、２２０ 静止物体判定部、２３０ 物体相関部、２４０ 状態推定部、２５１ 角速度誤差記憶部、２５２ 角速度補正部、２５３ 角速度検定部、２５４ 角速度誤差算出部、８００ ヨーレートバイアス補正装置、８１０ レーダ、８１１ 周辺物体観測装置、８２０ 車速センサ、８２１ 移動速度観測装置、８３０ ヨーレートセンサ、８３１ 角速度観測装置、８４０ バイアス補正部、９０１ 出力装置、９０２ 入力装置、９１１ 処理装置、９１４ 記憶装置。
【特許請求の範囲】
【請求項１】
相対位置取得部と、静止物体判定部と、物体相関部と、角速度推定部とを有し、
上記相対位置取得部は、移動体の周辺に存在する物体について、上記移動体を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置が観測した観測結果を取得し、
上記静止物体判定部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が相対位置を観測した物体が静止しているか否かを判定し、
上記物体相関部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置が観測した複数の相対位置を判定し、
上記角速度推定部は、上記相対位置取得部が取得した観測結果と、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、上記移動体の角速度を推定することを特徴とする角速度推定装置。
【請求項２】
上記角速度推定装置は、相対位置算出部を有し、
上記相対位置算出部は、上記相対位置取得部が取得した観測結果と、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、静止している物体を基準とした上記移動体の相対位置を算出し、
上記角速度推定部は、上記相対位置算出部が算出した相対位置に基づいて、上記移動体の角速度を推定することを特徴とする請求項１に記載の角速度推定装置。
【請求項３】
上記角速度推定装置は、角速度観測値取得部と、角速度観測誤差算出部とを有し、
上記角速度観測値取得部は、上記移動体の角速度を観測する角速度観測装置が観測した観測結果を取得し、
上記角速度観測誤差算出部は、上記角速度推定部が推定した推定結果と、上記角速度観測値取得部が取得した観測結果とに基づいて、上記角速度観測装置が観測した角速度の誤差を算出することを特徴とする請求項１または請求項２に記載の角速度推定装置。
【請求項４】
上記角速度推定装置は、速度観測値取得部を有し、
上記速度観測値取得部は、上記移動体の移動速度を観測する移動速度観測装置が観測した観測結果を取得し、
上記静止物体判定部は、上記相対位置取得部が取得した観測結果と、上記速度観測値取得部が取得した観測結果とに基づいて、上記物体が静止しているか否かを判定することを特徴とする請求項１乃至請求項３のいずれかに記載の角速度推定装置。
【請求項５】
上記周辺物体観測装置は、上記移動体を基準とした上記物体の相対速度を観測し、
上記静止物体判定部は、上記移動速度観測装置が観測した上記移動体の移動速度と、上記周辺物体観測装置が観測した上記物体の相対速度とに基づいて、上記物体が静止しているか否かを判定することを特徴とする請求項４に記載の角速度推定装置。
【請求項６】
上記角速度推定装置は、相対速度推定部を有し、
上記相対速度推定部は、上記物体相関部が判定した判定結果に基づいて、上記周辺物体観測装置が複数回観測した物体について、上記移動体に対する上記物体の相対速度を推定し、
上記静止物体判定部は、上記移動速度観測装置が観測した上記移動体の移動速度と、上記相対速度推定部が推定した上記物体の相対速度とに基づいて、上記物体が静止しているか否かを判定することを特徴とする請求項４または請求項５に記載の角速度推定装置。
【請求項７】
コンピュータが実行することにより、上記コンピュータが請求項１乃至請求項６のいずれかに記載の角速度推定装置として機能することを特徴とするコンピュータプログラム。
【請求項８】
相対位置取得部と、静止物体判定部と、物体相関部と、角速度推定部とを有する角速度推定装置が移動体の角速度を推定する角速度推定方法において、
上記相対位置取得部が、移動体の周辺に存在する物体について、上記移動体を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置が観測した観測結果を取得し、
上記静止物体判定部が、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が相対位置を観測した物体が静止しているか否かを判定し、
上記物体相関部が、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置が観測した複数の相対位置を判定し、
上記角速度推定部が、上記相対位置取得部が取得した観測結果と、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、上記移動体の角速度を推定することを特徴とする角速度推定方法。
【請求項１】
相対位置取得部と、静止物体判定部と、物体相関部と、角速度推定部とを有し、
上記相対位置取得部は、移動体の周辺に存在する物体について、上記移動体を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置が観測した観測結果を取得し、
上記静止物体判定部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が相対位置を観測した物体が静止しているか否かを判定し、
上記物体相関部は、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置が観測した複数の相対位置を判定し、
上記角速度推定部は、上記相対位置取得部が取得した観測結果と、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、上記移動体の角速度を推定することを特徴とする角速度推定装置。
【請求項２】
上記角速度推定装置は、相対位置算出部を有し、
上記相対位置算出部は、上記相対位置取得部が取得した観測結果と、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、静止している物体を基準とした上記移動体の相対位置を算出し、
上記角速度推定部は、上記相対位置算出部が算出した相対位置に基づいて、上記移動体の角速度を推定することを特徴とする請求項１に記載の角速度推定装置。
【請求項３】
上記角速度推定装置は、角速度観測値取得部と、角速度観測誤差算出部とを有し、
上記角速度観測値取得部は、上記移動体の角速度を観測する角速度観測装置が観測した観測結果を取得し、
上記角速度観測誤差算出部は、上記角速度推定部が推定した推定結果と、上記角速度観測値取得部が取得した観測結果とに基づいて、上記角速度観測装置が観測した角速度の誤差を算出することを特徴とする請求項１または請求項２に記載の角速度推定装置。
【請求項４】
上記角速度推定装置は、速度観測値取得部を有し、
上記速度観測値取得部は、上記移動体の移動速度を観測する移動速度観測装置が観測した観測結果を取得し、
上記静止物体判定部は、上記相対位置取得部が取得した観測結果と、上記速度観測値取得部が取得した観測結果とに基づいて、上記物体が静止しているか否かを判定することを特徴とする請求項１乃至請求項３のいずれかに記載の角速度推定装置。
【請求項５】
上記周辺物体観測装置は、上記移動体を基準とした上記物体の相対速度を観測し、
上記静止物体判定部は、上記移動速度観測装置が観測した上記移動体の移動速度と、上記周辺物体観測装置が観測した上記物体の相対速度とに基づいて、上記物体が静止しているか否かを判定することを特徴とする請求項４に記載の角速度推定装置。
【請求項６】
上記角速度推定装置は、相対速度推定部を有し、
上記相対速度推定部は、上記物体相関部が判定した判定結果に基づいて、上記周辺物体観測装置が複数回観測した物体について、上記移動体に対する上記物体の相対速度を推定し、
上記静止物体判定部は、上記移動速度観測装置が観測した上記移動体の移動速度と、上記相対速度推定部が推定した上記物体の相対速度とに基づいて、上記物体が静止しているか否かを判定することを特徴とする請求項４または請求項５に記載の角速度推定装置。
【請求項７】
コンピュータが実行することにより、上記コンピュータが請求項１乃至請求項６のいずれかに記載の角速度推定装置として機能することを特徴とするコンピュータプログラム。
【請求項８】
相対位置取得部と、静止物体判定部と、物体相関部と、角速度推定部とを有する角速度推定装置が移動体の角速度を推定する角速度推定方法において、
上記相対位置取得部が、移動体の周辺に存在する物体について、上記移動体を基準とした上記物体の相対位置を繰り返し観測する周辺物体観測装置が観測した観測結果を取得し、
上記静止物体判定部が、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が相対位置を観測した物体が静止しているか否かを判定し、
上記物体相関部が、上記相対位置取得部が取得した観測結果に基づいて、上記周辺物体観測装置が観測した複数の相対位置のなかから、同一の物体について上記周辺物体観測装置が観測した複数の相対位置を判定し、
上記角速度推定部が、上記相対位置取得部が取得した観測結果と、上記静止物体判定部が判定した判定結果と、上記物体相関部が判定した判定結果とに基づいて、上記移動体の角速度を推定することを特徴とする角速度推定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２８】
【公開番号】特開２０１１−２４７７２１（Ｐ２０１１−２４７７２１Ａ）
【公開日】平成２３年１２月８日（２０１１．１２．８）
【国際特許分類】
【出願番号】特願２０１０−１２０５０２（Ｐ２０１０−１２０５０２）
【出願日】平成２２年５月２６日（２０１０．５．２６）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２３年１２月８日（２０１１．１２．８）
【国際特許分類】
【出願日】平成２２年５月２６日（２０１０．５．２６）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]