JP5673071B2 - 位置推定装置及びプログラム - Google Patents

Description

本発明は、位置推定装置及びプログラムに係り、特に、移動体の位置を推定するための位置推定装置及びプログラムに関する。

従来、測位衛星からの複数の擬似距離と、測位航法部と慣性航法部とカルマンフィルタと、により移動体の航法位置を求める移動体測位装置が提案されている（特許文献１参照）。特許文献１に記載の移動体測位装置は、測位衛星と移動体間のドップラ観測値と、航法暦とカルマンフィルタ出力とにより測位衛星と移動体間の相対速度の計算をしてドップラ推定値を求め、複数の擬似距離から選択擬似距離を出力する衛星情報選択部と、測位衛星から得る搬送波位相情報と選択擬似距離とカルマンフィルタの出力とで演算出力する擬似距離推定部とを備えている。そして、測位航法部が、擬似距離推定値を入力として、測位航法速度と測位航法位置を出力し、カルマンフィルタが、擬似距離推定値と、測位航法部の出力と慣性航法部の出力とで状態推定値を求めている。

特開２００６−２０８３９２号公報

しかしながら、上記の特許文献１に記載の技術では、移動体の軌跡を推定する慣性航法部において、移動体の移動方位が推定できないため、測位航法部との情報の統合の際に方位推定が必要になる、という問題がある。また、測位航法部では、擬似距離のみを入力として位置推定を行っているため、エポック毎に誤差が生じて位置推定の精度にばらつきがある、という問題がある。

本発明は、上記の問題点を解決するためになされたもので、精度よく移動体の位置を推定することができる位置推定装置及びプログラムを提供することを目的とする。

上記目的を達成するために、本発明の位置推定装置は、移動体に搭載される位置推定装置であって、複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルを所定時間分積算することにより、前記移動体の軌跡を算出する軌跡算出手段と、前記軌跡算出手段により算出された移動体の軌跡上の異なる時刻の複数の点の各々における、各点と該点の時刻における前記情報発信源の各々の位置との距離と、前記取得手段により取得された前記発信源情報から得られる各点の時刻における前記情報発信源の各々と前記移動体との距離との差の前記複数の点分の和が最小となるように前記移動体の軌跡を平行移動させた位置を前記移動体の位置として推定する推定手段と、を含んで構成されている。

本発明の位置推定装置によれば、取得手段が、複数の情報発信源の各々から発信された、情報発信源の各々の位置に関する情報、情報発信源の各々と移動体との距離に関する情報、及び情報発信源の各々に対する移動体の相対速度に関する情報を含む発信源情報を取得し、軌跡算出手段が、取得手段により取得された発信源情報に基づいて、移動体の速度ベクトルを所定時間分積算することにより、移動体の軌跡を算出する。ここで、所定時間分積算することとは、所望の精度が得られる適切な範囲分を積分することである。速度ベクトルを積算することにより算出された移動体の軌跡は、形状及び方位において精度よく算出することができ、この移動体の軌跡により表される位置と絶対位置とは平行移動の関係である。

そこで、推定手段が、軌跡算出手段により算出された移動体の軌跡上の異なる時刻の複数の点の各々における、各点と該点の時刻における情報発信源の各々の位置との距離と、取得手段により取得された発信源情報から得られる各点の時刻における情報発信源の各々と移動体との距離との差の複数の点分の和が最小となるように移動体の軌跡を平行移動させた位置を移動体の位置として推定する。

このように、移動体の速度ベクトルから算出された形状及び方位の精度が高い移動体の軌跡をベースにして、取得された情報発信源の各々と移動体との距離に関する情報とカップリングして、最適な位置に移動体の軌跡を平行移動させて位置を推定するため、精度よく移動体の位置を推定することができる。

また、前記軌跡算出手段は、前記情報発信源の各々の位置に関する情報、及び前記情報発信源の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記情報発信源の各々の方向を算出し、時系列の前記情報発信源の各々の位置に関する情報に基づいて、前記情報発信源の各々の速度を算出し、前記移動体から見た前記情報発信源の各々の方向、前記情報発信源の各々の速度、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体の情報発信源の各々の方向の速度を算出し、複数の前記移動体の情報発信源の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出するようにすることができる。

また、前記情報発信源を、前記情報発信源の各々の位置に関する情報として衛星軌道情報を、前記情報発信源の各々と前記移動体との距離に関する情報として擬似距離情報を、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報としてドップラー周波数情報を発信するＧＰＳ衛星とすることができる。情報発信源としては、例えば、擬似衛星やビーコンなどのように、発信源情報を発信するものであればよいが、代表的には、ＧＰＳ衛星とすることができる。

また、前記軌跡算出手段は、前記取得手段により取得された前記発信源情報、及び前記移動体の運動を検出する検出手段により検出された移動体運動情報に基づいて、前記移動体の速度ベクトルを算出するようにすることができる。これにより、情報発信源から取得した発信源情報の数が不足する場合でも、精度よく移動体の位置を推定することができる。また、発信源情報と移動体運動情報とのカップリングを移動体の速度ベクトルを算出する段階で行うため、情報発信源と移動体との距離に関する情報もあわせてカップリングして絶対位置を推定する場合と比較して、誤差が生じやすい情報発信源と移動体との距離に関する情報による推定精度の劣化が生じにくくなるため、精度よく移動体の位置を推定することができる。

また、前記推定手段は、前記異なる時刻の複数の点を、隣接する点と点との距離が予め定めた距離以上離間するように選択することができる。これにより、推定手段での最適推定に用いられる観測値の誤差にランダム性を持たせることができるため、推定精度を向上させることができる。

また、本発明の位置推定プログラムは、コンピュータを、移動体に搭載される位置推定装置の各手段として機能させるための位置推定プログラムであって、前記コンピュータを、複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段、前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルを所定時間分積算することにより、前記移動体の軌跡を算出する軌跡算出手段、及び前記軌跡算出手段により算出された移動体の軌跡上の異なる時刻の複数の点の各々における、各点と該点の時刻における前記情報発信源の各々の位置との距離と、前記取得手段により取得された前記発信源情報から得られる各点の時刻における前記情報発信源の各々と前記移動体との距離との差の前記複数の点分の和が最小となるように前記移動体の軌跡を平行移動させた位置を前記移動体の位置として推定する推定手段として機能させるためのプログラムである。

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

以上説明したように、本発明の位置推定装置及びプログラムによれば、移動体の速度ベクトルから算出された形状及び方位の精度が高い移動体の軌跡をベースにして、取得された各々の情報発信源と移動体との距離に関する情報とカップリングして、最適な位置に移動体の軌跡を平行移動させて位置を推定するため、精度よく移動体の位置を推定することができる、という効果が得られる。

第１の実施の形態に係る車載位置推定装置を示すブロック図である。 三角測量の原理に従って自車両の位置が推定される様子を示すイメージ図である。 各ＧＰＳ衛星の速度ベクトル及び各ＧＰＳ衛星の方向に基づいて、ＧＰＳ衛星方向の自車両の速度を算出する様子を示すイメージ図である。 自車両の軌跡を平行移動させながら最適位置を推定する様子を示すイメージ図である。 第１の実施の形態に係る車載位置推定装置のコンピュータにおける位置推定処理ルーチンの内容を示すフローチャートである。 第１の実施の形態に係る車載位置推定装置のコンピュータにおける自車両軌跡推定処理ルーチンの内容を示すフローチャートである。 第１の実施の形態に係る車載位置推定装置のコンピュータにおける全体最適化位置推定処理ルーチンの内容を示すフローチャートである。 第２の実施の形態に係る車載位置推定装置を示すブロック図である。 第３の実施の形態に係る車載位置推定装置を示すブロック図である。 使用する擬似距離を所定時間間隔で取得する場合を説明するためのイメージ図である。 図１０におけるＧＰＳ衛星Ｂの擬似距離誤差を示す図である。 図１１におけるＧＰＳ衛星Ａの擬似距離誤差を示す図である。 使用する擬似距離を所定距離間隔で取得する場合を説明するためのイメージ図である。 図１３におけるＧＰＳ衛星Ｂの擬似距離誤差を示す図である。 図１３におけるＧＰＳ衛星Ａの擬似距離誤差を示す図である。 図１３におけるＧＰＳ衛星Ｃの擬似距離誤差を示す図である。 軌跡上から所定距離間隔で点を選択する場合を説明するためのイメージ図である。 軌跡上から所定時間間隔で点を選択する場合を説明するためのイメージ図である。 第３の実施の形態に係る車載位置推定装置のコンピュータにおける位置推定処理ルーチンの内容を示すフローチャートである。 第３の実施の形態に係る車載位置推定装置のコンピュータにおける全体最適化位置推定処理ルーチンの内容を示すフローチャートである。 第３の実施の形態の車載位置推定装置による真値および位置推定結果を示す図である。 図２１における実測を行った場所の地図を示すイメージ図である。

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

図１に示すように、第１の実施の形態に係る車載位置推定装置１０は、ＧＰＳ衛星からの電波を受信するＧＰＳ受信部１２と、ＧＰＳ受信部１２によって受信されたＧＰＳ衛星からの受信信号に基づいて、自車両の位置を推定する処理を実行するコンピュータ１４とを備えている。

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

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

コンピュータ１４を以下で説明する位置推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ擬似距離データ、ドップラー周波数、及びＧＰＳ衛星の位置座標を算出して取得するＧＰＳ情報取得部２０と、取得したＧＰＳ擬似距離データ及びＧＰＳ衛星の位置座標を所定期間分記憶する擬似距離・衛星位置記憶部３０と、取得したＧＰＳ衛星の情報に基づいて、所定時間分の自車両の軌跡を推定する軌跡推定部４０と、推定された自車両の軌跡と記憶された擬似距離及び衛星位置に基づいて、最適化処理により自車両の絶対位置を推定する最適化位置推定部６０と、を含んだ構成で表すことができる。

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

また、軌跡推定部４０は、取得した各ＧＰＳ衛星のドップラー周波数に基づいて、各ＧＰＳ衛星に対する自車両の相対速度を算出する相対速度算出部４２と、取得した各ＧＰＳ衛星の位置座標の時系列データに基づいて、各ＧＰＳ衛星の速度ベクトルを算出する衛星速度算出部４４と、取得した各ＧＰＳ衛星のＧＰＳ擬似距離データに基づいて、自車両の位置を算出する自車位置算出部４６と、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星の方向（角度の関係）を算出する衛星方向算出部４８と、算出された相対速度、各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の方向に基づいて、各ＧＰＳ衛星方向の自車両の速度を算出する衛星方向自車速算出部５０と、算出された複数の各ＧＰＳ衛星方向の自車両の速度に基づいて、自車両の速度ベクトルを算出する自車両速度算出部５２と、算出された自車両の速度ベクトルを所定時間分積算して、所定時間分の自車両の軌跡を算出する自車両軌跡算出部５４と、を備えた構成で表すことができる。

相対速度算出部４２は、ドップラー周波数とＧＰＳ衛星に対する相対速度との関係を表わす以下の（１）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度を算出する。

ただし、ｖ_ｊはＧＰＳ衛星ｊに対する相対速度であり、Ｄ１_ｊはＧＰＳ衛星ｊから得られるドップラー周波数（ドップラーシフト量）である。また、Ｃは光速であり、Ｆ_１は、ＧＰＳ衛星から送信される信号の既知のＬ１周波数である。

衛星速度算出部４４は、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（３次元速度ＶＸ_ｊ、ＶＹ_ｊ、ＶＺ_ｊ）を算出する。例えば、非特許文献（Ｐｒａｔａｐ Ｍｉｓｒａ ａｎｄ Ｐｅｒ Ｅｎｇｅ原著 日本航海学会ＧＰＳ研究会訳：“精説ＧＰＳ基本概念・測位原理・信号と受信機”正陽文庫，２００４．）に記載された方法を用いて、各ＧＰＳ衛星の速度ベクトルを算出することができる。

自車位置算出部４６は、以下のように、ＧＰＳ情報取得部２０によって取得された各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。

ＧＰＳを用いた測位では、図２に示すように、既知であるＧＰＳ衛星の位置座標と、各ＧＰＳ衛星から受信した受信信号の伝播距離である擬似距離とに基づいて、三角測量の原理に従って、自車両の位置が推定される。

ここで、ＧＰＳ衛星までの真の距離ｒ_ｊは、以下の（２）式で表され、ＧＰＳで観測される擬似距離ρ_ｊは、以下の（３）式で表される。

ただし、（Ｘ_ｊ，Ｙ_ｊ，Ｚ_ｊ）がＧＰＳ衛星ｊの位置座標であり、（ｘ，ｙ，ｚ）が自車両の位置座標である。ｓは、ＧＰＳ受信部１２の時計誤差による距離誤差である。

上記（２）式、（３）式より、４つ以上のＧＰＳ衛星のＧＰＳ擬似距離データから得られる以下の（４）式の連立方程式を解くことによって、自車両の位置（ｘ、ｙ、ｚ）が算出される。

なお、本実施の形態では、自車両の位置を、ＧＰＳ衛星の方向（ＧＰＳ衛星と自車両との角度）を求めるために算出しているが、遠方に存在するＧＰＳ衛星の方向であるため、位置決めは大まかでよく、擬似距離を用いた位置決定を行わなくてもよい。時刻の影響が少なく、システム等で許容される推定精度に依存するが、自車の位置決めの誤差も数百ｍの範囲であれば、速度推定誤差も１ｍ／ｓｅｃ以下程度となり大きな問題はないため、例えば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、自車両の位置を決定してもよい。

衛星方向算出部４８は、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係（水平方向に対する仰角θ_ｊ、北方向に対する方位角φ_ｊ）を、各ＧＰＳ衛星の方向として算出する。

衛星方向自車速算出部５０は、図３に示すように、算出された各ＧＰＳ衛星に対する自車両の相対速度ｖ_ｊ、各ＧＰＳ衛星の速度ベクトル（ＶＸ_ｊ、ＶＹ_ｊ、ＶＺ_ｊ）、及び各ＧＰＳ衛星の方向Ｒ_ｊ（θ_ｊ、φ_ｊ）に基づいて、以下の（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖ_ｊを算出する。

ｖ_ｊは、ＧＰＳ衛星ｊに対する自車両の相対速度（衛星方向におけるＧＰＳ衛星との相対速度）である。また、Ｖｓ_ｊは、自車方向のＧＰＳ衛星ｊの速度であり、Ｖｓ_ｊ＝Ｒ_ｊ［ＶＸ_ｊ，ＶＹ_ｊ，ＶＺ_ｊ］^Ｔにより求まる。また、Ｖｖ_ｊは、ＧＰＳ衛星ｊの方向の自車速であり、ｖＣｂは、クロックバイアス変動である。

上述したように、ＧＰＳ衛星方向の自車速は、ＧＰＳ衛星位置の三次元位置ではなく、ＧＰＳ衛星との方位関係によってのみ算出される。ＧＰＳ衛星は遥か遠方にあり、１日で地球をほぼ２周するため１分間の角度変化は０．５度である。通常、ＧＰＳ衛星とＧＰＳ受信機との時計誤差は通常１ｍｓｅｃ以下であるため、ＧＰＳ衛星との方位関係に大きな影響はない。また、同じくＧＰＳ衛星は遥か遠方にあるため、自車の位置決定に数１００ｍ程度の誤差が生じていたとしても、ＧＰＳ衛星との方位関係に大きな影響はない。このため、擬似距離に誤差が乗りやすい状況であったとしても、ＧＰＳ衛星方向の自車速は、擬似距離に比較して正確に算出され得る。

自車両速度算出部５２は、以下に説明するように、自車両の速度ベクトルの最適推定を行う。

まず、自車の速度ベクトルを（Ｖｘ，Ｖｙ，Ｖｚ）としたとき、ＧＰＳ衛星方向の自車両の速度Ｖｖ_ｊとの関係は以下の（６）式で表される。

各ＧＰＳ衛星ｊについて得られる上記（６）式より、Ｖｘ，Ｖｙ，Ｖｚ及びＣｂを推定値とした、以下の（７）式で表される連立方程式が得られる。

電波を受信したＧＰＳ衛星が４個以上である場合に、上記（７）式の連立方程式を解くことによって、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。

自車両軌跡算出部５４は、算出された自車両の速度ベクトルを所定時間分積算することにより自車両の軌跡を算出する。所定時間は、所望の精度が得られる適切な時間を定めておく。上述のように、擬似距離に比較して正確に算出されるＧＰＳ衛星方向の自車速を用いて自車両の速度ベクトルを算出し、これを用いて自車両の軌跡を算出することで、精度の高い形状及び方位の自車両の軌跡を算出することができる。

また、最適化位置推定部６０は、算出された自車両の軌道及び平行移動量に基づいて暫定位置を算出する暫定位置算出部６２と、暫定位置における自車両の軌跡上のエポック毎のＧＰＳ衛星との距離と、そのエポックにおける擬似距離との差の全ＧＰＳ衛星の全エポック数分の和を評価値として算出する評価値算出部６４と、算出された評価値が最適値となったか否かを判定すると共に、評価値が最適ではない場合に、自車両の軌跡を平行移動させる評価値判定部６６と、評価値が最適となったときの自車両の軌跡上の各位置を最適化された絶対位置として出力する位置最適化部６８と、を含んだ構成で表すことができる。

暫定位置算出部６２は、算出された自車両の軌跡ｆ（ｔ）について、以下の（８）式で示す暫定位置を算出する。

ΔＸ、ΔＹ、ΔＺ、及びΔＣｂは、それぞれｘ方向、ｙ方向、ｚ方向、及び時計誤差への平行移動量であり、ｔはエポック毎の時刻を示す（ｔ＝０〜ｔｎ）。自車両の軌跡は、ある時点を基準とした形状及び方位が精度よく表現されているため、この形状及び方位を維持したまま（ΔＸ，ΔＹ，ΔＺ，ΔＣｂ）分平行移動させて最適な位置を推定するものである。暫定位置算出部６２は、まず、平行移動量の初期値として（ΔＸ_０，ΔＹ_０，ΔＺ_０，ΔＣｂ_０）を与えて暫定位置を算出する。また暫定位置算出部６２は、後述する評価値判定部６６の判定結果に基づいて、平行移動量（ΔＸ，ΔＹ，ΔＺ，ΔＣｂ）を変更して暫定位置を算出する。

評価値算出部６４は、算出された暫定位置における自車両の軌跡上のエポック毎に、以下の（９）式に従って評価値Ｅを算出する。

図４に示すように、ｄ_ｊ（ｔ）は、暫定位置における自車両の軌跡上の時刻ｔの位置と、時刻ｔにおける各ＧＰＳ衛星との距離であり、ρ_ｊ（ｔ）は、時刻ｔにおける擬似距離である。時刻ｔにおける各ＧＰＳ衛星との距離は、擬似距離・衛星位置記憶部３０に記憶された時刻ｔのＧＰＳ衛星の位置座標を用いて算出する。時刻ｔにおける擬似距離は、擬似距離・衛星位置記憶部３０に記憶された時刻ｔのＧＰＳ擬似距離データを用いる。

評価値判定部６６は、算出された評価値が最小となったか否かを判定し、評価値が最小となっていない場合には、評価値が最小に近づくような平行移動量（ΔＸ，ΔＹ，ΔＺ，ΔＣｂ）を算出して暫定位置算出部６２へ受け渡す。暫定位置算出部６２では、新たに算出された平行移動量（ΔＸ，ΔＹ，ΔＺ，ΔＣｂ）に基づいて暫定位置を算出し、評価値算出部６４でその暫定位置についての評価値Ｅを算出する。そして、評価値判定部６６で評価値が最小となったと判定されるまでこの処理を繰り返して最適化を行う。なお、評価値判定部６６で実行される最適化の手法は、バンドルアジャストの原理等の周知の技術を用いることができる。

位置最適化部６８は、自車両軌跡算出部５４で算出された自車両の軌跡の形状を保持したまま、自車両の軌跡を評価値判定部６６で評価値が最小となったと判定されたときの平行移動量（ΔＸ，ΔＹ，ΔＺ，ΔＣｂ）分平行移動させた位置を最適化された位置として推定する。

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

ＧＰＳ受信部１２によって、複数のＧＰＳ衛星から電波を受信しているときに、コンピュータ１４において、図５に示す位置推定処理ルーチンが実行される。

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

次に、ステップ１０２で、上記ステップ１００で取得した各ＧＰＳ衛星の擬似距離データ及び位置座標を擬似距離・衛星位置記憶部３０に記憶する。

次に、ステップ１０４で、後述する自車両軌跡推定処理を実行して、所定時間分の自車両の軌跡を算出し、次に、ステップ１０６で、後述する全体最適化位置推定処理を実行して、最適化された絶対位置を推定する。

次に、図６を参照して、自車両軌跡推定処理ルーチンについて説明する。

ステップ１１０で、上記（１）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度ｖ_ｊを算出する。

次に、ステップ１１２で、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（ＶＸ_ｊ、ＶＹ_ｊ、ＶＺ_ｊ）を算出する。

次に、ステップ１１４で、各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、上記（２）〜（４）式に従って、自車両の位置を算出する。なお、ここでは、自車両の位置を、ＧＰＳ衛星の方向（ＧＰＳ衛星と自車両との角度）を求めるために算出しており、自車両の位置として大まかな位置が決定できればよく、例えば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、自車両の位置を決定してもよい。

次に、ステップ１１６で、上記ステップ１１４で算出された自車両の位置及び取得された各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係Ｒ_ｊ（水平方向に対する仰角θ_ｊ、北方向に対する方位角φ_ｊ）を、各ＧＰＳ衛星の方向として算出する。

次に、ステップ１１８で、上記ステップ１１０で算出された各ＧＰＳ衛星に対する自車両の相対速度ｖ_ｊ、上記ステップ１１２で算出された各ＧＰＳ衛星の速度ベクトルＶ_ｊ（ＶＸ_ｊ、ＶＹ_ｊ、ＶＺ_ｊ）、及び上記ステップ１１６で算出された各ＧＰＳ衛星の方向Ｒ_ｊ（θ_ｊ、φ_ｊ）に基づいて、上記（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖ_ｊを算出する。（５）式における自車方向のＧＰＳ衛星ｊの速度Ｖｓ_ｊは、Ｖｓ_ｊ＝Ｒ_ｊ［ＶＸ_ｊ，ＶＹ_ｊ，ＶＺ_ｊ］^Ｔにより算出する。

次に、ステップ１２０で、上記（６）式、及び（７）式に従って、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。

次に、ステップ１２２で、上記ステップ１２０で算出された自車両の速度ベクトルを積算することにより自車両の軌跡を算出して、リターンする。

次に、図７を参照して、全体最適化位置推定処理ルーチンについて説明する。

ステップ１３０で、算出された自車両の軌跡、及び平行移動量の初期値（ΔＸ_０，ΔＹ_０，ΔＺ_０，ΔＣｂ_０）に基づいて、上記（８）式で示す暫定位置を算出する。

次に、ステップ１３２で、上記（９）式に従って、（８）式で示される暫定位置における自車両の軌跡上の時刻ｔの位置と時刻ｔにおけるＧＰＳ衛星ｊとの距離ｄ_ｊ（ｔ）と、時刻ｔにおける擬似距離ρ_ｊ（ｔ）との差である擬似距離差分の全ＧＰＳ衛星の全エポック数分の和を、評価値Ｅとして算出する。

次に、ステップ１３４で、上記ステップ１３４で算出された評価値Ｅが最小となったか否かを判定する。最小となったと判定された場合には、ステップ１３８へ移行し、最小ではないと判定された場合には、ステップ１３６へ移行する。

ステップ１３６では、評価値Ｅが最小値に近づくような平行移動量（ΔＸ，ΔＹ，ΔＺ，ΔＣｂ）を算出して、ステップ１３０へ戻り、自車両の軌跡を平行移動量（ΔＸ，ΔＹ，ΔＺ，ΔＣｂ）分平行移動させた暫定位置を算出して、ステップ１３２及びステップ１３４の処理を、ステップ１３４で評価値Ｅが最小となったと判定されるまで繰り返す。

ステップ１３８では、自車両の軌跡の形状を保持したまま、上記ステップ１３４で最小となったと判定されたときの平行移動量（ΔＸ，ΔＹ，ΔＺ，ΔＣｂ）分平行移動させた位置を最適化された位置として推定して、推定結果を出力してリターンし、処理を終了する。

なお、本ルーチンは、全体位置の最適化の一例であり、他の方法を採用してもよい。例えば、推定パラメータで偏微分し、数回の更新で最適解を得るような手法を用いてもよい。

以上説明したように、第１の実施の形態に係る車載位置推定装置によれば、精度よく算出された自車両の速度ベクトルを積算して算出した自車両の軌跡をベースに擬似距離データとカップリングして、全ＧＰＳ衛星の全エポック数分の擬似距離差分の和が最小となるように自車両の軌跡を平行移動させて、最適な位置を推定するため、精度よく自車両の位置を推定することができる。

また、慣性航法装置等のデータを用いて自車両の軌跡を算出する場合には方位が不定となるが、本実施の形態のように自車両の速度ベクトルを用いて自車両の軌跡を算出すれば方位についても定まるため、最適化による位置推定の際にパラメータを低減して計算を平易にすることができる。

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

第２の実施の形態では、ＧＰＳ衛星以外の移動体の運動を検出する装置により検出された情報をカップリングして、自車両の速度ベクトルを算出する点が、第１の実施の形態と主に異なっている。第２の実施の形態では、移動体の運動を検出する装置として慣性航法装置（Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ：ＩＮＳ）で検出された情報（以下、ＩＮＳ情報という）をカップリングする場合について説明する。

図８に示すように、第２の実施の形態に係る車載位置推定装置２１０のコンピュータ２１４は、衛星方向自車速算出部５０で算出された複数のＧＰＳ衛星方向の自車両の速度Ｖｖ_ｊに、慣性航法装置１６で検出されたＩＮＳ情報に含まれる自車両の速度も加えて自車両の速度ベクトルの最適推定を行う自車両速度算出部２５２を含んだ構成で表すことができる。その他の構成については、第１の実施の形態と同様である。

自車両速度算出部２５２は、上記（６）式の関係から、上記（７）式の連立方程式の最適解として自車の速度ベクトルを算出するが、ＧＰＳ受信部１２において、（７）式の連立方程式の最適解を得るために必要な数のＧＰＳ衛星からの受信信号を受信できなかった場合には、慣性航法装置１６で検出された自車両の速度を加えて、精度を落とすことなく自車両の速度ベクトルを算出する。また、ＧＰＳ衛星からの受信信号の数が不足する場合に限らず、慣性航法装置１６で検出された自車両の速度を加えて、自車両の速度ベクトルを算出するようにして、精度の向上を図ってもよい。

また、自車両速度算出部２５２は、ＧＰＳ衛星からの受信信号の数が不足する場合には、クロックバイアスのドリフトは一定であると仮定して、（７）式の連立方程式のｖＣｂを予め定めた一定値として、連立方程式の次元数を下げるようにしてもよい。さらに、自車両の軌跡を求めるための所定時間における一定区間内であれば、平面内での移動であると仮定することができるため、（７）式の連立方程式のＶｚを０として、連立方程式の次元数を下げるようにしてもよい。

以上説明したように、第２の実施の形態に係る車載位置推定装置によれば、ＩＮＳ情報に含まれる自車両の速度も用いて自車両の速度ベクトルを算出するため、ＧＰＳ衛星からの受信信号の数が不足する場合でも、精度よく自車両の位置を推定することができる。

また、ＩＮＳ情報とＧＰＳ情報（ドップラー周波数）とのカップリングを自車両の速度ベクトルを算出する段階で行うため、ＩＮＳ情報とＧＰＳ情報（ドップラー周波数及び擬似距離）とをカップリングして自車両の絶対位置を算出する場合に比べて、誤差が生じやすい擬似距離による推定精度の劣化が生じにくくなるため、精度よく自車両の位置を推定することができる。

なお、第２の実施の形態では、ＧＰＳ衛星以外の移動体の運動を検出する装置として慣性航法装置を用いる場合について説明したが、車載外界監視センサ等を用いてもよい。

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

第３の実施の形態では、最適化された絶対位置を推定する際に、エポック毎のデータではなく、所定距離間隔毎の点となるように選択された点のデータを用いる点が、第１の実施の形態と主に異なっている。

図９に示すように、第３の実施の形態に係る車載位置推定装置３１０のコンピュータ３１４は、擬似距離の誤差が小さいと推定されるＧＰＳ衛星を選択する衛星選択部７０、及び最適化位置推定部６０の評価値算出部２６４で使用される軌跡の粒度を選択する軌跡粒度選択部７２を含んだ構成で表すことができる。その他の構成については、第１の実施の形態と同様である。

衛星選択部７０は、ＧＰＳ受信部１２で受信信号を受信した際のＳＮ比が閾値以下となる受信信号を送信したＧＰＳ衛星を棄却し、ＳＮ比が閾値を超える受信信号を送信したＧＰＳ衛星の受信信号を使用するように選択する。これにより、擬似距離誤差が大きいと推定されるＧＰＳ衛星を排除することができる。

軌跡粒度選択部７２は、評価値算出部２６４で評価値Ｅを算出する際に、第１の実施の形態のように、ｔ０〜ｔｎまでの所定時間間隔の全エポック毎に計算するのではなく、受信信号の受信位置間の距離が所定距離以上となるエポック毎に計算するように、評価値算出部２６４で使用される軌跡の粒度を選択する。言い換えると、軌跡粒度選択部７２は、擬似距離・衛星位置記憶部３０に記憶された擬似距離の中から、評価値算出部２６４で使用する擬似距離を選択することで、暫定位置算出部６２で算出された軌跡の粒度を設定するものである。このように、受信位置間の距離が所定距離以上となるように選択されたエポック毎の擬似距離を使用することで、特にマルチパスに起因する擬似距離誤差の偏りを低減して、位置最適化の精度を向上させることができる。

ここで、軌跡の粒度と位置最適化の精度との関係について説明する。

図１０に、使用する擬似距離を所定時間間隔で取得した場合を示す。図１０では、交差点で車両が停止する状況を仮定し、一定の時間間隔毎の時刻ｔｎ（ｎ＝１、２、・・・、８）における車両位置と、時刻ｔｎで受信したＧＰＳ衛星Ａからの電波の経路を示している。この場合、各時刻ｔ１〜ｔ８における車両位置では、ＧＰＳ衛星Ｂからは常にマルチパスの影響を受けることなく電波を直接受信できるため、図１１に示すように、計測できる擬似距離の誤差は小さくなり、また各時刻での擬似距離誤差はランダム性を有する傾向がある。なお、その誤差は大きくても数ｍのオーダーである。

一方、ＧＰＳ衛星Ａからの電波は建物で反射して受信される可能性があり、マルチパスの影響が生じる。このようなマルチパスの影響が生じた電波が受信された場合には、受信した受信信号から算出される擬似距離の誤差は大きくなる。擬似距離の誤差は、直接波と反射波との経路差であるので、数ｍ〜百数十ｍの誤差となることが想定される。

位置の最適化の推定に一般的に用いられる最小二乗法では、誤差がランダム性（ガウス性）を有すると想定して計算が実施される。しかし、上記の図１０に示すような状況でＧＰＳ衛星Ａ及びＧＰＳ衛星Ｂから時刻ｔ１〜ｔ８で受信したＧＰＳ情報に基づいて擬似距離を取得した場合、すなわち所定時間間隔で使用する擬似距離を取得した場合には、車両が停止する直前にＧＰＳ衛星Ａからの電波のような反射波を受信してしまい、図１２に示すように、連続的に大きく、かつ偏った傾向の誤差を生じてしまう。マルチパスによる誤差は、場所に依存する傾向が強いため、近接した場所で受信したマルチパス誤差は似た傾向を示すためである。市街地の交差点付近は、道が四方に開けているため、受信可能な衛星数が増える反面、反射波も受け易くなる。実際の環境では、複数個の衛星がＧＰＳ衛星Ａのような状況にある場合もあり、誤差が増大する原因となる。加えて、交差点付近は車両の速度が低下する場合が多いため、擬似距離を取得した区間の距離が短くなり、電波を受信できる衛星の種類（数）も限定される。例えば、図１０の状況では、ＧＰＳ衛星Ｃは建物の影に隠れるため、ＧＰＳ衛星Ｃからの電波は、時刻ｔ１〜ｔ８の全区間において受信することができない。

上述のように、最小二乗法では、誤差がランダム性を有することを前提として計算が実施されるため、最小二乗法による最適化の誤差を低下させるためには、用いる観測値の誤差にランダム性を持たせることが効果的である。そのためには、図１２に示したような、大きな誤差や偏った傾向の誤差を除く必要がある。しかし、所定時間間隔で取得した擬似距離は、ＧＰＳ衛星の種類が限定され、取得された擬似距離の誤差も大きくかつ偏った傾向を有しているため、最小二乗法の残差を利用してマルチパスの衛星を除く手法や、従来提案されているような、衛星の組み合わせを変更して誤差が大きい衛星を除く手法では、対応することが困難である。

これに対して、図１３に、使用する擬似距離を所定距離間隔で取得した場合を示す。図１３では、図１０の場合と同様に交差点で車両が停止する状況を仮定し、一定の距離間隔（所定距離以上の間隔）毎の位置ｄｍ（ｍ＝１、２、・・・、８）における車両位置と、位置ｄｍで受信したＧＰＳ衛星Ａからの電波の経路を示している。この場合も、各位置ｄ１〜ｄ８における車両位置では、ＧＰＳ衛星Ｂからは常にマルチパスの影響を受けることなく電波を直接受信できるため、図１４に示すように、計測できる擬似距離の誤差は小さくなり、また各位置での擬似距離誤差はランダム性を有する傾向がある。

一方、ＧＰＳ衛星Ａからの電波は、所定時間間隔の場合に比べて、受信した地点の間隔離間する。このため、マルチパスの影響を受けたとしても場所により異なる誤差となる。例えば、図１５に示すように、位置ｄ２及び位置ｄ８で受信した電波はマルチパスの影響を受けており誤差が生じているが、各々異なる傾向の誤差となっている。従って、取得した擬似距離の中にマルチパスの影響が含まれているとしても、所定区間（ここでは、ｄ１〜ｄ８）全体でみれば、誤差のランダム性が確保できる。加えて、車速によらず、所定距離間隔で擬似距離を取得するため、図１６に示すように、ＧＰＳ衛星Ｃからの電波も受信して擬似距離を取得することができる。

その結果、ＧＰＳ衛星Ａ、ＧＰＳ衛星Ｂ、及びＧＰＳ衛星Ｃからの電波を位置ｄ１〜ｄ８の位置の各々で受信して擬似距離を取得することで、最小二乗法に用いる擬似距離の誤差のランダム性を高めることができる。加えて、所定時間間隔で擬似距離を取得する場合よりも、電波を受信できる衛星の種類を増やすことができ、位置最適化の精度が向上することが見込める。

従って、軌跡粒度選択部７２は、具体的には、図１７に示すように、暫定位置算出部６２で算出された軌跡上から、各点間の距離が距離Ｄ（距離Ｄは所定距離以上）となるように複数の点（ここでは、ｄ０〜ｄ３）を選択する。なお、距離Ｄは、図１８に示すように、所定時間間隔毎（全エポック毎）に軌跡上の点を選択した場合に比べて軌跡の粒度が粗くなるように設定する。そして、擬似距離・衛星位置記憶部３０に記憶された擬似距離及び衛星位置の中から、軌跡上から選択した各点ｄｍに対応するエポックでの擬似距離及び衛星位置を選択する。

評価値算出部２６４は、軌跡粒度選択部７２で選択されたエポック毎の擬似距離を用いて、評価値Ｅを算出する。具体的には、下記（１０）式に従って算出する。

ここで、ｔ_ｄｍは、軌跡粒度選択部７２で選択された各点ｄｍに対応するエポックの時刻である。従って、ｄ_ｊ（ｔ_ｄｍ）は、軌跡粒度選択部７２により選択された軌跡上の点ｄｍの位置と、点ｄｍに対応するエポックの衛星位置とに基づいて算出される。また、ρ_ｊ（ｔ_ｄｍ）は、軌跡粒度選択部７２により選択された軌跡上の点ｄｍに対応するエポックの擬似距離である。

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

ＧＰＳ受信部１２によって、複数のＧＰＳ衛星から電波を受信しているときに、コンピュータ３１４において、図１９に示す位置推定処理ルーチンが実行される。なお、第1の実施の形態の位置推定処理と同一の処理については、同一の符号を付して、詳細な説明を省略する。

ステップ３００で、ＧＰＳ受信部１２で受信信号を受信した際のＳＮ比が予め定めた閾値以下となるＧＰＳ衛星を棄却し、ＳＮ比が閾値を超えるＧＰＳ衛星を選択する。

次に、ステップ３０２で、ＧＰＳ受信部１２から、上記ステップ３００で選択された複数のＧＰＳ衛星の情報を取得すると共に、複数のＧＰＳ衛星のＧＰＳ擬似距離データ、ドップラー周波数、ＧＰＳ衛星の位置座標を算出して取得する。

次に、ステップ１０２で、上記ステップ３０２で取得した各ＧＰＳ衛星の擬似距離及び衛星位置を擬似距離・衛星位置記憶部３０に記憶する。

次に、ステップ１０４で、第１の実施の形態と同様に自車両軌跡推定処理を実行して、所定時間分の自車両の軌跡を算出し、次に、ステップ３０６で、後述する全体最適化位置推定処理を実行して、最適化された絶対位置を推定する。

次に、図２０を参照して、全体最適化位置推定処理ルーチンについて説明する。なお、第１の実施の形態の全体最適化位置推定処理と同一の処理については、同一の符号を付して、詳細な説明を省略する。

ステップ１３０で、算出された自車両の軌跡、及び平行移動量の初期値（ΔＸ_０，ΔＹ_０，ΔＺ_０，ΔＣｂ_０）に基づいて、上記（８）式で示す暫定位置を算出する。

次に、ステップ３３０で、上記ステップ１３０で算出された暫定位置における自車両の軌跡上から、各点間の距離が距離Ｄとなるように複数の点ｄｍを選択する。また、擬似距離・衛星位置記憶部３０に記憶された擬似距離及び衛星位置の中から、軌跡上から選択した各点ｄｍに対応するエポックでの擬似距離及び衛星位置を選択する。

次に、ステップ３３２で、上記ステップ３３０で選択された軌跡上の点ｄｍの位置、点ｄｍに対応するエポックでの衛星位置、及び点ｄｍに対応するエポックでの擬似距離を用いて、上記（１０）式に従って、評価値Ｅを算出する。

以下、第１の実施の形態と同様に、ステップ１３４〜１３８を実行して、自車両の軌跡の形状を保持したまま、評価値Ｅが最小となったときの平行移動量（ΔＸ，ΔＹ，ΔＺ，ΔＣｂ）分平行移動させた位置を最適化された位置として推定して、推定結果を出力してリターンする。

ここで、距離Ｄを「０．１ｍ」、「１ｍ」、「５ｍ」と変化させた場合に、実際に推定されたデータを図２１に示す。同図（ａ）は、距離Ｄ＝０．１ｍ、同図（ｂ）は、距離Ｄ＝１ｍ、同図（ｃ）は、距離Ｄ＝５ｍに設定して、バンドルアジャストにより位置最適化推定を行った結果である。実線が真値、破線が位置推定結果である。なお、図２２が実測を行った場所の地図であり、交差点を中心とした直線道路を対象区域としている。車両は東から走行しており、交差点までの区間では、ＧＰＳ衛星からの電波の受信個数が少ないため、バンドルアジャストによる位置推定が行われず、軌跡推定部による車両軌跡により補間されているため、距離Ｄによる相違は見られない。

しかし、交差点に差し掛かると空が開け始めるため、捕捉できる衛星数が増加し、その分マルチパスも増加するため、測位誤差が増大する危険性がある。図２１（ａ）及び（ｂ）に示すように、距離Ｄが１ｍ以下の場合には、同じ傾向のマルチパス誤差が集まってしまい、観測値の誤差がランダム性を有さいない状態で最小二乗法により測位が行われたため、大きな誤差が発生していることがわかる。一方、同図（ｃ）に示すように、距離Ｄを５ｍとした場合には、同じ傾向のマルチパスが集まることを防いで、観測値の誤差にランダム性を持たせることができるため、位置推定精度が向上していることが分かる。

交差点では信号で停止するため、時間間隔で区切ると、距離Ｄを短くした場合と同じ状態になる。すなわち、バンドルアジャストの手法で測位を行う場合には、距離Ｄをある程度長く設定することで、マルチパスの誤差を低減させることができる。

以上説明したように、第３の実施の形態に係る車載位置推定装置によれば、精度よく算出された自車両の速度ベクトルを積算して算出した自車両の軌跡をベースに擬似距離データとカップリングする際に、受信位置間の距離が所定距離以上となるように選択されたエポック毎の擬似距離を使用することで、特にマルチパスに起因する擬似距離誤差の偏りを低減して、位置最適化の精度を向上させることができる。

なお、第３の実施の形態では、軌跡上から複数の点を選択する際の各点間の距離Ｄを、所定時間間隔毎（全エポック毎）に軌跡上の点を選択した場合に比べて軌跡の粒度が粗くなるように設定する、としたが、具体的には、上記のような実測データや経験値から、例えば「５ｍ」のように設定することができる。また、第３の実施の形態では、距離Ｄを一定とする場合について説明したが、市街地か郊外かの情報や周辺の建物の高さの情報等の車両周辺の状況に応じて変更するようにしてもよい。また、車両の速度に応じて変更するようにしてもよい。

また、上記第１〜第３の実施の形態では、ＧＰＳ衛星のＧＰＳ情報を用いる場合について説明したが、情報発信源の位置に関する情報、情報発信源と移動体との距離に関する情報、及び情報発信源と移動体との相対速度に関する情報を含む発信源情報を発信する情報発信源からの情報が取得できればよい。例えば、擬似衛星から発信される情報を受信するようにしてもよい。

また、上記第１〜第３の実施の形態では、車両に搭載される位置推定装置について説明したが、本発明の位置推定装置が搭載される移動体は車両に限定されない。例えば、位置推定装置をロボットに搭載してもよいし、歩行者が携帯できるように位置推定装置をポータブル端末として構成するようにしてもよい。

１０、２１０ 車載位置推定装置
１２ ＧＰＳ受信部
１４、２１４ コンピュータ
１６ 慣性航法装置
２０ ＧＰＳ情報取得部
３０ 擬似距離・衛星位置記憶部
４０ 軌跡推定部
４２ 相対速度算出部
４４ 衛星速度算出部
４６ 自車位置算出部
４８ 衛星方向算出部
５０ 衛星方向自車速算出部
５２、２５２ 自車両速度算出部
５４ 自車両軌跡算出部
６０ 最適化位置推定部
６２ 暫定位置算出部
６４ 評価値算出部
６６ 評価値判定部
６８ 位置最適化部
７０ 衛星選択部
７２ 軌跡粒度選択部

Claims (8)

1. 移動体に搭載される位置推定装置であって、
   複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、
   前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルを所定時間分積算することにより、前記移動体の軌跡を算出する軌跡算出手段と、
   前記軌跡算出手段により算出された移動体の軌跡上の異なる時刻の複数の点の各々における、各点と該点の時刻における前記情報発信源の各々の位置との距離と、前記取得手段により取得された前記発信源情報から得られる各点の時刻における前記情報発信源の各々と前記移動体との距離との差の前記複数の点分の和が最小となるように前記移動体の軌跡を平行移動させた位置を前記移動体の位置として推定する推定手段と、
   を含む位置推定装置。
2. 前記推定手段は、下記式で表される評価値Ｅが最小となるように前記移動体の軌跡を平行移動させる請求項１記載の位置推定装置。
   
   ただし、ｄ _ｊ （ｔ）は、移動体の軌跡上の時刻ｔ（ｔ＝ｔ０，ｔ１，・・・，ｔｎ）の点と、時刻ｔにおける情報発信源ｊの位置との距離、ρ _ｊ （ｔ）は、発信源情報から得られる時刻ｔにおける情報発信源ｊと移動体との距離、及びＮは情報発信源の数である。
3. 前記軌跡算出手段は、前記情報発信源の各々の位置に関する情報、及び前記情報発信源の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記情報発信源の各々の方向を算出し、時系列の前記情報発信源の各々の位置に関する情報に基づいて、前記情報発信源の各々の速度を算出し、前記移動体から見た前記情報発信源の各々の方向、前記情報発信源の各々の速度、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体の情報発信源の各々の方向の速度を算出し、複数の前記移動体の情報発信源の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出する請求項１または請求項２記載の位置推定装置。
4. 前記情報発信源を、前記情報発信源の各々の位置に関する情報として衛星軌道情報を、前記情報発信源の各々と前記移動体との距離に関する情報として擬似距離情報を、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報としてドップラー周波数情報を発信するＧＰＳ衛星とした請求項１〜請求項３のいずれか１項記載の位置推定装置。
5. 前記軌跡算出手段は、前記取得手段により取得された前記発信源情報、及び前記移動体の運動を検出する検出手段により検出された移動体運動情報に基づいて、前記移動体の速度ベクトルを算出する請求項１〜請求項４のいずれか１項記載の位置推定装置。
6. 前記推定手段は、前記異なる時刻の複数の点を、隣接する点と点との距離が予め定めた距離以上離間するように選択する請求項１〜請求項５のいずれか１項記載の位置推定装置。
7. コンピュータを、移動体に搭載される位置推定装置の各手段として機能させるための位置推定プログラムであって、
   前記コンピュータを、
   複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段、
   前記取得手段により取得された前記発信源情報に基づいて得られる前記移動体の速度ベクトルを所定時間分積算することにより、前記移動体の軌跡を算出する軌跡算出手段、及び
   前記軌跡算出手段により算出された移動体の軌跡上の異なる時刻の複数の点の各々における、各点と該点の時刻における前記情報発信源の各々の位置との距離と、前記取得手段により取得された前記発信源情報から得られる各点の時刻における前記情報発信源の各々と前記移動体との距離との差の前記複数の点分の和が最小となるように前記移動体の軌跡を平行移動させた位置を前記移動体の位置として推定する推定手段
   として機能させるための位置推定プログラム。
8. コンピュータを、請求項１〜請求項６のいずれか１項記載の位置推定装置の各手段として機能させるための位置推定プログラム。