JP7148039B2

JP7148039B2 - 移動体情報推定装置及びプログラム

Info

Publication number: JP7148039B2
Application number: JP2018054430A
Authority: JP
Inventors: 徳祥鈴木; 和也下岡; 洪二郎武山; 勝彦武藤
Original assignee: Denso Corp; Toyota Central R&D Labs Inc
Current assignee: Denso Corp; Toyota Central R&D Labs Inc
Priority date: 2018-03-22
Filing date: 2018-03-22
Publication date: 2022-10-05
Anticipated expiration: 2038-03-22
Also published as: JP2019168257A

Description

本発明は、移動体情報推定装置及びプログラムに係り、特に、ＧＰＳ衛星の衛星情報から移動体の測位又は速度推定を行う移動体情報推定装置及びプログラムに関する。

衛星から受信機までの距離を伝搬時間で測定（受信機時計の誤差Cbを含む）し、疑似距離を求め、４つ以上の衛星を用いて、(X,Y,Z,Cb)の４つの未知数を求める。

このとき、衛星からの電波が周辺建物等で反射・回折するマルチパスによる誤差は大きな測位誤差が発生する主要因である。周囲に建物等がある環境を走行する車両で発生しやすいため、大きな課題である（図９参照）。

また、マルチパス誤差のある衛星を判別・排除するための従来の衛星選択で一般的な手法としては、SNの低い衛星を排除、仰角が閾値以下の衛星を排除、疑似距離残差の大きい衛星を排除、等が用いられている

また、ドップラー周波数を用いた速度推定が知られている（非特許文献１）。この速度推定の手法では、図１０に示すように、受信機－衛星の視線方向の相対速度に対応したドップラーシフト発生を用いて、４つ以上の衛星を用いて、(Vx, Vy, Vz, Cd)の４つの未知数を求める。位置に比べ速度は誤差が発生しにくく、精度良く車両速度が求められる。

また、マルチパスによりドップラー周波数への影響を考慮する必要がある。ここで、各衛星の方向は、衛星軌道情報・概略車両位置から算出できる。また、衛星速度によるドップラーシフトは、衛星が遠方のため変わらないとみなせる。車両速度によるドップラーシフトは、マルチパスによる到来方向により変わる。

従って、到来方向と車両速度・進行方向により観測されるドップラー周波数が変化する（図１１参照）。

また、ドップラー誤差を、ドップラー速度から逆算したドップラー予測値と観測値との差であるドップラー残差から求め、ドップラー残差が閾値以上であるか否かでマルチパスの影響を判定し、衛星を排除する速度推定装置が知られている（特許文献１）。

Y. Kojima, "Proposal for a new localization method using tightly coupled integration based on a precise estimation of trajectory from GPS Doppler", Proceedings of AVEC2010, Laughborough UK, 2010.

特許第５８７９９７７号公報

マルチパスにより観測されるドップラーシフトが変化する状況において、衛星速度に依存して観測されるドップラーシフトは変わらない。衛星が遠方にあり、マルチパス反射点が衛星までの距離に対して車両近傍のため、見かけの方位変化はほとんど無視できる。

車両速度に依存して観測されるドップラーシフトは、V・cosθ に比例する様に変化する（図１２参照）。すわなち、車速（V）が遅い（停止を含む）と判定できない。また、マルチパスにより到来方向（cosθ）が変わらないと判定できない。

また、上記の特許文献１に記載の技術では、ドップラー残差が閾値以上の衛星を排除する処理を実施している間に、車速が変化すると次のような動作となる。

まず、車速がある程度以上速い場合には、ドップラー残差によるマルチパスの影響を判定することができる。一方、車速が低い場合、あるいは停止している場合には、ドップラー残差によるマルチパスの影響を判定することができない。

本発明は、上記の問題点を解決するためになされたもので、移動体の測位又は速度推定に用いる衛星情報を適切に選択することができる移動体情報推定装置及びプログラムを提供することを目的とする。

上記目的を達成するために、第１の発明の移動体情報推定装置は、移動体の速度を取得する速度取得手段と、複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する衛星情報取得手段と、前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記ＧＰＳ衛星の各々のドップラー誤差を算出するドップラー誤差算出手段と、前記ドップラー誤差算出手段によって算出されたドップラー誤差に基づいて、前記ドップラー誤差が閾値以上となる前記ＧＰＳ衛星の前記衛星情報を排除すると判定する排除判定手段と、前記排除判定手段による判定結果に基づいて、前記複数のＧＰＳ衛星の各々の前記衛星情報から、前記移動体の測位又は速度推定に用いる測位情報を選択する前記衛星情報を選択する衛星選択手段と、を含み、前記衛星選択手段は、前記速度取得手段によって取得された前記移動体の速度が閾値以下である場合、前記排除判定手段による前回の判定結果に基づいて、複数のＧＰＳ衛星の各々の前記衛星情報から、測位又は速度推定に用いる前記衛星情報を選択する。

また、第２の発明のプログラムは、コンピュータを、移動体の速度を取得する速度取得手段、複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する衛星情報取得手段、前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記ＧＰＳ衛星の各々のドップラー誤差を算出するドップラー誤差算出手段、前記ドップラー誤差算出手段によって算出されたドップラー誤差に基づいて、前記ドップラー誤差が閾値以上となる前記ＧＰＳ衛星の前記衛星情報を排除すると判定する排除判定手段、及び前記排除判定手段による判定結果に基づいて、前記複数のＧＰＳ衛星の各々の前記衛星情報から、前記移動体の測位又は速度推定に用いる測位情報を選択する前記衛星情報を選択する衛星選択手段として機能させるためのプログラムであって、前記衛星選択手段は、前記速度取得手段によって取得された前記移動体の速度が閾値以下である場合、前記排除判定手段による前回の判定結果に基づいて、複数のＧＰＳ衛星の各々の前記衛星情報から、測位又は速度推定に用いる前記衛星情報を選択するプログラムである。

第１の発明及び第２の発明によれば、前記移動体の速度が閾値以下である場合、前記ドップラー誤差が閾値以上となる前記ＧＰＳ衛星の前記衛星情報を排除すると判定する前回の判定結果に基づいて、複数のＧＰＳ衛星の各々の前記衛星情報から、測位又は速度推定に用いる前記衛星情報を選択することにより、移動体の測位又は速度推定に用いる衛星情報を適切に選択することができる。

第３の発明の移動体情報推定装置は、複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する衛星情報取得手段と、前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記ＧＰＳ衛星の各々のドップラー誤差を算出するドップラー誤差算出手段と、前記ドップラー誤差算出手段によって算出されたドップラー誤差に基づいて、前記ドップラー誤差が閾値以上となる前記ＧＰＳ衛星の前記衛星情報を排除すると判定する排除判定手段と、前記排除判定手段による判定結果に基づいて、前記複数のＧＰＳ衛星の各々の前記衛星情報から、前記移動体の測位又は速度推定に用いる測位情報を選択する前記衛星情報を選択する衛星選択手段と、を含み、前記衛星選択手段は、前記ＧＰＳ衛星の衛星情報を排除する判定された時点からの移動距離が、閾値以下の間、前記ＧＰＳ衛星の衛星情報を排除する状態を保持して、前記複数のＧＰＳ衛星の各々の前記衛星情報から、測位又は速度推定に用いる前記衛星情報を選択する。

第４の発明のプログラムは、コンピュータを、複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する衛星情報取得手段、前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記ＧＰＳ衛星の各々のドップラー誤差を算出するドップラー誤差算出手段、前記ドップラー誤差算出手段によって算出されたドップラー誤差に基づいて、前記ドップラー誤差が閾値以上となる前記ＧＰＳ衛星の前記衛星情報を排除すると判定する排除判定手段、及び前記排除判定手段による判定結果に基づいて、前記複数のＧＰＳ衛星の各々の前記衛星情報から、前記移動体の測位又は速度推定に用いる測位情報を選択する前記衛星情報を選択する衛星選択手段として機能させるためのプログラムであって、前記衛星選択手段は、前記ＧＰＳ衛星の衛星情報を排除する判定された時点からの移動距離が、閾値以下の間、前記ＧＰＳ衛星の衛星情報を排除する状態を保持して、前記複数のＧＰＳ衛星の各々の前記衛星情報から、測位又は速度推定に用いる前記衛星情報を選択するプログラムである。

第３の発明及び第４の発明によれば、ＧＰＳ衛星の衛星情報を排除する判定された時点からの移動距離が、閾値以下の間、前記ドップラー誤差が閾値以上となる前記ＧＰＳ衛星の前記衛星情報を排除する状態を保持して、前記複数のＧＰＳ衛星の各々の前記衛星情報から、測位又は速度推定に用いる前記衛星情報を選択することにより、移動体の測位又は速度推定に用いる衛星情報を適切に選択することができる。

なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ－ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。

以上説明したように、本発明の移動体情報推定装置及びプログラムによれば、移動体の測位又は速度推定に用いる衛星情報を適切に選択することができる、という効果が得られる。

第１の実施の形態に係る移動体情報推定装置を示すブロック図である。第１の実施の形態に係る移動体情報推定装置のドップラー速度ベクトル算出部を示すブロック図である。ドップラー誤差判定の結果を用いた衛星選択の方法を説明するための図である。第１の実施の形態に係る移動体情報推定装置のコンピュータにおける測位処理ルーチンの内容を示すフローチャートである。第１の実施の形態に係る移動体情報推定装置のコンピュータにおける衛星情報選択処理ルーチンの内容を示すフローチャートである。第２の実施の形態に係る移動体情報推定装置を示すブロック図である。第２の実施の形態に係る移動体情報推定装置のコンピュータにおける測位処理ルーチンの内容を示すフローチャートである。第１の変形例に係る移動体情報推定装置のコンピュータにおける衛星情報選択処理ルーチンの内容を示すフローチャートである。測位の基本原理を説明するための図である。ドップラー周波数を用いた速度推定を行う方法を説明するための図である。マルチパスによるドップラー周波数への影響を説明するための図である。従来技術のドップラー誤差によるマルチパス判定の課題を説明するための図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、ＧＰＳ衛星から発信されたＧＰＳ情報を取得して測位を行う測位装置に、本発明を適用した場合を例に説明する。

＜本発明の実施の形態の概要＞
ドップラー周波数の観測値は、疑似距離と異なり誤差要因の影響が小さく、精度良く求められることが知られており、自動車の様にある程度の速度で移動する移動体では、ドップラー誤差による衛星判定・選択は、マルチパスの影響を受けた衛星の判定に有効である。

しかし、ドップラー誤差による衛星判定・選択が可能かどうかは、車速および衛星からの電波の到来方向変化に依存する。

「車速が遅い場合」や「電波の到来方向が変化しない場合」には、マルチパスにより発生するドップラー誤差そのものが小さくなるため、ドップラー誤差を用いた衛星選択（排除）が適切にできなくなるという問題がある。

例えば車速が遅い場合、マルチパスにより生じるドップラー誤差は小さくなるが、疑似距離誤差は小さくなる訳ではなく、位置の推定において大きな測位誤差が発生する場合があり、ドップラー誤差での衛星選択がうまく働かない。

そのため、停止状態や低速移動でも利用可能な、疑似距離残差・SN・仰角等の他の手段を組み合わせて衛星選択を行うことが一般的であり、また測位計算にはドップラー観測値は不要であるため、ドップラーを用いた衛星選択はあまり用いられない。

そこで、本発明の実施の形態では、ドップラー誤差がある閾値以上となり排除と判定された衛星を、その後のドップラー誤差に関わらず、排除状態をある期間保持する。また、マルチパスによる誤差が場所・時間依存という性質を利用し、マルチパスの影響がほぼ同様とみなせる環境にいる期間を何らかの手段で定めて、衛星の排除状態を維持する。

＜第１の実施の形態＞
図１に示すように、第１の実施の形態に係る測位装置１０は、ＧＰＳ衛星からの電波を受信するＧＰＳ受信部１２と、自車両の速度を検出する速度センサ１６と、ＧＰＳ受信部１２によって受信されたＧＰＳ衛星からの受信信号、及び速度センサ１６の検出値に基づいて、自車両の位置を推定する測位処理を実行するコンピュータ１８とを備えている。

ＧＰＳ受信部１２は、複数のＧＰＳ衛星からの電波を受信して、受信した全てのＧＰＳ衛星からの受信信号から、ＧＰＳ衛星の情報として、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報（エフェメリス）、ＧＰＳ衛星が電波を送信した時刻、受信信号の強度、周波数などを取得し、コンピュータ１８に出力する。

コンピュータ１８は、ＣＰＵ、後述する測位処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。

コンピュータ１８を機能ブロックで表すと、図１に示すように、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ疑似距離データ、ドップラー周波数、及びＧＰＳ衛星の位置座標を算出して取得するＧＰＳ情報取得部２０と、速度センサ１６の検出値を取得する車速情報取得部２２と、取得したＧＰＳ衛星の情報に含まれるドップラー周波数に基づいて、自車両の速度ベクトルを算出するドップラー速度ベクトル算出部２４と、算出された速度ベクトルから逆算したＧＰＳ衛星の各々のドップラー周波数であるドップラー予測値と、ＧＰＳ衛星の情報から得られるドップラー観測値との差から、ＧＰＳ衛星の各々のドップラー誤差を算出するドップラー残差算出部２６と、算出されたドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定し、複数のＧＰＳ衛星の各々の衛星情報から、移動体の測位に用いる衛星情報を選択する衛星選択部２８と、選択された衛星情報を用いて移動体の位置を算出する測位部３０とを備えている。なお、衛星選択部２８は、排除判定手段及び衛星選択手段の一例である。

ＧＰＳ情報取得部２０は、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ衛星が電波を送信した時刻及び自車両で電波を受信した時刻に基づいて、ＧＰＳ疑似距離データを算出する。また、ＧＰＳ情報取得部２０は、各ＧＰＳ衛星から送信される信号の既知の周波数と、各ＧＰＳ衛星から受信した受信信号の周波数とに基づいて、各ＧＰＳ衛星からの受信信号のドップラー周波数を各々算出する。なお、ドップラー周波数は、ＧＰＳ衛星と自車との相対速度による、搬送波周波数のドップラーシフト量を観測したものである。また、ＧＰＳ情報取得部２０は、ＧＰＳ衛星の軌道情報及びＧＰＳ衛星が電波を送信した時刻に基づいて、ＧＰＳ衛星の位置座標を各々算出する。

ドップラー速度ベクトル算出部２４は、取得したＧＰＳ衛星の情報に含まれるドップラー周波数に基づいて、特許文献２（特開2014-077769号公報）に記載の手法に同様に、自車両の速度ベクトルを算出する。

具体的には、ドップラー速度ベクトル算出部２４は、図２に示すように、取得したＧＰＳ情報に基づいて、ＧＰＳ衛星の位置及び自車両の位置を算出する位置算出部５８と、取得した各ＧＰＳ衛星のドップラー周波数に基づいて、各ＧＰＳ衛星に対する自車両の相対速度を算出する相対速度算出部６０と、取得した各ＧＰＳ衛星の位置座標の時系列データに基づいて、各ＧＰＳ衛星の速度ベクトルを算出する衛星速度算出部６２と、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星の方向（角度の関係）を算出する衛星方向算出部６６と、算出された相対速度、各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の方向に基づいて、各ＧＰＳ衛星方向の自車両の速度を算出する衛星方向自車速算出部６８と、算出された複数の各ＧＰＳ衛星方向の自車両の速度に基づいて、自車両の速度ベクトルを算出する自車両速度算出部７０と、算出された自車両の速度ベクトルに基づいて自車両の方位角を算出して方位角の変化（時系列）を算出する自車両方位角算出部７２と、を含んだ構成で表すことができる。

ドップラー速度ベクトル算出部２４の各部の処理は、上記特許文献２に記載の手法と同様であるため、説明を省略する。

ドップラー残差算出部２６は、算出された速度ベクトルと、各ＧＰＳ衛星の位置座標とから逆算してＧＰＳ衛星の各々のドップラー周波数の予測値を求め、ＧＰＳ衛星の情報から得られるドップラー周波数の観測値との差から、ＧＰＳ衛星の各々のドップラー誤差を算出する。

衛星選択部２８は、ドップラー残差算出部２６によって算出されたドップラー誤差に基づいて、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定し、この判定結果に基づいて、複数のＧＰＳ衛星の各々の衛星情報から、自車両の測位に用いる測位情報を選択する衛星情報を選択する。

このとき、速度センサ１６によって検出された速度が低い場合には、直前の判定時からの移動距離が小さく、また、マルチパスの影響は場所依存であるため、マルチパスの影響は直前の判定時と同等である。そこで、衛星選択部２８は、速度センサ１６によって検出された速度が閾値以下である場合、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除する判定を行わずに、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定した前回の判定結果を有効として保持し、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる衛星情報を選択する（図３参照）。

また、衛星選択部２８は、ＧＰＳ衛星の衛星情報を排除すると判定された時点からの経過時間が閾値以上となった場合、当該ＧＰＳ衛星の衛星情報を排除する状態を解除して、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる前記衛星情報を選択する。

また、衛星選択部２８は、複数のＧＰＳ衛星の各々の衛星情報から、自車両の測位に用いる測位情報を選択する衛星情報を選択する際に、更に、ＳＮの低い衛星や、仰角が閾値以下の衛星、疑似距離残差の大きい衛星の衛星情報を排除するようにして、衛星情報を選択してもよい。

測位部３０は、選択された衛星情報を用いて自車両の位置を算出する。なお、衛星情報を用いた自車両の位置を算出する方法については、従来既知の手法を用いればよく、詳細な説明を省略する。

次に、第１の実施の形態に係る測位装置１０の作用について説明する。

速度センサ１６によって速度を検出すると共に、ＧＰＳ受信部１２によって、複数のＧＰＳ衛星から電波を受信しているときに、コンピュータ１８において、図４に示す測位処理ルーチンが繰り返し実行される。

ステップＳ１００で、ＧＰＳ受信部１２から複数のＧＰＳ衛星の情報を取得すると共に、複数のＧＰＳ衛星のＧＰＳ疑似距離データ、ドップラー周波数、ＧＰＳ衛星の位置座標を算出して取得する。同一時刻に取得された複数のＧＰＳ衛星分のＧＰＳ情報を、ＧＰＳ情報群として取得する。

次に、ステップＳ１０２で、速度センサ１６から検出値を取得する。

次に、ステップＳ１０４で、上記ステップＳ１００で取得したＧＰＳ衛星の情報に含まれるドップラー周波数に基づいて、自車両の速度ベクトルを算出する。

ステップＳ１０６では、上記ステップＳ１０４で算出された速度ベクトルと、各ＧＰＳ衛星の位置座標とから逆算してＧＰＳ衛星の各々のドップラー周波数の予測値を求め、ＧＰＳ衛星の情報から得られるドップラー周波数の観測値との差から、ＧＰＳ衛星の各々のドップラー誤差を算出する。

ステップＳ１０８では、複数のＧＰＳ衛星の各々の衛星情報から、自車両の測位に用いる測位情報を選択する衛星情報を選択する。

ステップＳ１１０では、上記ステップＳ１０８で選択された衛星情報を用いて自車両の位置を算出し、上記ステップＳ１００へ戻る。

上記ステップＳ１０８は、図５に示す衛星情報選択処理ルーチンによって実現される。

まず、ステップＳ１２０では、速度センサ１６によって検出された速度が閾値以下であるか否かを判定する。

速度センサ１６によって検出された速度が閾値より速い場合には、ステップＳ１２２において、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定し、この判定結果に基づいて、複数のＧＰＳ衛星の各々の衛星情報から、自車両の測位に用いる測位情報を選択する衛星情報を選択する。

一方、速度センサ１６によって検出された速度が閾値以下である場合、ステップＳ１２４において、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定した前回の判定結果を保持して、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる衛星情報を選択する。このとき、ＧＰＳ衛星の衛星情報を排除すると判定された時点からの経過時間が閾値を超えたＧＰＳ衛星が存在する場合、当該ＧＰＳ衛星の衛星情報を排除する状態を解除して、測位に用いる前記衛星情報として選択する。

以上説明したように、第１の実施の形態に係る測位装置によれば、自車両の速度が閾値以下である場合、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定する前回の判定結果に基づいて、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる衛星情報を選択することにより、移動体の測位に用いる衛星情報を適切に選択することができる。

また、マルチパスの影響を受けた衛星をドップラー誤差に基づいて判定する場合、車速が低下すると判定できなくなるが、マルチパスの影響は場所依存の性質を持つため、直前の車速が速い状態での判定結果を保持することにより、車速低下時においてもマルチパスの影響を受けた衛星を排除することが可能になる。その結果、マルチパス誤差の影響を受けた衛星を事前に排除でき、誤差の少ない衛星の情報のみを用いて、速度推定や位置推定を行うことができるため精度改善が可能となる。

＜第２の実施の形態＞
次に、第２の実施の形態について説明する。なお、第２の実施の形態の測位装置について、第１の実施の形態の測位装置１０と同一の構成については、同一の符号を付して説明を省略する。

第２の実施の形態では、推測航法から得られる車速および進行方向に基づいて各ＧＰＳ衛星のドップラー周波数の予測値を求め、ドップラー周波数の予測値と各ＧＰＳ衛星のドップラー周波数の観測値から、各ＧＰＳ衛星のドップラー周波数の誤差を求めている点が、第１の実施の形態と主に異なっている。

図６に示すように、第２の実施の形態に係る測位装置２１０は、ＧＰＳ受信部１２と、速度センサ１６と、自車両のヨーレートを検出するジャイロセンサ２１４と、ＧＰＳ受信部１２によって受信されたＧＰＳ衛星からの受信信号、及び速度センサ１６、ジャイロセンサ２１４の検出値に基づいて、自車両の位置を推定する測位処理を実行するコンピュータ２１８とを備えている。

コンピュータ２１８は、ＧＰＳ情報取得部２０と、速度センサ１６及びジャイロセンサ２１４の検出値を取得するセンサ情報取得部２２２と、取得した速度センサ１６及びジャイロセンサ２１４の検出値に基づいて、各ＧＰＳ衛星のドップラー周波数であるドップラー予測値を算出するドップラー予測値算出部２２４と、算出されたＧＰＳ衛星の各々のドップラー予測値と、ＧＰＳ衛星の情報から得られるドップラー観測値との差から、ＧＰＳ衛星の各々のドップラー誤差を算出するドップラー誤差算出部２２６と、衛星選択部２８と、測位部３０とを備えている。
ドップラー予測値算出部２２４は、車両の速度および進行方向と、各衛星の視線方向からドップラー予測値を求める。このとき、ドップラー予測値算出部２２４は、ジャイロセンサ２１４の検出値に基づいてヨーレートを積算してヨ一角（方位角）の変化量を求めることにより、車両の進行方向を推定する。ただし、少なくとも１回は他の手段で正しい進行方向（方位）を決めておく。例えば、ＧＰＳの位置の変化やドップラー速度等で進行方向を決めておけばよい。なお、地磁気センサを用いて車両の進行方向（方位）を求めてもよい。または、カーナビ等で位置から走行中の道路を求め、地図から道路の方向を求め、進行方向としてもよい。これらの進行方向を求める方法は一例であり、その他の手段で求めてもよい。

次に、第２の実施の形態に係る測位装置２１０の作用について説明する。なお、第１の実施の形態と同様の処理となる部分については、同一符号を付して詳細な説明を省略する。

速度センサ１６及びジャイロセンサ２１４によって速度及びヨーレートを検出すると共に、ＧＰＳ受信部１２によって、複数のＧＰＳ衛星から電波を受信しているときに、コンピュータ２１８において、図７に示す測位処理ルーチンが繰り返し実行される。

次に、ステップＳ２００で、速度センサ１６及びジャイロセンサ２１４から検出値を取得する。

次に、ステップＳ２０２で、上記ステップＳ２００で取得した速度センサ１６及びジャイロセンサ２１４から算出される速度ベクトルと、各ＧＰＳ衛星の位置座標とから逆算してＧＰＳ衛星の各々のドップラー周波数の予測値を算出する。

ステップＳ２０４では、上記ステップＳ２０２で算出されたＧＰＳ衛星の各々のドップラー周波数の予測値を求め、ＧＰＳ衛星の情報から得られるドップラー周波数の観測値との差から、ＧＰＳ衛星の各々のドップラー誤差を算出する。

なお、第２の実施の形態に係る測位装置２１０の他の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。

以上説明したように、第２の実施の形態に係る測位装置によれば、自車両の速度が閾値以下である場合、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定する前回の判定結果に基づいて、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる衛星情報を選択することにより、移動体の測位に用いる衛星情報を適切に選択することができる。

＜第１の変形例＞
次に、本発明の第１の変形例について説明する。

上記第１の実施の形態、第２の実施の形態では、速度が閾値以下である場合、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定した前回の判定結果を保持して、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる衛星情報を選択する場合を例に説明したが、第１の変形例では、ＧＰＳ衛星の衛星情報を排除すると判定された時点からの移動距離が閾値以内となった場合、当該ＧＰＳ衛星の衛星情報を排除する状態を保持して、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる前記衛星情報を選択する。

具体的には、上記図４、７の測位処理ルーチンにおけるステップＳ１０８を、図８に示す衛星情報選択処理ルーチンによって実現する。なお、上記図５に示す衛星情報選択処理ルーチンと同様の処理となる部分については、同一符号を付して詳細な説明を省略する。

まず、ステップＳ１２２において、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定し、この判定結果に基づいて、複数のＧＰＳ衛星の各々の衛星情報から、自車両の測位に用いる測位情報を選択する衛星情報を選択する。

ステップＳ３００では、前回までに排除すると判定されたＧＰＳ衛星の各々について、当該ＧＰＳ衛星の衛星情報を排除すると判定された時点からの移動距離を算出する。移動距離の算出においては、車速を時間積分することにより移動距離を算出してもよい。また、車輪速パルスのパルス数をカウントすることにより移動距離を算出してもよい。

ステップＳ３０２では、前回までに排除すると判定されたＧＰＳ衛星のうち、上記ステップＳ３００で算出された移動距離が閾値以内のＧＰＳについては、当該ＧＰＳ衛星の衛星情報を排除する状態を保持する。

ステップＳ３０４では、前回までに排除すると判定されたＧＰＳ衛星のうち、ＧＰＳ衛星の衛星情報を排除すると判定された時点からの経過時間が閾値を超えたＧＰＳ衛星が存在する場合、当該ＧＰＳ衛星の衛星情報を排除する状態を解除して、測位に用いる前記衛星情報として選択する。

以上説明したように、本発明の第１の変形例に係る測位装置によれば、ＧＰＳ衛星の衛星情報を排除する判定された時点からの移動距離が、閾値以下の間、ドップラー誤差が閾値以上となる当該ＧＰＳ衛星の衛星情報を排除する状態を保持して、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる衛星情報を選択することにより、移動体の測位に用いる衛星情報を適切に選択することができる。

＜第２の変形例＞
次に、本発明の第２の変形例について説明する。

上記第１の実施の形態、第２の実施の形態では、ドップラー誤差が閾値以上となる場合に、マルチパス誤差の影響があると判断して当該ＧＰＳ衛星の衛星情報を排除する場合を例に説明したが、第２の変形例では、ドップラー誤差の大きさによる閾値判定に加えて、更に、ドップラー誤差の変動のばらつきを見て、ばらつきが大きい場合にも、当該ＧＰＳ衛星の衛星情報を排除する。

具体的には、衛星選択部２８は、ドップラー残差算出部２６によって算出されたドップラー誤差に基づいて、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定すると共に、現時点から所定時間前までの区間におけるドップラー誤差の変動のばらつき（分散又は標準偏差）に基づいて、ドップラー誤差の変動のばらつきが閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定する。そして、衛星選択部２８は、これらの判定結果に基づいて、複数のＧＰＳ衛星の各々の衛星情報から、自車両の測位に用いる測位情報を選択する衛星情報を選択する。

このとき、衛星選択部２８は、速度センサ１６によって検出された速度が閾値以下である場合、ドップラー誤差又はドップラー誤差の変動のばらつきが閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定した前回の判定結果を保持して、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる衛星情報を選択する。

以上説明したように、第２の変形例に係る測位装置によれば、自車両の速度が閾値以下である場合、ドップラー誤差の大きさ又は変動のばらつきが閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定する前回の判定結果に基づいて、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる衛星情報を選択することにより、移動体の測位に用いる衛星情報を適切に選択することができる。

なお、上述した第１の変形例において、上記第２の変形例を適用してもよい。この場合には、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定すると共に、ドップラー誤差の変動のばらつきに基づいて、ドップラー誤差の変動のばらつきが閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定し、当該ＧＰＳ衛星の衛星情報を排除すると判定された時点からの移動距離が閾値以内となった場合、当該ＧＰＳ衛星の衛星情報を排除する状態を保持して、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる衛星情報を選択するようにすればよい。

また、上記の実施の形態及び変形例では、車速センサから得られた速度が閾値以下である場合に、今回の判定結果を無効とする場合を例に説明したが、これに限定されるものではなく、ドップラー周波数を用いて推定された速度ベクトルから得られる速度が閾値以下である場合に、今回の判定結果を無効としてもよい。

また、衛星情報を排除する今回の判定結果を無効とするか否かを、毎回同じ車速の閾値で決める場合を例に説明したが、これに限定されるものではない。例えば、前回の判定において、速度が閾値より速くなり、衛星情報を排除する判定結果を有効とすると判定した場合に今回の判定で用いる車速の閾値と、前回の判定において、速度が閾値以下となり、衛星情報を排除する判定結果を無効とすると判定した場合に今回の判定で用いる車速の閾値とを別の値に設定し、ヒステリシスを持たせてもよい。

また、速度が閾値以下である場合には、ドップラー誤差に関する判定を実施しない場合を例に説明したが、速度が閾値以下であるか否かに関わらず、ドップラー誤差に関する判定を実施するようにしてもよい。この場合、速度が閾値以下であれば、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定した今回の判定結果を無効とし、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定した前回の判定結果を有効として保持し、複数のＧＰＳ衛星の各々の衛星情報から、測位に用いる衛星情報を選択するようにすればよい。また、この場合、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定した今回の判定結果を無効とするものの、前回の判定結果で、ドップラー誤差が閾値以上となるＧＰＳ衛星の衛星情報を排除すると判定されたＧＰＳ衛星について、当該ＧＰＳ衛星の衛星情報を排除すると判定された時点を更新する。また、車速センサから得られた速度が閾値以下である場合に、今回の判定結果を全て無効としてもよいが、今回の判定結果で新たに排除すると判定されたＧＰＳ衛星の衛星情報は排除するようにしてもよい。

また、選択された衛星情報を用いて測位を行う場合を例に説明したが、これに限定されるものではなく、選択された衛星情報を用いて速度推定を行うようにしてもよい。

また、車両に搭載される測位装置に、本発明を適用する場合を例に説明したが、本発明の移動体情報推定装置が搭載される移動体は車両に限定されない。例えば、移動体情報推定装置をロボットに搭載してもよい。
また、衛星航法システムとしてＧＰＳを用いた場合を例に説明したが、他の衛星測位システム（ＧＬＯＮＡＳＳ，ＢｅｉＤｏｕ，Ｇａｌｉｌｅｏ，ＱＺＳＳ）を用いてもよいし、これらを併用してもよい。

１０、２１０測位装置
１２ＧＰＳ受信部
１６速度センサ
１８コンピュータ
２０ＧＰＳ情報取得部
２２車速情報取得部
２４ドップラー速度ベクトル算出部
２６ドップラー残差算出部
２８衛星選択部
３０測位部
２１４ジャイロセンサ
２１８コンピュータ
２２２センサ情報取得部
２２４ドップラー予測値算出部
２２６ドップラー誤差算出部

Claims

複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する衛星情報取得手段と、
前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記ＧＰＳ衛星の各々のドップラー誤差を算出するドップラー誤差算出手段と、
前記ドップラー誤差算出手段によって算出されたドップラー誤差に基づいて、前記ドップラー誤差が閾値以上となる前記ＧＰＳ衛星の前記衛星情報を排除すると判定する排除判定手段と、
前記排除判定手段による判定結果に基づいて、前記複数のＧＰＳ衛星の各々の前記衛星情報から、前記移動体の測位又は速度推定に用いる測位情報を選択する前記衛星情報を選択する衛星選択手段と、
を含み、
前記衛星選択手段は、前記ＧＰＳ衛星の衛星情報を排除する判定された時点からの移動距離が、閾値以下の間、前記ＧＰＳ衛星の衛星情報を排除する状態を保持して、前記複数のＧＰＳ衛星の各々の前記衛星情報から、測位又は速度推定に用いる前記衛星情報を選択する
移動体情報推定装置。
前記衛星選択手段は、前記ＧＰＳ衛星の衛星情報を排除する判定された時点からの経過時間が閾値以上となった場合、前記ＧＰＳ衛星の衛星情報を排除する状態を解除して、前記複数のＧＰＳ衛星の各々の前記衛星情報から、測位又は速度推定に用いる前記衛星情報を選択する請求項１記載の移動体情報推定装置。
前記排除判定手段は、更に、所定の時間区間における前記ドップラー誤差のばらつきが閾値以上となる前記ＧＰＳ衛星の前記衛星情報を排除すると判定する請求項１又は２記載の移動体情報推定装置。
前記ＧＰＳ衛星の各々の位置に関する情報、及び前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記ＧＰＳ衛星の各々の方向を算出し、時系列の前記ＧＰＳ衛星の各々の位置に関する情報に基づいて、前記ＧＰＳ衛星の各々の速度を算出し、前記移動体から見た前記ＧＰＳ衛星の各々の方向、前記ＧＰＳ衛星の各々の速度、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体のＧＰＳ衛星の各々の方向の速度を算出し、複数の前記移動体のＧＰＳ衛星の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出する速度取得手段を更に含み、
前記ドップラー誤差算出手段は、前記速度取得手段によって算出された前記速度ベクトルから逆算したドップラー周波数の予測値と、前記衛星情報から得られるドップラー周波数の観測値との差から、前記ＧＰＳ衛星の各々のドップラー誤差を算出する請求項１記載の移動体情報推定装置。
移動体の速度を取得する速度取得手段と、
移動体の進行方向を取得する進行方向取得手段とを更に含み、
前記ドップラー誤差算出手段は、前記速度取得手段によって取得された速度及び前記進行方向取得手段によって取得された進行方向から算出される前記ＧＰＳ衛星の各々のドップラー周波数の予測値と、前記衛星情報から得られるドップラー周波数の観測値との差から、前記ＧＰＳ衛星の各々のドップラー誤差を算出する請求項１記載の移動体情報推定装置。
前記ＧＰＳ衛星の各々の位置に関する情報を、衛星軌道情報とし、前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報を、疑似距離情報とし、前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を、ドップラー周波数情報とした請求項１～請求項５の何れか１項記載の移動体情報推定装置。
コンピュータを、
複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する衛星情報取得手段、
前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記ＧＰＳ衛星の各々のドップラー誤差を算出するドップラー誤差算出手段、
前記ドップラー誤差算出手段によって算出されたドップラー誤差に基づいて、前記ドップラー誤差が閾値以上となる前記ＧＰＳ衛星の前記衛星情報を排除すると判定する排除判定手段、及び
前記排除判定手段による判定結果に基づいて、前記複数のＧＰＳ衛星の各々の前記衛星情報から、前記移動体の測位又は速度推定に用いる測位情報を選択する前記衛星情報を選択する衛星選択手段
として機能させるためのプログラムであって、
前記衛星選択手段は、前記ＧＰＳ衛星の衛星情報を排除する判定された時点からの移動距離が、閾値以下の間、前記ＧＰＳ衛星の衛星情報を排除する状態を保持して、前記複数のＧＰＳ衛星の各々の前記衛星情報から、測位又は速度推定に用いる前記衛星情報を選択する
プログラム。