方位角推定装置及びプログラム
【課題】マルチパスを適切に判定して、精度良く移動体の方位角を推定する。
【解決手段】複数のＧＰＳ衛星の各々から発信されたＧＰＳ情報を取得し、方位角推定部３０で、ＧＰＳ情報に基づいて自車両の方位角（推定方位角）を推定する。最適推定部６２は、ジャイセンサ１４の検出値に基づいて自車両の方位角（観測方位角）を算出し、推定方位角と観測方位角とを統合して最適値を推定する。精度判定部６４は、最適値と推定方位角との残差の分散が閾値以下か否かに基づいて、最適値の精度を判定する。速度ベクトル算出部６６は、最適値の精度が高いと判定された場合には、最適値を推定する方位角として採用し、最適値及び速度センサ１６で検出された速度を用いて速度ベクトルを算出し、精度が低いと判定された場合には、過去に推定された方位角を用いてジャイロセンサ１４の検出値を積算して算出した方位角を採用して、速度ベクトルを算出する。
【解決手段】複数のＧＰＳ衛星の各々から発信されたＧＰＳ情報を取得し、方位角推定部３０で、ＧＰＳ情報に基づいて自車両の方位角（推定方位角）を推定する。最適推定部６２は、ジャイセンサ１４の検出値に基づいて自車両の方位角（観測方位角）を算出し、推定方位角と観測方位角とを統合して最適値を推定する。精度判定部６４は、最適値と推定方位角との残差の分散が閾値以下か否かに基づいて、最適値の精度を判定する。速度ベクトル算出部６６は、最適値の精度が高いと判定された場合には、最適値を推定する方位角として採用し、最適値及び速度センサ１６で検出された速度を用いて速度ベクトルを算出し、精度が低いと判定された場合には、過去に推定された方位角を用いてジャイロセンサ１４の検出値を積算して算出した方位角を採用して、速度ベクトルを算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、方位角推定装置及びプログラムに係り、特に、移動体の方位角を推定するための方位角推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、衛星信号を利用して、高層ビル街などのマルチパスの影響が大きい環境下でも、移動体の最適な速度ベクトルを算出するシステムが提案されている（例えば、特許文献１参照）。特許文献１記載の速度ベクトル決定システムは、衛星信号追尾装置で追尾している複数衛星の中から、衛星軌道情報に基づく衛星仰角情報及び追尾情報に基づく信号レベル情報を利用して、２つの衛星が共通に含まれ且つ他の衛星が含まれ、３以上の衛星信号からなる複数の組み合わせを求める。そして、その複数の組み合わせ毎の速度ベクトル解をそれぞれ演算して速度ベクトル解候補を得、且つそれら各速度ベクトル解候補間の一致度に基づいて、最適速度ベクトルを求めている。
【０００３】
また、測位衛星からの複数の擬似距離と、測位航法部と慣性航法部とカルマンフィルタと、により移動体の航法位置を求める移動体測位装置が提案されている（例えば、特許文献２参照）。特許文献２に記載の移動体測位装置は、カルマンフィルタの出力から得られるドップラ推定値とドップラ観測値との差の絶対値を所定の条件で比較してマルチパスを判定している。
【０００４】
また、誤差の影響の少ないＧＰＳデータを選択的に用いるＧＰＳナビゲーション装置が提案されている（例えば、特許文献３参照）。特許文献３に記載のＧＰＳナビゲーション装置では、ＧＰＳ速度計測部からの移動ベクトルからＧＰＳ方位を求め、ＧＰＳ方位、ジャイロセンサで検出された移動体の回転角速度、及び地図メモリの道路リンク情報から移動体の進行方位し、新たなＧＰＳデータが検出される毎に、当該データに基づいて算出されたＧＰＳ方位とその時点で求められている推定方位とを比較することで、当該データに含まれる誤差の影響の大きさを判定し、その影響が大きい場合には当該データを位置推定に用いないように制御している。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００６−２０８３９２号公報
【特許文献２】特許第３８７５７１４号公報
【特許文献３】特開平８−３３４３３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記の特許文献１に記載の技術では、都市部などでは、高仰角の衛星からもマルチパスが発生する場合もあるが、この点が考慮されておらず、最適な速度ベクトルを推定することができない場合がある、という問題がある。また、同様に都市部などでは、マルチパスが発生している衛星が受信している衛星群が過半数を超える場合も多く、そのような場合には、組み合わせの変更により最適な速度ベクトルを推定することができない、という問題がある。さらに、衛星信号を受信できない場合には、速度ベクトルを推定することができない、という問題がある。
【０００７】
このような問題に対応するために、特許文献２に記載の技術のように、衛星信号と慣性センサの検出値とを統合して移動体に関する物理量を推定することが考えられる。しかし、特許文献２に記載の技術では、カルマンフィルタにより最適推定値を求めてマルチパスの判定に用いているが、カルマンフィルタに入力される衛星信号の情報に多数のマルチパスの影響が含まれている場合には、最適推定値が真値から外れる可能性が高まるため、誤差が発生している状況の観測値と比較したとしても、マルチパス誤差が未検出になる可能性がある、という問題がある。また、推定値に誤差があり、観測値の方が正しい場合においては、正しい観測値がマルチパス誤差として誤判定される可能性が高い、という問題がある。加えて、特許文献２に記載の技術では、擬似距離を用いて速度や位置といった物理量を算出しているため、推定する物理量は擬似距離の精度の影響を大きく受ける、という問題がある。
【０００８】
また、特許文献３に記載の技術では、ＧＰＳデータの擬似距離変化率から車両の進行方位を推定し、時系列のデータをジャイロセンサの計測値と併せて比較することで、ＧＰＳデータの誤差が少ない地点を判定している。しかし、マルチパス誤差は場所に依存するため、マルチパスが発生している場所において、短い時間で計測された２つの方位角には、同じ傾向の誤差が発生している可能性が高く、マルチパス誤差が未検出になる可能性がある、という問題がある。また、マルチパスが発生しない場所においても、ＧＰＳデータにはガウス性の誤差が発生している。そのため、計測した車両の方位角には少なくともランダム性の誤差が生じるため、この計測値と推定値との差を用いてマルチパス誤差を判定したとしても、閾値の設定次第では誤検出や未検出が頻発してしまう可能性がある、という問題がある。誤判定を防ぐために、閾値を小さくしたとしても、前述の理由によりマルチパス誤差の未検出を防ぐことはできない。
【０００９】
本発明は、上記の問題点を解決するためになされたもので、マルチパスを適切に判定して、精度良く移動体の方位角を推定することができる方位角推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記目的を達成するために、本発明の方位角推定装置は、移動体に搭載される方位角推定装置であって、複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、前記移動体の方位角に関する物理量を検出する検出手段と、前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルから前記移動体の方位角を算出する算出手段と、前記算出手段により算出された方位角、及び前記検出手段により検出された物理量から得られる方位角に基づいて、前記移動体の方位角の最適値を推定する最適値推定手段と、前記最適値推定手段により推定された最適値と前記算出手段により算出された方位角との差の分布に基づいて、前記方位角の最適値の精度を判定する判定手段と、前記判定手段により精度が予め定めた閾値よりも高いと判定されたときの最適値を用いて前記移動体の方位角を推定する方位角推定手段と、を含んで構成されている。
【００１１】
本発明の方位角推定装置によれば、取得手段が、移動体に搭載される方位角推定装置であって、複数の情報発信源の各々から発信された情報発信源の各々の位置に関する情報、情報発信源の各々と移動体との距離に関する情報、及び情報発信源の各々に対する移動体の相対速度に関する情報を含む発信源情報を取得し、検出手段が、移動体の方位角に関する物理量を検出する。検出手段は慣性センサであり、例えば、自車両の角速度または角加速度を検出するジャイロセンサや、方位角を検出する磁気方位計を用いることができる。
【００１２】
そして、算出手段が、取得手段により取得された発信源情報に基づいて得られる移動体の速度ベクトルから移動体の方位角を算出し、最適値推定手段が、算出手段により算出された方位角、及び検出手段により検出された物理量から得られる方位角に基づいて、移動体の方位角の最適値を推定する。算出手段により算出された移動体の方位角は絶対量であり、検出手段により検出された物理量に基づく移動体の方位角は相対量であり、これらを統合して最適値を推定する場合に、マルチパスなどによる絶対量のばらつきが最適値の精度に影響を与える。そこで、判定手段は、推定した最適値と算出手段により算出された方位角との差の分布に基づいて、方位角の最適値の精度を判定する。そして、方位角推定手段が、判定手段により精度が予め定めた閾値よりも高いと判定されたときの最適値を用いて移動体の方位角を推定する。
【００１３】
このように、発信源情報に基づいて算出された絶対量である移動体の方位角と、検出手段の検出値に基づく相対量である移動体の方位角とを統合して移動体の方位角の最適値を推定し、最適値と算出された方位角との差の分布に基づいて最適値の精度を判定するため、マルチパスを適切に判定して、精度良く移動体の方位角を推定することができる。
【００１４】
また、前記判定手段は、前記差のばらつきが予め定めた閾値以下か否か、または前記差の分布がガウス分布に適合するか否かを判定することによって、前記方位角の最適値の精度を判定することができる。最適値と算出された方位角との差のばらつきが小さい場合、または差の分布がガウス分布に適合する場合には、最適値の精度が高いと判定することができる。
【００１５】
また、前記方位角推定手段は、前記判定手段により精度が予め定めた閾値よりも高いと判定された場合には、前記最適値を前記移動体の方位角として推定し、前記判定手段により精度が予め定めた閾値よりも低いと判定された場合には、過去に推定された方位角及び前記検出手段により検出された物理量に基づいて、前記移動体の方位角を推定することができる。これにより、絶対量として算出された方位角を相対量として検出された方位角で補間して、精度良く移動体の方位角を推定することができる。
【００１６】
また、前記最適値推定手段は、所定の初期値を基準に前記検出手段により検出された物理量を所定時間分積分して前記検出手段により検出された物理量から得られる方位角を算出し、該方位角と前記算出手段により算出された方位角との一定時間における差の和が最小となる前記初期値を探索することで、前記方位角の最適値を推定することができる。
【００１７】
また、前記算出手段は、前記情報発信源の各々の位置に関する情報、及び前記情報発信源の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記情報発信源の各々の方向を算出し、時系列の前記情報発信源の各々の位置に関する情報に基づいて、前記情報発信源の各々の速度を算出し、前記移動体から見た前記情報発信源の各々の方向、前記情報発信源の各々の速度、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体の情報発信源の各々の方向の速度を算出し、複数の前記移動体の情報発信源の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出することができる。
【００１８】
また、前記情報発信源を、前記情報発信源の各々の位置に関する情報として衛星軌道情報を、前記情報発信源の各々と前記移動体との距離に関する情報として擬似距離情報を、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報としてドップラー周波数情報を発信するＧＰＳ衛星とすることができる。情報発信源としては、例えば、擬似衛星やビーコンなどのように、発信源情報を発信するものであればよいが、代表的には、ＧＰＳ衛星とすることができる。
【００１９】
また、本発明の方位角推定プログラムは、コンピュータを、移動体に搭載される方位角推定装置の各手段として機能させるための方位角推定プログラムであって、前記コンピュータを、複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段、前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルから前記移動体の方位角を算出する算出手段、前記算出手段により算出された方位角、及び前記移動体の方位角に関する物理量を検出する検出手段により検出された物理量から得られる方位角に基づいて、前記移動体の方位角の最適値を推定する最適値推定手段、前記最適値推定手段により推定された最適値と前記算出手段により算出された方位角との差の分布に基づいて、前記方位角の最適値の精度を判定する判定手段、及び前記判定手段により精度が予め定めた閾値よりも高いと判定されたときの最適値を用いて前記移動体の方位角を推定する方位角推定手段として機能させるためのプログラムである。
【００２０】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２１】
以上説明したように、本発明の方位角推定装置及びプログラムによれば、発信源情報に基づいて算出された絶対量である移動体の方位角と、検出手段の検出値に基づく相対量である移動体の方位角とを統合して移動体の方位角の最適値を推定し、最適値と算出された方位角との差の分布に基づいて最適値の精度を判定するため、マルチパスを適切に判定して、精度良く移動体の方位角を推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２２】
【図１】本実施の形態に係る車載速度ベクトル推定装置を示すブロック図である。
【図２】三角測量の原理に従って自車両の位置が推定される様子を示すイメージ図である。
【図３】各ＧＰＳ衛星の速度ベクトル及び各ＧＰＳ衛星の方向に基づいて、ＧＰＳ衛星方向の自車両の速度を算出する様子を示すイメージ図である。
【図４】（ａ）マルチパスの影響がない場合、及び（ｂ）マルチパスの影響がある場合における最適値の推定を説明するための図である。
【図５】（ａ）マルチパスの影響がない場合、及び（ｂ）マルチパスの影響がある場合における最適値と推定方位角との残差の分布を示す図である。
【図６】本実施の形態に係る車載速度ベクトル推定装置のコンピュータにおける速度ベクトル推定処理ルーチンの内容を示すフローチャートである。
【図７】本実施の形態に係る車載速度ベクトル推定装置のコンピュータにおけるＧＰＳ情報に基づく自車両方位角推定処理ルーチンの内容を示すフローチャートである。
【図８】本実施の形態に係る車載速度ベクトル推定装置のコンピュータにおける速度ベクトル算出処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００２３】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、ＧＰＳ衛星から発信されたＧＰＳ情報を取得して自車両の方位角を推定し、推定した方位角から自車両の速度ベクトルを推定する車載速度ベクトル推定装置に、本発明を適用した場合を例に説明する。
【００２４】
図１に示すように、本実施の形態に係る車載速度ベクトル推定装置１０は、ＧＰＳ衛星からの電波を受信するＧＰＳ受信部１２と、自車両のヨーレートを検出するジャイロセンサ１４と、自車両の速度を検出する速度センサ１６と、ＧＰＳ受信部１２によって受信されたＧＰＳ衛星からの受信信号、並びにジャイロセンサ１４及び速度センサ１６の検出値に基づいて、自車両の速度ベクトルを推定する処理を実行するコンピュータ１８と、を備えている。
【００２５】
ＧＰＳ受信部１２は、複数のＧＰＳ衛星からの電波を受信して、受信した全てのＧＰＳ衛星からの受信信号から、ＧＰＳ衛星の情報として、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報（エフェメリス）、ＧＰＳ衛星が電波を送信した時刻、受信信号の強度、周波数などを取得し、コンピュータ１８に出力する。
【００２６】
コンピュータ１８は、ＣＰＵ、後述する速度ベクトル推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。
【００２７】
コンピュータ１８を以下で説明する速度ベクトル推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ擬似距離データ、ドップラー周波数、及びＧＰＳ衛星の位置座標を算出して取得するＧＰＳ情報取得部２０と、取得したＧＰＳ情報に基づいて、自車両の方位角を推定する方位角推定部３０と、推定された方位角、ジャイロセンサ１４の検出値、及び速度センサ１６の検出値を対応させて記憶するデータ記憶部４０と、データ記憶部４０に記憶された各種データに基づいて、自車両の速度ベクトルを推定する速度ベクトル推定部６０と、を含んだ構成で表すことができる。
【００２８】
ＧＰＳ情報取得部２０は、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ衛星が電波を送信した時刻及び自車両で電波を受信した時刻に基づいて、ＧＰＳ擬似距離データを算出する。また、ＧＰＳ情報取得部２０は、各ＧＰＳ衛星から送信される信号の既知の周波数と、各ＧＰＳ衛星から受信した受信信号の周波数とに基づいて、各ＧＰＳ衛星からの受信信号のドップラー周波数を各々算出する。なお、ドップラー周波数は、ＧＰＳ衛星と自車との相対速度による、搬送波周波数のドップラーシフト量を観測したものである。また、ＧＰＳ情報取得部２０は、ＧＰＳ衛星の軌道情報及びＧＰＳ衛星が電波を送信した時刻に基づいて、ＧＰＳ衛星の位置座標を各々算出する。
【００２９】
また、方位角推定部３０は、さらに、取得した各ＧＰＳ衛星のドップラー周波数に基づいて、各ＧＰＳ衛星に対する自車両の相対速度を算出する相対速度算出部４２と、取得した各ＧＰＳ衛星の位置座標の時系列データに基づいて、各ＧＰＳ衛星の速度ベクトルを算出する衛星速度算出部４４と、取得した各ＧＰＳ衛星のＧＰＳ擬似距離データに基づいて、自車両の位置を算出する自車位置算出部４６と、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星の方向（角度の関係）を算出する衛星方向算出部４８と、算出された相対速度、各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の方向に基づいて、各ＧＰＳ衛星方向の自車両の速度を算出する衛星方向自車速算出部５０と、算出された複数の各ＧＰＳ衛星方向の自車両の速度に基づいて、自車両の速度ベクトルを算出する自車両速度算出部５２と、算出された自車両の速度ベクトルに基づいて自車両の方位角を算出する自車両方位角算出部５４と、を備えた構成で表すことができる。
【００３０】
相対速度算出部４２は、ドップラー周波数とＧＰＳ衛星に対する相対速度との関係を表わす以下の（１）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度を算出する。
【００３１】
【数１】
【００３２】
ただし、ｖｊはＧＰＳ衛星ｊに対する相対速度であり、Ｄ１ｊはＧＰＳ衛星ｊから得られるドップラー周波数（ドップラーシフト量）である。また、Ｃは光速であり、Ｆ１は、ＧＰＳ衛星から送信される信号の既知のＬ１周波数である。
【００３３】
衛星速度算出部４４は、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（３次元速度ＶＸｊ、ＶＹｊ、ＶＺｊ）を算出する。例えば、非特許文献（Ｐｒａｔａｐ Ｍｉｓｒａ ａｎｄ Ｐｅｒ Ｅｎｇｅ原著 日本航海学会ＧＰＳ研究会訳：“精説ＧＰＳ基本概念・測位原理・信号と受信機”正陽文庫，２００４．）に記載された方法を用いて、各ＧＰＳ衛星の速度ベクトルを算出することができる。
【００３４】
自車位置算出部４６は、以下のように、ＧＰＳ情報取得部２０によって取得された各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。
【００３５】
ＧＰＳを用いた測位では、図２に示すように、既知であるＧＰＳ衛星の位置座標と、各ＧＰＳ衛星から受信した受信信号の伝播距離である擬似距離とに基づいて、三角測量の原理に従って、自車両の位置が推定される。
【００３６】
ここで、ＧＰＳ衛星までの真の距離ｒｊは、以下の（２）式で表され、ＧＰＳで観測される擬似距離ρｊは、以下の（３）式で表される。
【００３７】
【数２】
【００３８】
ただし、（Ｘｊ，Ｙｊ，Ｚｊ）がＧＰＳ衛星ｊの位置座標であり、（ｘ，ｙ，ｚ）が自車両の位置座標である。ｓは、ＧＰＳ受信部１２の時計誤差による距離誤差である。
【００３９】
上記（２）式、（３）式より、４つ以上のＧＰＳ衛星のＧＰＳ擬似距離データから得られる以下の（４）式の連立方程式を解くことによって、自車両の位置（ｘ、ｙ、ｚ）が算出される。
【００４０】
【数３】
【００４１】
なお、本実施の形態では、自車両の位置を、ＧＰＳ衛星の方向（ＧＰＳ衛星と自車両との角度）を求めるために算出しているが、遠方に存在するＧＰＳ衛星の方向であるため、位置決めは大まかでよく、擬似距離を用いた位置決定を行わなくてもよい。時刻の影響が少なく、システム等で許容される推定精度に依存するが、自車の位置決めの誤差も数百ｍの範囲であれば、速度推定誤差も１ｍ／ｓｅｃ以下程度となり大きな問題はないため、例えば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、自車両の位置を決定してもよい。
【００４２】
衛星方向算出部４８は、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係（水平方向に対する仰角θｊ、北方向に対する方位角φｊ）を、各ＧＰＳ衛星の方向として算出する。
【００４３】
衛星方向自車速算出部５０は、図３に示すように、算出された各ＧＰＳ衛星に対する自車両の相対速度ｖｊ、各ＧＰＳ衛星の速度ベクトル（ＶＸｊ、ＶＹｊ、ＶＺｊ）、及び各ＧＰＳ衛星の方向Ｒｊ（θｊ、φｊ）に基づいて、以下の（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖｊを算出する。
【００４４】
【数４】
【００４５】
ｖｊは、ＧＰＳ衛星ｊに対する自車両の相対速度（衛星方向におけるＧＰＳ衛星との相対速度）である。また、Ｖｓｊは、自車方向のＧＰＳ衛星ｊの速度であり、Ｖｓｊ＝Ｒｊ［ＶＸｊ，ＶＹｊ，ＶＺｊ］Ｔにより求まる。また、Ｖｖｊは、ＧＰＳ衛星ｊの方向の自車速であり、ｖＣｂは、クロックバイアス変動である。
【００４６】
上述したように、ＧＰＳ衛星方向の自車速は、ＧＰＳ衛星位置の三次元位置ではなく、ＧＰＳ衛星との方位関係によってのみ算出される。ＧＰＳ衛星は遥か遠方にあり、１日で地球をほぼ２周するため１分間の角度変化は０．５度である。通常、ＧＰＳ衛星とＧＰＳ受信機との時計誤差は通常１ｍｓｅｃ以下であるため、ＧＰＳ衛星との方位関係に大きな影響はない。また、同じくＧＰＳ衛星は遥か遠方にあるため、自車の位置決定に数１００ｍ程度の誤差が生じていたとしても、ＧＰＳ衛星との方位関係に大きな影響はない。このため、擬似距離に誤差が乗りやすい状況であったとしても、ＧＰＳ衛星方向の自車速は、擬似距離に比較して正確に算出され得る。
【００４７】
自車両速度算出部５２は、以下に説明するように、自車両の速度ベクトルの最適推定を行う。
【００４８】
まず、自車の速度ベクトルを（Ｖｘ，Ｖｙ，Ｖｚ）としたとき、ＧＰＳ衛星方向の自車両の速度Ｖｖｊとの関係は以下の（６）式で表される。
【００４９】
【数５】
【００５０】
各ＧＰＳ衛星ｊについて得られる上記（６）式より、Ｖｘ，Ｖｙ，Ｖｚ及びＣｂを推定値とした、以下の（７）式で表される連立方程式が得られる。
【００５１】
【数６】
【００５２】
電波を受信したＧＰＳ衛星が４個以上である場合に、上記（７）式の連立方程式を解くことによって、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。
【００５３】
自車両方位角算出部５４は、自車両速度算出部５２により算出された自車両の速度ベクトルから三角関数を用いて自車両の方位角を算出し、方位角推定部３０での方位角の推定結果としてデータ記憶部４０に記憶する。ここで推定された方位角は、ＧＰＳ情報に基づいて推定された方位角であるので、以下、「推定方位角」という。これに対し、ジャイロセンサ１４の検出値から求まる自車両の方位角を「観測方位角」という。ただし、ジャイロセンサ１４では、方位角の変化分しか計測できないため、ある初期値を基準に時間経過分を積分することで、ジャイロセンサ１４の検出値から観測方位角を算出する。例えば、下記（８）式では、ある時間ｔ０の初期値ψｉｎｉｔ（ｔ０）を基準にｋ秒前の観測方位角ψｍ（ｔ０−ｋ）を算出している。つまり、データ記憶部４０にジャイロセンサ１４の検出値ψｇｙｒｏの微分値を記憶しておくことで、観測方位角が算出可能になる。
【００５４】
【数７】
【００５５】
なお、データ記憶部４０には、所定時間分の推定方位角と共に、対応する所定時間内に検出されたジャイロセンサ１４の検出値、及び速度センサ１６の検出値が記憶される。
【００５６】
また、速度ベクトル推定部６０は、さらに、推定方位角と観測方位角とを統合して方位角の最適値を推定する最適推定部６２と、推定された最適値と推定方位角との残差の分散に基づいて、推定方位角の精度を判定する精度判定部６４と、精度判定部６４での判定結果に基づいて自車両の方位角を推定し、推定した方位角を用いて速度ベクトルを算出する速度ベクトル算出部６６と、を含んだ構成で表すことができる。
【００５７】
最適推定部６２は、データ記憶部４０から所定期間分のジャイロセンサ１４の検出値である自車両のヨーレートを読み出し、観測毎のヨーレートを各々積分して時系列に積算して自車両の観測方位角を算出する。また、対応する所定期間内に推定された推定方位角をデータ記憶部４０から読み出す。
【００５８】
ここで、ＧＰＳ情報に基づいて推定された推定方位角は、マルチパスや大気に起因するＧＰＳの誤差要因により、推定結果にばらつきがあるという特徴を持つ。一方、ジャイロセンサ１４の検出値に基づく観測方位角は、連続した角度変化は精度よく推定できるが、絶対値の信頼度が不明であるという特徴を持つ。そこで、最適推定部６２は、最小二乗法等に代表される最適計算により、推定方位角と観測方位角とを統合した方位角の最適値を推定する。例えば、最小二乗法では、下記（９）式により推定方位角ψｇｐｓと、観測方位角ψｍとの一定時間における差の和が最小になるように、観測方位角ψｍの算出の基準となる最適な初期値ψｉｎｉｔ（ｔ０）を探索することで、方位角の最適値を推定する。
【００５９】
【数８】
【００６０】
精度判定部６４は、最適推定部６２で推定された方位角の最適値と推定方位角の各々との残差を算出し、所定期間分の残差のばらつき（分散または標準偏差）が所定の閾値以下か否かにより、方位角の最適値の精度を判定する。
【００６１】
ここで、残差のばらつきを用いて方位角の最適値の精度を判定する原理について説明する。図４（ａ）にマルチパスの影響がない場合、同図（ｂ）にマルチパスの影響がある場合の方位角の最適値を示す。マルチパスの影響がある場合には、残差が大きくなる外れ値が生じ、最適値が真値より外れる可能性が高まる。次に、図５（ａ）及び（ｂ）に、図４（ａ）及び（ｂ）の各々の場合における残差のヒストグラムを示す。マルチパスの影響がある場合は、マルチパスの影響がない場合と比較して、分散値が大きくなる傾向にある。従って、分散値を閾値と比較することで、方位角の最適値の精度を判定することができる。
【００６２】
速度ベクトル算出部６６は、最適推定部６２により推定された所定期間分の方位角の最適値が精度判定部６４により精度が高いと判定された場合には、その方位角の最適値及び速度センサ１６で検出された速度を用いて、速度ベクトルを算出する。一方、精度が低いと判定された最適値については、速度ベクトルの算出に用いない。その間の速度ベクトルは、過去に推定された方位角を用いてジャイロセンサ１４の検出値を積算して方位角を算出して、速度センサ１６で検出された速度を用いて算出する。速度ベクトル算出部６６は、算出した速度ベクトルを速度ベクトルの推定結果として出力する。
【００６３】
次に、本実施の形態に係る車載速度ベクトル推定装置１０の作用について説明する。
【００６４】
ＧＰＳ受信部１２によって、複数のＧＰＳ衛星から電波を受信しているときに、コンピュータ１８において、図６に示す速度ベクトル推定処理ルーチンが実行される。
【００６５】
ステップ１００で、ＧＰＳ受信部１２から複数のＧＰＳ衛星の情報を取得すると共に、複数のＧＰＳ衛星のＧＰＳ擬似距離データ、ドップラー周波数、ＧＰＳ衛星の位置座標を算出して取得する。
【００６６】
次に、ステップ１０２で、ジャイロセンサ１４の検出値である自車両のヨーレート、及び速度センサ１６の検出値である自車両の速度を取得して、データ記憶部４０に記憶する。
【００６７】
次に、ステップ１０４で、後述するＧＰＳ情報に基づく自車両方位角処理を実行して、推定方位角を推定し、次に、ステップ１０６で、後述する速度ベクトル算出処理を実行して、速度ベクトルを推定する。
【００６８】
次に、図７を参照して、ＧＰＳ情報に基づく自車両方位角推定処理ルーチンについて説明する。
【００６９】
ステップ１０４０で、上記（１）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度ｖｊを算出する。
【００７０】
次に、ステップ１０４２で、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（ＶＸｊ、ＶＹｊ、ＶＺｊ）を算出する。
【００７１】
次に、ステップ１０４４で、各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、上記（２）〜（４）式に従って、自車両の位置を算出する。なお、ここでは、自車両の位置を、ＧＰＳ衛星の方向（ＧＰＳ衛星と自車両との角度）を求めるために算出しており、自車両の位置として大まかな位置が決定できればよく、例えば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、自車両の位置を決定してもよい。
【００７２】
次に、ステップ１０４６で、上記ステップ１１４で算出された自車両の位置及び取得された各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係Ｒｊ（水平方向に対する仰角θｊ、北方向に対する方位角φｊ）を、各ＧＰＳ衛星の方向として算出する。
【００７３】
次に、ステップ１０４８で、上記ステップ１０４０で算出された各ＧＰＳ衛星に対する自車両の相対速度ｖｊ、上記ステップ１０４２で算出された各ＧＰＳ衛星の速度ベクトルＶｊ（ＶＸｊ、ＶＹｊ、ＶＺｊ）、及び上記ステップ１０４６で算出された各ＧＰＳ衛星の方向Ｒｊ（θｊ、φｊ）に基づいて、上記（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖｊを算出する。（５）式における自車方向のＧＰＳ衛星ｊの速度Ｖｓｊは、Ｖｓｊ＝Ｒｊ［ＶＸｊ，ＶＹｊ，ＶＺｊ］Ｔにより算出する。
【００７４】
次に、ステップ１０５０で、上記（６）式、及び（７）式に従って、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。
【００７５】
次に、ステップ１０５２で、上記ステップ１０５０で算出された自車両の速度ベクトルから三角関数を用いて自車両の方位角を算出し、算出した方位角を推定方位角としてデータ記憶部４０に記憶して、リターンする。
【００７６】
次に、図７を参照して、速度ベクトル推定処理ルーチンについて説明する。
【００７７】
ステップ１０６０で、データ記憶部４０から所定期間分のジャイロセンサ１４の検出値である自車両のヨーレートを読み出し、観測毎のヨーレートを各々積分して時系列に積算して自車両の観測方位角を算出する。
【００７８】
次に、ステップ１０６２で、データ記憶部４０から所定期間内に推定された推定方位角をデータ記憶部４０から読み出し、上記ステップ１０６０で算出した観測方位角の時系列データを平行移動して探索しながら、最小二乗法等に代表される最適計算により、方位角の最適値を推定する。
【００７９】
次に、ステップ１０６４で、上記ステップ１０６２で推定された方位角の最適値と推定方位角の各々との残差を算出し、所定期間分の残差の分散を算出する。
【００８０】
次に、ステップ１０６６で、上記ステップ１０６４で算出した残差の分散が予め定めた閾値以下か否可を判定することにより、方位角の最適値の精度を判定する。残差の分散が閾値以下の場合には、方位角の最適値の精度が高いと判断してステップ１０６８へ移行し、上記ステップ１０６２で推定された方位角の最適値を、方位角の推定値として採用する。一方、残差の分散が閾値を超える場合には、方位角の最適値の精度が低いと判断して、ステップ１０７０へ移行し、過去に推定された方位角を用いてジャイロセンサ１４の検出値を積算して算出した方位角を、方位角の推定値として採用する。
【００８１】
次に、ステップ１０７２で、データ記憶部４０から速度センサ１６で検出された速度を読み出し、上記ステップ１０６８または１０７０で採用された方位角の推定値及び読み出した速度を用いて速度ベクトルを算出し、算出した速度ベクトルを速度ベクトルの推定結果として出力する。
【００８２】
以上説明したように、本実施の形態の車載速度ベクトル推定装置によれば、ＧＰＳ情報に基づいて推定した推定方位角と、ジャイロセンサの検出値に基づく観測方位角とを統合して求めた方位角の最適値と推定方位角との残差の分散が閾値以下か否かによって、最適値の精度、すなわちマルチパスの影響が大きいか否かを判定するため、マルチパスを適切に判定して、精度良く自車両の方位角を推定することができる。また、精度の高い所定期間の推定方位角を使用して観測方位角で補間するため、ＧＰＳ衛星からの衛星信号を受信できない場合や、衛星信号の数が不足する場合でも、精度良く自車両の方位角を推定することができる。
【００８３】
なお、上記実施の形態では、ＧＰＳ情報に基づいて自車両の方位角を推定して精度の判定に用いる場合について説明したが、精度の判定に速度を用いてもよい。具体的には、自車両速度算出部５２で算出された速度ベクトルから自車両の速度の絶対値を算出し、最適推定部６２において、速度センサ１６で検出された速度と算出された速度の絶対値とを統合して、速度の最適値を推定し、この最適値と算出された速度の絶対値との残差の分散に基づいて、精度を判定することができる。そして、精度が高いと判定された所定期間の速度ベクトルから推定方位角を算出して、観測方位角と統合した方位角を推定するようにするとよい。
【００８４】
また、上記実施の形態では、最適値と推定方位角との残差の分散に基づいて、精度を判定する場合について説明したが、これに限定されない。例えば、残差の分布がガウス分布に適合しているか否かにより精度を判定してもよい。この場合、速度ベクトル推定処理（図８）のステップ１０６４で、残差の分布を算出し、ステップ１０６６で、例えばコルモゴロフ−スミルノフ検定等を用いて、残差の分布とガウス分布との適合性を検定すればよい。適合度が高ければ、最適値の精度も高いと判定することができる。
【００８５】
また、上記実施の形態では、観測方位角を得るために、ジャイロセンサを用いる場合について説明したが、慣性航法系のセンサであればよく、磁気方位計でもよい。
【００８６】
また、上記実施の形態では、ＧＰＳ衛星のＧＰＳ情報を用いる場合について説明したが、情報発信源の位置に関する情報、情報発信源と移動体との距離に関する情報、及び情報発信源と移動体との相対速度に関する情報を含む発信源情報を発信する情報発信源からの情報が取得できればよい。例えば、擬似衛星から発信される情報を受信するようにしてもよい。
【００８７】
また、上記実施の形態では、推定した方位角から速度ベクトルを推定して出力する場合について説明したが、速度ベクトルを算出することなく推定した方位角を推定結果として出力してもよいし、速度ベクトルからさらに自車両の走行軌跡を推定して出力するようにしてもよい。
【００８８】
また、上記実施の形態では、車両に搭載される速度ベクトル装置について説明したが、本発明の方位角推定装置が搭載される移動体は車両に限定されない。例えば、方位角推定装置をロボットに搭載してもよいし、歩行者が携帯できるように方位角推定装置をポータブル端末として構成するようにしてもよい。
【符号の説明】
【００８９】
１０ 車載速度ベクトル推定装置
１２ ＧＰＳ受信部
１４ ジャイロセンサ
１６ 速度センサ
１８ コンピュータ
２０ ＧＰＳ情報取得部
３０ 方位角推定部
４０ データ記憶部
４２ 相対速度算出部
４４ 衛星速度算出部
４６ 自車位置算出部
４８ 衛星方向算出部
５０ 衛星方向自車速算出部
５２ 自車両速度算出部
５４ 自車両方位角算出部
６０ 速度ベクトル推定部
６２ 最適推定部
６４ 精度判定部
６６ 速度ベクトル算出部
【技術分野】
【０００１】
本発明は、方位角推定装置及びプログラムに係り、特に、移動体の方位角を推定するための方位角推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、衛星信号を利用して、高層ビル街などのマルチパスの影響が大きい環境下でも、移動体の最適な速度ベクトルを算出するシステムが提案されている（例えば、特許文献１参照）。特許文献１記載の速度ベクトル決定システムは、衛星信号追尾装置で追尾している複数衛星の中から、衛星軌道情報に基づく衛星仰角情報及び追尾情報に基づく信号レベル情報を利用して、２つの衛星が共通に含まれ且つ他の衛星が含まれ、３以上の衛星信号からなる複数の組み合わせを求める。そして、その複数の組み合わせ毎の速度ベクトル解をそれぞれ演算して速度ベクトル解候補を得、且つそれら各速度ベクトル解候補間の一致度に基づいて、最適速度ベクトルを求めている。
【０００３】
また、測位衛星からの複数の擬似距離と、測位航法部と慣性航法部とカルマンフィルタと、により移動体の航法位置を求める移動体測位装置が提案されている（例えば、特許文献２参照）。特許文献２に記載の移動体測位装置は、カルマンフィルタの出力から得られるドップラ推定値とドップラ観測値との差の絶対値を所定の条件で比較してマルチパスを判定している。
【０００４】
また、誤差の影響の少ないＧＰＳデータを選択的に用いるＧＰＳナビゲーション装置が提案されている（例えば、特許文献３参照）。特許文献３に記載のＧＰＳナビゲーション装置では、ＧＰＳ速度計測部からの移動ベクトルからＧＰＳ方位を求め、ＧＰＳ方位、ジャイロセンサで検出された移動体の回転角速度、及び地図メモリの道路リンク情報から移動体の進行方位し、新たなＧＰＳデータが検出される毎に、当該データに基づいて算出されたＧＰＳ方位とその時点で求められている推定方位とを比較することで、当該データに含まれる誤差の影響の大きさを判定し、その影響が大きい場合には当該データを位置推定に用いないように制御している。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００６−２０８３９２号公報
【特許文献２】特許第３８７５７１４号公報
【特許文献３】特開平８−３３４３３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記の特許文献１に記載の技術では、都市部などでは、高仰角の衛星からもマルチパスが発生する場合もあるが、この点が考慮されておらず、最適な速度ベクトルを推定することができない場合がある、という問題がある。また、同様に都市部などでは、マルチパスが発生している衛星が受信している衛星群が過半数を超える場合も多く、そのような場合には、組み合わせの変更により最適な速度ベクトルを推定することができない、という問題がある。さらに、衛星信号を受信できない場合には、速度ベクトルを推定することができない、という問題がある。
【０００７】
このような問題に対応するために、特許文献２に記載の技術のように、衛星信号と慣性センサの検出値とを統合して移動体に関する物理量を推定することが考えられる。しかし、特許文献２に記載の技術では、カルマンフィルタにより最適推定値を求めてマルチパスの判定に用いているが、カルマンフィルタに入力される衛星信号の情報に多数のマルチパスの影響が含まれている場合には、最適推定値が真値から外れる可能性が高まるため、誤差が発生している状況の観測値と比較したとしても、マルチパス誤差が未検出になる可能性がある、という問題がある。また、推定値に誤差があり、観測値の方が正しい場合においては、正しい観測値がマルチパス誤差として誤判定される可能性が高い、という問題がある。加えて、特許文献２に記載の技術では、擬似距離を用いて速度や位置といった物理量を算出しているため、推定する物理量は擬似距離の精度の影響を大きく受ける、という問題がある。
【０００８】
また、特許文献３に記載の技術では、ＧＰＳデータの擬似距離変化率から車両の進行方位を推定し、時系列のデータをジャイロセンサの計測値と併せて比較することで、ＧＰＳデータの誤差が少ない地点を判定している。しかし、マルチパス誤差は場所に依存するため、マルチパスが発生している場所において、短い時間で計測された２つの方位角には、同じ傾向の誤差が発生している可能性が高く、マルチパス誤差が未検出になる可能性がある、という問題がある。また、マルチパスが発生しない場所においても、ＧＰＳデータにはガウス性の誤差が発生している。そのため、計測した車両の方位角には少なくともランダム性の誤差が生じるため、この計測値と推定値との差を用いてマルチパス誤差を判定したとしても、閾値の設定次第では誤検出や未検出が頻発してしまう可能性がある、という問題がある。誤判定を防ぐために、閾値を小さくしたとしても、前述の理由によりマルチパス誤差の未検出を防ぐことはできない。
【０００９】
本発明は、上記の問題点を解決するためになされたもので、マルチパスを適切に判定して、精度良く移動体の方位角を推定することができる方位角推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記目的を達成するために、本発明の方位角推定装置は、移動体に搭載される方位角推定装置であって、複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、前記移動体の方位角に関する物理量を検出する検出手段と、前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルから前記移動体の方位角を算出する算出手段と、前記算出手段により算出された方位角、及び前記検出手段により検出された物理量から得られる方位角に基づいて、前記移動体の方位角の最適値を推定する最適値推定手段と、前記最適値推定手段により推定された最適値と前記算出手段により算出された方位角との差の分布に基づいて、前記方位角の最適値の精度を判定する判定手段と、前記判定手段により精度が予め定めた閾値よりも高いと判定されたときの最適値を用いて前記移動体の方位角を推定する方位角推定手段と、を含んで構成されている。
【００１１】
本発明の方位角推定装置によれば、取得手段が、移動体に搭載される方位角推定装置であって、複数の情報発信源の各々から発信された情報発信源の各々の位置に関する情報、情報発信源の各々と移動体との距離に関する情報、及び情報発信源の各々に対する移動体の相対速度に関する情報を含む発信源情報を取得し、検出手段が、移動体の方位角に関する物理量を検出する。検出手段は慣性センサであり、例えば、自車両の角速度または角加速度を検出するジャイロセンサや、方位角を検出する磁気方位計を用いることができる。
【００１２】
そして、算出手段が、取得手段により取得された発信源情報に基づいて得られる移動体の速度ベクトルから移動体の方位角を算出し、最適値推定手段が、算出手段により算出された方位角、及び検出手段により検出された物理量から得られる方位角に基づいて、移動体の方位角の最適値を推定する。算出手段により算出された移動体の方位角は絶対量であり、検出手段により検出された物理量に基づく移動体の方位角は相対量であり、これらを統合して最適値を推定する場合に、マルチパスなどによる絶対量のばらつきが最適値の精度に影響を与える。そこで、判定手段は、推定した最適値と算出手段により算出された方位角との差の分布に基づいて、方位角の最適値の精度を判定する。そして、方位角推定手段が、判定手段により精度が予め定めた閾値よりも高いと判定されたときの最適値を用いて移動体の方位角を推定する。
【００１３】
このように、発信源情報に基づいて算出された絶対量である移動体の方位角と、検出手段の検出値に基づく相対量である移動体の方位角とを統合して移動体の方位角の最適値を推定し、最適値と算出された方位角との差の分布に基づいて最適値の精度を判定するため、マルチパスを適切に判定して、精度良く移動体の方位角を推定することができる。
【００１４】
また、前記判定手段は、前記差のばらつきが予め定めた閾値以下か否か、または前記差の分布がガウス分布に適合するか否かを判定することによって、前記方位角の最適値の精度を判定することができる。最適値と算出された方位角との差のばらつきが小さい場合、または差の分布がガウス分布に適合する場合には、最適値の精度が高いと判定することができる。
【００１５】
また、前記方位角推定手段は、前記判定手段により精度が予め定めた閾値よりも高いと判定された場合には、前記最適値を前記移動体の方位角として推定し、前記判定手段により精度が予め定めた閾値よりも低いと判定された場合には、過去に推定された方位角及び前記検出手段により検出された物理量に基づいて、前記移動体の方位角を推定することができる。これにより、絶対量として算出された方位角を相対量として検出された方位角で補間して、精度良く移動体の方位角を推定することができる。
【００１６】
また、前記最適値推定手段は、所定の初期値を基準に前記検出手段により検出された物理量を所定時間分積分して前記検出手段により検出された物理量から得られる方位角を算出し、該方位角と前記算出手段により算出された方位角との一定時間における差の和が最小となる前記初期値を探索することで、前記方位角の最適値を推定することができる。
【００１７】
また、前記算出手段は、前記情報発信源の各々の位置に関する情報、及び前記情報発信源の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記情報発信源の各々の方向を算出し、時系列の前記情報発信源の各々の位置に関する情報に基づいて、前記情報発信源の各々の速度を算出し、前記移動体から見た前記情報発信源の各々の方向、前記情報発信源の各々の速度、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体の情報発信源の各々の方向の速度を算出し、複数の前記移動体の情報発信源の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出することができる。
【００１８】
また、前記情報発信源を、前記情報発信源の各々の位置に関する情報として衛星軌道情報を、前記情報発信源の各々と前記移動体との距離に関する情報として擬似距離情報を、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報としてドップラー周波数情報を発信するＧＰＳ衛星とすることができる。情報発信源としては、例えば、擬似衛星やビーコンなどのように、発信源情報を発信するものであればよいが、代表的には、ＧＰＳ衛星とすることができる。
【００１９】
また、本発明の方位角推定プログラムは、コンピュータを、移動体に搭載される方位角推定装置の各手段として機能させるための方位角推定プログラムであって、前記コンピュータを、複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段、前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルから前記移動体の方位角を算出する算出手段、前記算出手段により算出された方位角、及び前記移動体の方位角に関する物理量を検出する検出手段により検出された物理量から得られる方位角に基づいて、前記移動体の方位角の最適値を推定する最適値推定手段、前記最適値推定手段により推定された最適値と前記算出手段により算出された方位角との差の分布に基づいて、前記方位角の最適値の精度を判定する判定手段、及び前記判定手段により精度が予め定めた閾値よりも高いと判定されたときの最適値を用いて前記移動体の方位角を推定する方位角推定手段として機能させるためのプログラムである。
【００２０】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２１】
以上説明したように、本発明の方位角推定装置及びプログラムによれば、発信源情報に基づいて算出された絶対量である移動体の方位角と、検出手段の検出値に基づく相対量である移動体の方位角とを統合して移動体の方位角の最適値を推定し、最適値と算出された方位角との差の分布に基づいて最適値の精度を判定するため、マルチパスを適切に判定して、精度良く移動体の方位角を推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２２】
【図１】本実施の形態に係る車載速度ベクトル推定装置を示すブロック図である。
【図２】三角測量の原理に従って自車両の位置が推定される様子を示すイメージ図である。
【図３】各ＧＰＳ衛星の速度ベクトル及び各ＧＰＳ衛星の方向に基づいて、ＧＰＳ衛星方向の自車両の速度を算出する様子を示すイメージ図である。
【図４】（ａ）マルチパスの影響がない場合、及び（ｂ）マルチパスの影響がある場合における最適値の推定を説明するための図である。
【図５】（ａ）マルチパスの影響がない場合、及び（ｂ）マルチパスの影響がある場合における最適値と推定方位角との残差の分布を示す図である。
【図６】本実施の形態に係る車載速度ベクトル推定装置のコンピュータにおける速度ベクトル推定処理ルーチンの内容を示すフローチャートである。
【図７】本実施の形態に係る車載速度ベクトル推定装置のコンピュータにおけるＧＰＳ情報に基づく自車両方位角推定処理ルーチンの内容を示すフローチャートである。
【図８】本実施の形態に係る車載速度ベクトル推定装置のコンピュータにおける速度ベクトル算出処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００２３】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、ＧＰＳ衛星から発信されたＧＰＳ情報を取得して自車両の方位角を推定し、推定した方位角から自車両の速度ベクトルを推定する車載速度ベクトル推定装置に、本発明を適用した場合を例に説明する。
【００２４】
図１に示すように、本実施の形態に係る車載速度ベクトル推定装置１０は、ＧＰＳ衛星からの電波を受信するＧＰＳ受信部１２と、自車両のヨーレートを検出するジャイロセンサ１４と、自車両の速度を検出する速度センサ１６と、ＧＰＳ受信部１２によって受信されたＧＰＳ衛星からの受信信号、並びにジャイロセンサ１４及び速度センサ１６の検出値に基づいて、自車両の速度ベクトルを推定する処理を実行するコンピュータ１８と、を備えている。
【００２５】
ＧＰＳ受信部１２は、複数のＧＰＳ衛星からの電波を受信して、受信した全てのＧＰＳ衛星からの受信信号から、ＧＰＳ衛星の情報として、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報（エフェメリス）、ＧＰＳ衛星が電波を送信した時刻、受信信号の強度、周波数などを取得し、コンピュータ１８に出力する。
【００２６】
コンピュータ１８は、ＣＰＵ、後述する速度ベクトル推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。
【００２７】
コンピュータ１８を以下で説明する速度ベクトル推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ擬似距離データ、ドップラー周波数、及びＧＰＳ衛星の位置座標を算出して取得するＧＰＳ情報取得部２０と、取得したＧＰＳ情報に基づいて、自車両の方位角を推定する方位角推定部３０と、推定された方位角、ジャイロセンサ１４の検出値、及び速度センサ１６の検出値を対応させて記憶するデータ記憶部４０と、データ記憶部４０に記憶された各種データに基づいて、自車両の速度ベクトルを推定する速度ベクトル推定部６０と、を含んだ構成で表すことができる。
【００２８】
ＧＰＳ情報取得部２０は、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ衛星が電波を送信した時刻及び自車両で電波を受信した時刻に基づいて、ＧＰＳ擬似距離データを算出する。また、ＧＰＳ情報取得部２０は、各ＧＰＳ衛星から送信される信号の既知の周波数と、各ＧＰＳ衛星から受信した受信信号の周波数とに基づいて、各ＧＰＳ衛星からの受信信号のドップラー周波数を各々算出する。なお、ドップラー周波数は、ＧＰＳ衛星と自車との相対速度による、搬送波周波数のドップラーシフト量を観測したものである。また、ＧＰＳ情報取得部２０は、ＧＰＳ衛星の軌道情報及びＧＰＳ衛星が電波を送信した時刻に基づいて、ＧＰＳ衛星の位置座標を各々算出する。
【００２９】
また、方位角推定部３０は、さらに、取得した各ＧＰＳ衛星のドップラー周波数に基づいて、各ＧＰＳ衛星に対する自車両の相対速度を算出する相対速度算出部４２と、取得した各ＧＰＳ衛星の位置座標の時系列データに基づいて、各ＧＰＳ衛星の速度ベクトルを算出する衛星速度算出部４４と、取得した各ＧＰＳ衛星のＧＰＳ擬似距離データに基づいて、自車両の位置を算出する自車位置算出部４６と、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星の方向（角度の関係）を算出する衛星方向算出部４８と、算出された相対速度、各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の方向に基づいて、各ＧＰＳ衛星方向の自車両の速度を算出する衛星方向自車速算出部５０と、算出された複数の各ＧＰＳ衛星方向の自車両の速度に基づいて、自車両の速度ベクトルを算出する自車両速度算出部５２と、算出された自車両の速度ベクトルに基づいて自車両の方位角を算出する自車両方位角算出部５４と、を備えた構成で表すことができる。
【００３０】
相対速度算出部４２は、ドップラー周波数とＧＰＳ衛星に対する相対速度との関係を表わす以下の（１）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度を算出する。
【００３１】
【数１】
【００３２】
ただし、ｖｊはＧＰＳ衛星ｊに対する相対速度であり、Ｄ１ｊはＧＰＳ衛星ｊから得られるドップラー周波数（ドップラーシフト量）である。また、Ｃは光速であり、Ｆ１は、ＧＰＳ衛星から送信される信号の既知のＬ１周波数である。
【００３３】
衛星速度算出部４４は、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（３次元速度ＶＸｊ、ＶＹｊ、ＶＺｊ）を算出する。例えば、非特許文献（Ｐｒａｔａｐ Ｍｉｓｒａ ａｎｄ Ｐｅｒ Ｅｎｇｅ原著 日本航海学会ＧＰＳ研究会訳：“精説ＧＰＳ基本概念・測位原理・信号と受信機”正陽文庫，２００４．）に記載された方法を用いて、各ＧＰＳ衛星の速度ベクトルを算出することができる。
【００３４】
自車位置算出部４６は、以下のように、ＧＰＳ情報取得部２０によって取得された各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。
【００３５】
ＧＰＳを用いた測位では、図２に示すように、既知であるＧＰＳ衛星の位置座標と、各ＧＰＳ衛星から受信した受信信号の伝播距離である擬似距離とに基づいて、三角測量の原理に従って、自車両の位置が推定される。
【００３６】
ここで、ＧＰＳ衛星までの真の距離ｒｊは、以下の（２）式で表され、ＧＰＳで観測される擬似距離ρｊは、以下の（３）式で表される。
【００３７】
【数２】
【００３８】
ただし、（Ｘｊ，Ｙｊ，Ｚｊ）がＧＰＳ衛星ｊの位置座標であり、（ｘ，ｙ，ｚ）が自車両の位置座標である。ｓは、ＧＰＳ受信部１２の時計誤差による距離誤差である。
【００３９】
上記（２）式、（３）式より、４つ以上のＧＰＳ衛星のＧＰＳ擬似距離データから得られる以下の（４）式の連立方程式を解くことによって、自車両の位置（ｘ、ｙ、ｚ）が算出される。
【００４０】
【数３】
【００４１】
なお、本実施の形態では、自車両の位置を、ＧＰＳ衛星の方向（ＧＰＳ衛星と自車両との角度）を求めるために算出しているが、遠方に存在するＧＰＳ衛星の方向であるため、位置決めは大まかでよく、擬似距離を用いた位置決定を行わなくてもよい。時刻の影響が少なく、システム等で許容される推定精度に依存するが、自車の位置決めの誤差も数百ｍの範囲であれば、速度推定誤差も１ｍ／ｓｅｃ以下程度となり大きな問題はないため、例えば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、自車両の位置を決定してもよい。
【００４２】
衛星方向算出部４８は、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係（水平方向に対する仰角θｊ、北方向に対する方位角φｊ）を、各ＧＰＳ衛星の方向として算出する。
【００４３】
衛星方向自車速算出部５０は、図３に示すように、算出された各ＧＰＳ衛星に対する自車両の相対速度ｖｊ、各ＧＰＳ衛星の速度ベクトル（ＶＸｊ、ＶＹｊ、ＶＺｊ）、及び各ＧＰＳ衛星の方向Ｒｊ（θｊ、φｊ）に基づいて、以下の（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖｊを算出する。
【００４４】
【数４】
【００４５】
ｖｊは、ＧＰＳ衛星ｊに対する自車両の相対速度（衛星方向におけるＧＰＳ衛星との相対速度）である。また、Ｖｓｊは、自車方向のＧＰＳ衛星ｊの速度であり、Ｖｓｊ＝Ｒｊ［ＶＸｊ，ＶＹｊ，ＶＺｊ］Ｔにより求まる。また、Ｖｖｊは、ＧＰＳ衛星ｊの方向の自車速であり、ｖＣｂは、クロックバイアス変動である。
【００４６】
上述したように、ＧＰＳ衛星方向の自車速は、ＧＰＳ衛星位置の三次元位置ではなく、ＧＰＳ衛星との方位関係によってのみ算出される。ＧＰＳ衛星は遥か遠方にあり、１日で地球をほぼ２周するため１分間の角度変化は０．５度である。通常、ＧＰＳ衛星とＧＰＳ受信機との時計誤差は通常１ｍｓｅｃ以下であるため、ＧＰＳ衛星との方位関係に大きな影響はない。また、同じくＧＰＳ衛星は遥か遠方にあるため、自車の位置決定に数１００ｍ程度の誤差が生じていたとしても、ＧＰＳ衛星との方位関係に大きな影響はない。このため、擬似距離に誤差が乗りやすい状況であったとしても、ＧＰＳ衛星方向の自車速は、擬似距離に比較して正確に算出され得る。
【００４７】
自車両速度算出部５２は、以下に説明するように、自車両の速度ベクトルの最適推定を行う。
【００４８】
まず、自車の速度ベクトルを（Ｖｘ，Ｖｙ，Ｖｚ）としたとき、ＧＰＳ衛星方向の自車両の速度Ｖｖｊとの関係は以下の（６）式で表される。
【００４９】
【数５】
【００５０】
各ＧＰＳ衛星ｊについて得られる上記（６）式より、Ｖｘ，Ｖｙ，Ｖｚ及びＣｂを推定値とした、以下の（７）式で表される連立方程式が得られる。
【００５１】
【数６】
【００５２】
電波を受信したＧＰＳ衛星が４個以上である場合に、上記（７）式の連立方程式を解くことによって、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。
【００５３】
自車両方位角算出部５４は、自車両速度算出部５２により算出された自車両の速度ベクトルから三角関数を用いて自車両の方位角を算出し、方位角推定部３０での方位角の推定結果としてデータ記憶部４０に記憶する。ここで推定された方位角は、ＧＰＳ情報に基づいて推定された方位角であるので、以下、「推定方位角」という。これに対し、ジャイロセンサ１４の検出値から求まる自車両の方位角を「観測方位角」という。ただし、ジャイロセンサ１４では、方位角の変化分しか計測できないため、ある初期値を基準に時間経過分を積分することで、ジャイロセンサ１４の検出値から観測方位角を算出する。例えば、下記（８）式では、ある時間ｔ０の初期値ψｉｎｉｔ（ｔ０）を基準にｋ秒前の観測方位角ψｍ（ｔ０−ｋ）を算出している。つまり、データ記憶部４０にジャイロセンサ１４の検出値ψｇｙｒｏの微分値を記憶しておくことで、観測方位角が算出可能になる。
【００５４】
【数７】
【００５５】
なお、データ記憶部４０には、所定時間分の推定方位角と共に、対応する所定時間内に検出されたジャイロセンサ１４の検出値、及び速度センサ１６の検出値が記憶される。
【００５６】
また、速度ベクトル推定部６０は、さらに、推定方位角と観測方位角とを統合して方位角の最適値を推定する最適推定部６２と、推定された最適値と推定方位角との残差の分散に基づいて、推定方位角の精度を判定する精度判定部６４と、精度判定部６４での判定結果に基づいて自車両の方位角を推定し、推定した方位角を用いて速度ベクトルを算出する速度ベクトル算出部６６と、を含んだ構成で表すことができる。
【００５７】
最適推定部６２は、データ記憶部４０から所定期間分のジャイロセンサ１４の検出値である自車両のヨーレートを読み出し、観測毎のヨーレートを各々積分して時系列に積算して自車両の観測方位角を算出する。また、対応する所定期間内に推定された推定方位角をデータ記憶部４０から読み出す。
【００５８】
ここで、ＧＰＳ情報に基づいて推定された推定方位角は、マルチパスや大気に起因するＧＰＳの誤差要因により、推定結果にばらつきがあるという特徴を持つ。一方、ジャイロセンサ１４の検出値に基づく観測方位角は、連続した角度変化は精度よく推定できるが、絶対値の信頼度が不明であるという特徴を持つ。そこで、最適推定部６２は、最小二乗法等に代表される最適計算により、推定方位角と観測方位角とを統合した方位角の最適値を推定する。例えば、最小二乗法では、下記（９）式により推定方位角ψｇｐｓと、観測方位角ψｍとの一定時間における差の和が最小になるように、観測方位角ψｍの算出の基準となる最適な初期値ψｉｎｉｔ（ｔ０）を探索することで、方位角の最適値を推定する。
【００５９】
【数８】
【００６０】
精度判定部６４は、最適推定部６２で推定された方位角の最適値と推定方位角の各々との残差を算出し、所定期間分の残差のばらつき（分散または標準偏差）が所定の閾値以下か否かにより、方位角の最適値の精度を判定する。
【００６１】
ここで、残差のばらつきを用いて方位角の最適値の精度を判定する原理について説明する。図４（ａ）にマルチパスの影響がない場合、同図（ｂ）にマルチパスの影響がある場合の方位角の最適値を示す。マルチパスの影響がある場合には、残差が大きくなる外れ値が生じ、最適値が真値より外れる可能性が高まる。次に、図５（ａ）及び（ｂ）に、図４（ａ）及び（ｂ）の各々の場合における残差のヒストグラムを示す。マルチパスの影響がある場合は、マルチパスの影響がない場合と比較して、分散値が大きくなる傾向にある。従って、分散値を閾値と比較することで、方位角の最適値の精度を判定することができる。
【００６２】
速度ベクトル算出部６６は、最適推定部６２により推定された所定期間分の方位角の最適値が精度判定部６４により精度が高いと判定された場合には、その方位角の最適値及び速度センサ１６で検出された速度を用いて、速度ベクトルを算出する。一方、精度が低いと判定された最適値については、速度ベクトルの算出に用いない。その間の速度ベクトルは、過去に推定された方位角を用いてジャイロセンサ１４の検出値を積算して方位角を算出して、速度センサ１６で検出された速度を用いて算出する。速度ベクトル算出部６６は、算出した速度ベクトルを速度ベクトルの推定結果として出力する。
【００６３】
次に、本実施の形態に係る車載速度ベクトル推定装置１０の作用について説明する。
【００６４】
ＧＰＳ受信部１２によって、複数のＧＰＳ衛星から電波を受信しているときに、コンピュータ１８において、図６に示す速度ベクトル推定処理ルーチンが実行される。
【００６５】
ステップ１００で、ＧＰＳ受信部１２から複数のＧＰＳ衛星の情報を取得すると共に、複数のＧＰＳ衛星のＧＰＳ擬似距離データ、ドップラー周波数、ＧＰＳ衛星の位置座標を算出して取得する。
【００６６】
次に、ステップ１０２で、ジャイロセンサ１４の検出値である自車両のヨーレート、及び速度センサ１６の検出値である自車両の速度を取得して、データ記憶部４０に記憶する。
【００６７】
次に、ステップ１０４で、後述するＧＰＳ情報に基づく自車両方位角処理を実行して、推定方位角を推定し、次に、ステップ１０６で、後述する速度ベクトル算出処理を実行して、速度ベクトルを推定する。
【００６８】
次に、図７を参照して、ＧＰＳ情報に基づく自車両方位角推定処理ルーチンについて説明する。
【００６９】
ステップ１０４０で、上記（１）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度ｖｊを算出する。
【００７０】
次に、ステップ１０４２で、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（ＶＸｊ、ＶＹｊ、ＶＺｊ）を算出する。
【００７１】
次に、ステップ１０４４で、各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、上記（２）〜（４）式に従って、自車両の位置を算出する。なお、ここでは、自車両の位置を、ＧＰＳ衛星の方向（ＧＰＳ衛星と自車両との角度）を求めるために算出しており、自車両の位置として大まかな位置が決定できればよく、例えば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、自車両の位置を決定してもよい。
【００７２】
次に、ステップ１０４６で、上記ステップ１１４で算出された自車両の位置及び取得された各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係Ｒｊ（水平方向に対する仰角θｊ、北方向に対する方位角φｊ）を、各ＧＰＳ衛星の方向として算出する。
【００７３】
次に、ステップ１０４８で、上記ステップ１０４０で算出された各ＧＰＳ衛星に対する自車両の相対速度ｖｊ、上記ステップ１０４２で算出された各ＧＰＳ衛星の速度ベクトルＶｊ（ＶＸｊ、ＶＹｊ、ＶＺｊ）、及び上記ステップ１０４６で算出された各ＧＰＳ衛星の方向Ｒｊ（θｊ、φｊ）に基づいて、上記（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖｊを算出する。（５）式における自車方向のＧＰＳ衛星ｊの速度Ｖｓｊは、Ｖｓｊ＝Ｒｊ［ＶＸｊ，ＶＹｊ，ＶＺｊ］Ｔにより算出する。
【００７４】
次に、ステップ１０５０で、上記（６）式、及び（７）式に従って、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。
【００７５】
次に、ステップ１０５２で、上記ステップ１０５０で算出された自車両の速度ベクトルから三角関数を用いて自車両の方位角を算出し、算出した方位角を推定方位角としてデータ記憶部４０に記憶して、リターンする。
【００７６】
次に、図７を参照して、速度ベクトル推定処理ルーチンについて説明する。
【００７７】
ステップ１０６０で、データ記憶部４０から所定期間分のジャイロセンサ１４の検出値である自車両のヨーレートを読み出し、観測毎のヨーレートを各々積分して時系列に積算して自車両の観測方位角を算出する。
【００７８】
次に、ステップ１０６２で、データ記憶部４０から所定期間内に推定された推定方位角をデータ記憶部４０から読み出し、上記ステップ１０６０で算出した観測方位角の時系列データを平行移動して探索しながら、最小二乗法等に代表される最適計算により、方位角の最適値を推定する。
【００７９】
次に、ステップ１０６４で、上記ステップ１０６２で推定された方位角の最適値と推定方位角の各々との残差を算出し、所定期間分の残差の分散を算出する。
【００８０】
次に、ステップ１０６６で、上記ステップ１０６４で算出した残差の分散が予め定めた閾値以下か否可を判定することにより、方位角の最適値の精度を判定する。残差の分散が閾値以下の場合には、方位角の最適値の精度が高いと判断してステップ１０６８へ移行し、上記ステップ１０６２で推定された方位角の最適値を、方位角の推定値として採用する。一方、残差の分散が閾値を超える場合には、方位角の最適値の精度が低いと判断して、ステップ１０７０へ移行し、過去に推定された方位角を用いてジャイロセンサ１４の検出値を積算して算出した方位角を、方位角の推定値として採用する。
【００８１】
次に、ステップ１０７２で、データ記憶部４０から速度センサ１６で検出された速度を読み出し、上記ステップ１０６８または１０７０で採用された方位角の推定値及び読み出した速度を用いて速度ベクトルを算出し、算出した速度ベクトルを速度ベクトルの推定結果として出力する。
【００８２】
以上説明したように、本実施の形態の車載速度ベクトル推定装置によれば、ＧＰＳ情報に基づいて推定した推定方位角と、ジャイロセンサの検出値に基づく観測方位角とを統合して求めた方位角の最適値と推定方位角との残差の分散が閾値以下か否かによって、最適値の精度、すなわちマルチパスの影響が大きいか否かを判定するため、マルチパスを適切に判定して、精度良く自車両の方位角を推定することができる。また、精度の高い所定期間の推定方位角を使用して観測方位角で補間するため、ＧＰＳ衛星からの衛星信号を受信できない場合や、衛星信号の数が不足する場合でも、精度良く自車両の方位角を推定することができる。
【００８３】
なお、上記実施の形態では、ＧＰＳ情報に基づいて自車両の方位角を推定して精度の判定に用いる場合について説明したが、精度の判定に速度を用いてもよい。具体的には、自車両速度算出部５２で算出された速度ベクトルから自車両の速度の絶対値を算出し、最適推定部６２において、速度センサ１６で検出された速度と算出された速度の絶対値とを統合して、速度の最適値を推定し、この最適値と算出された速度の絶対値との残差の分散に基づいて、精度を判定することができる。そして、精度が高いと判定された所定期間の速度ベクトルから推定方位角を算出して、観測方位角と統合した方位角を推定するようにするとよい。
【００８４】
また、上記実施の形態では、最適値と推定方位角との残差の分散に基づいて、精度を判定する場合について説明したが、これに限定されない。例えば、残差の分布がガウス分布に適合しているか否かにより精度を判定してもよい。この場合、速度ベクトル推定処理（図８）のステップ１０６４で、残差の分布を算出し、ステップ１０６６で、例えばコルモゴロフ−スミルノフ検定等を用いて、残差の分布とガウス分布との適合性を検定すればよい。適合度が高ければ、最適値の精度も高いと判定することができる。
【００８５】
また、上記実施の形態では、観測方位角を得るために、ジャイロセンサを用いる場合について説明したが、慣性航法系のセンサであればよく、磁気方位計でもよい。
【００８６】
また、上記実施の形態では、ＧＰＳ衛星のＧＰＳ情報を用いる場合について説明したが、情報発信源の位置に関する情報、情報発信源と移動体との距離に関する情報、及び情報発信源と移動体との相対速度に関する情報を含む発信源情報を発信する情報発信源からの情報が取得できればよい。例えば、擬似衛星から発信される情報を受信するようにしてもよい。
【００８７】
また、上記実施の形態では、推定した方位角から速度ベクトルを推定して出力する場合について説明したが、速度ベクトルを算出することなく推定した方位角を推定結果として出力してもよいし、速度ベクトルからさらに自車両の走行軌跡を推定して出力するようにしてもよい。
【００８８】
また、上記実施の形態では、車両に搭載される速度ベクトル装置について説明したが、本発明の方位角推定装置が搭載される移動体は車両に限定されない。例えば、方位角推定装置をロボットに搭載してもよいし、歩行者が携帯できるように方位角推定装置をポータブル端末として構成するようにしてもよい。
【符号の説明】
【００８９】
１０ 車載速度ベクトル推定装置
１２ ＧＰＳ受信部
１４ ジャイロセンサ
１６ 速度センサ
１８ コンピュータ
２０ ＧＰＳ情報取得部
３０ 方位角推定部
４０ データ記憶部
４２ 相対速度算出部
４４ 衛星速度算出部
４６ 自車位置算出部
４８ 衛星方向算出部
５０ 衛星方向自車速算出部
５２ 自車両速度算出部
５４ 自車両方位角算出部
６０ 速度ベクトル推定部
６２ 最適推定部
６４ 精度判定部
６６ 速度ベクトル算出部
【特許請求の範囲】
【請求項１】
移動体に搭載される方位角推定装置であって、
複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、
前記移動体の方位角に関する物理量を検出する検出手段と、
前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルから前記移動体の方位角を算出する算出手段と、
前記算出手段により算出された方位角、及び前記検出手段により検出された物理量から得られる方位角に基づいて、前記移動体の方位角の最適値を推定する最適値推定手段と、
前記最適値推定手段により推定された最適値と前記算出手段により算出された方位角との差の分布に基づいて、前記方位角の最適値の精度を判定する判定手段と、
前記判定手段により精度が予め定めた閾値よりも高いと判定されたときの最適値を用いて前記移動体の方位角を推定する方位角推定手段と、
を含む方位角推定装置。
【請求項２】
前記判定手段は、前記差のばらつきが予め定めた閾値以下か否か、または前記差の分布がガウス分布に適合するか否かを判定することによって、前記方位角の最適値の精度を判定する請求項１記載の方位角推定装置。
【請求項３】
前記方位角推定手段は、前記判定手段により精度が予め定めた閾値よりも高いと判定された場合には、前記最適値を前記移動体の方位角として推定し、前記判定手段により精度が予め定めた閾値よりも低いと判定された場合には、過去に推定された方位角及び前記検出手段により検出された物理量に基づいて、前記移動体の方位角を推定する請求項１または請求項２記載の方位角推定装置。
【請求項４】
前記最適値推定手段は、所定の初期値を基準に前記検出手段により検出された物理量を所定時間分積分して前記検出手段により検出された物理量から得られる方位角を算出し、該方位角と前記算出手段により算出された方位角との一定時間における差の和が最小となる前記初期値を探索することで、前記方位角の最適値を推定する請求項１〜請求項３のいずれか１項記載の方位角推定装置。
【請求項５】
前記算出手段は、前記情報発信源の各々の位置に関する情報、及び前記情報発信源の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記情報発信源の各々の方向を算出し、時系列の前記情報発信源の各々の位置に関する情報に基づいて、前記情報発信源の各々の速度を算出し、前記移動体から見た前記情報発信源の各々の方向、前記情報発信源の各々の速度、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体の情報発信源の各々の方向の速度を算出し、複数の前記移動体の情報発信源の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出する請求項１〜請求項４のいずれか１項記載の方位角推定装置。
【請求項６】
前記情報発信源を、前記情報発信源の各々の位置に関する情報として衛星軌道情報を、前記情報発信源の各々と前記移動体との距離に関する情報として擬似距離情報を、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報としてドップラー周波数情報を発信するＧＰＳ衛星とした請求項１〜請求項５のいずれか１項記載の方位角推定装置。
【請求項７】
コンピュータを、移動体に搭載される方位角推定装置の各手段として機能させるための方位角推定プログラムであって、
前記コンピュータを、
複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段、
前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルから前記移動体の方位角を算出する算出手段、
前記算出手段により算出された方位角、及び前記移動体の方位角に関する物理量を検出する検出手段により検出された物理量から得られる方位角に基づいて、前記移動体の方位角の最適値を推定する最適値推定手段、
前記最適値推定手段により推定された最適値と前記算出手段により算出された方位角との差の分布に基づいて、前記方位角の最適値の精度を判定する判定手段、及び
前記判定手段により精度が予め定めた閾値よりも高いと判定されたときの最適値を用いて前記移動体の方位角を推定する方位角推定手段
として機能させるための方位角推定プログラム。
【請求項８】
コンピュータを、請求項１〜請求項６のいずれか１項記載の方位角推定装置の各手段として機能させるための方位角推定プログラム。
【請求項１】
移動体に搭載される方位角推定装置であって、
複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、
前記移動体の方位角に関する物理量を検出する検出手段と、
前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルから前記移動体の方位角を算出する算出手段と、
前記算出手段により算出された方位角、及び前記検出手段により検出された物理量から得られる方位角に基づいて、前記移動体の方位角の最適値を推定する最適値推定手段と、
前記最適値推定手段により推定された最適値と前記算出手段により算出された方位角との差の分布に基づいて、前記方位角の最適値の精度を判定する判定手段と、
前記判定手段により精度が予め定めた閾値よりも高いと判定されたときの最適値を用いて前記移動体の方位角を推定する方位角推定手段と、
を含む方位角推定装置。
【請求項２】
前記判定手段は、前記差のばらつきが予め定めた閾値以下か否か、または前記差の分布がガウス分布に適合するか否かを判定することによって、前記方位角の最適値の精度を判定する請求項１記載の方位角推定装置。
【請求項３】
前記方位角推定手段は、前記判定手段により精度が予め定めた閾値よりも高いと判定された場合には、前記最適値を前記移動体の方位角として推定し、前記判定手段により精度が予め定めた閾値よりも低いと判定された場合には、過去に推定された方位角及び前記検出手段により検出された物理量に基づいて、前記移動体の方位角を推定する請求項１または請求項２記載の方位角推定装置。
【請求項４】
前記最適値推定手段は、所定の初期値を基準に前記検出手段により検出された物理量を所定時間分積分して前記検出手段により検出された物理量から得られる方位角を算出し、該方位角と前記算出手段により算出された方位角との一定時間における差の和が最小となる前記初期値を探索することで、前記方位角の最適値を推定する請求項１〜請求項３のいずれか１項記載の方位角推定装置。
【請求項５】
前記算出手段は、前記情報発信源の各々の位置に関する情報、及び前記情報発信源の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記情報発信源の各々の方向を算出し、時系列の前記情報発信源の各々の位置に関する情報に基づいて、前記情報発信源の各々の速度を算出し、前記移動体から見た前記情報発信源の各々の方向、前記情報発信源の各々の速度、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体の情報発信源の各々の方向の速度を算出し、複数の前記移動体の情報発信源の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出する請求項１〜請求項４のいずれか１項記載の方位角推定装置。
【請求項６】
前記情報発信源を、前記情報発信源の各々の位置に関する情報として衛星軌道情報を、前記情報発信源の各々と前記移動体との距離に関する情報として擬似距離情報を、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報としてドップラー周波数情報を発信するＧＰＳ衛星とした請求項１〜請求項５のいずれか１項記載の方位角推定装置。
【請求項７】
コンピュータを、移動体に搭載される方位角推定装置の各手段として機能させるための方位角推定プログラムであって、
前記コンピュータを、
複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段、
前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルから前記移動体の方位角を算出する算出手段、
前記算出手段により算出された方位角、及び前記移動体の方位角に関する物理量を検出する検出手段により検出された物理量から得られる方位角に基づいて、前記移動体の方位角の最適値を推定する最適値推定手段、
前記最適値推定手段により推定された最適値と前記算出手段により算出された方位角との差の分布に基づいて、前記方位角の最適値の精度を判定する判定手段、及び
前記判定手段により精度が予め定めた閾値よりも高いと判定されたときの最適値を用いて前記移動体の方位角を推定する方位角推定手段
として機能させるための方位角推定プログラム。
【請求項８】
コンピュータを、請求項１〜請求項６のいずれか１項記載の方位角推定装置の各手段として機能させるための方位角推定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１２−９８１８５（Ｐ２０１２−９８１８５Ａ）
【公開日】平成２４年５月２４日（２０１２．５．２４）
【国際特許分類】
【出願番号】特願２０１０−２４６７１０（Ｐ２０１０−２４６７１０）
【出願日】平成２２年１１月２日（２０１０．１１．２）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２４年５月２４日（２０１２．５．２４）
【国際特許分類】
【出願日】平成２２年１１月２日（２０１０．１１．２）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]