相対位置推定装置及びプログラム
【課題】少ないデータ通信量で、他の装置との相対位置を精度良く推定することができるようにする。
【解決手段】位置・速度ベクトル算出部２４によって自車両の位置及び速度ベクトルを算出する。無線通信部１８によって、相手車両の位置及び速度ベクトルを受信する。位置差分相対位置算出部２６によって、受信された相手車両の位置から、各時刻について相手車両の相対位置を算出する。軌跡算出部３２によって、自車両の速度ベクトルを積算して軌跡を算出する。相手軌跡算出部３４によって、受信された速度ベクトルを積算して相手車両の軌跡を算出する。軌跡利用相対位置推定部３６によって、自車両の軌跡と、相手車両の軌跡と、各時刻について算出された相対位置とに基づいて、相手車両の軌跡の最適値を推定し、軌跡上の特定点における相対位置を推定する。
【解決手段】位置・速度ベクトル算出部２４によって自車両の位置及び速度ベクトルを算出する。無線通信部１８によって、相手車両の位置及び速度ベクトルを受信する。位置差分相対位置算出部２６によって、受信された相手車両の位置から、各時刻について相手車両の相対位置を算出する。軌跡算出部３２によって、自車両の速度ベクトルを積算して軌跡を算出する。相手軌跡算出部３４によって、受信された速度ベクトルを積算して相手車両の軌跡を算出する。軌跡利用相対位置推定部３６によって、自車両の軌跡と、相手車両の軌跡と、各時刻について算出された相対位置とに基づいて、相手車両の軌跡の最適値を推定し、軌跡上の特定点における相対位置を推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、相対位置推定装置及びプログラムに係り、特に、他の装置と移動体との相対位置を推定するための相対位置推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、３台以上の移動体間の相対位置をＧＰＳの干渉測位によって求めると共に、干渉測位に関する信頼性判定を効率的に行う相対位置検知装置が知られている（特許文献１）。この相対位置検知装置では、３台以上の移動体が互いに通信可能な所定の場所において、基準移動体が観測データを含むデータを非基準移動体に送信し、非基準移動体は観測データ及び基準移動体から受信した観測データを含むデータを用いた干渉測位を行なって基準移動体との相対位置を算出する。そして、他の非基準移動体から整数値バイアスを含むデータを受信して干渉測位に関する信頼性判定を行なっている。
【０００３】
また、測位衛星受信手段および無線通信手段を利用して、任意の移動体と他の移動体間の相対位置を検出する相対位置検出システムが知られている（特許文献２）。この相対位置検出システムでは、衛星の組み合わせがより一致するように、過去に使用している衛星の状態から衛星の使用度を算出し、そのデータを基に衛星が一致しやすいように共通に使用する衛星を選択している。
【０００４】
また、複数の車両が保持するＧＰＳ受信機、測域センサ、および車車間通信を利用した、車両の安全走行支援アプリケーションのための協調型位置推定手法が提案されている（非特許文献１）。この協調型位置推定手法では、車載器非搭載車両が混在する環境において、自車両および周辺車両の位置を高精度に推定するために、異なる車両が保持するＧＰＳおよび測域センサの異なる時間の観測結果を推定に利用している。また、中心極限定理に基づき各推定結果の精度を求めることにより、最も高精度な推定結果を車両間で共有している。本手法の考え方は、基本的に、さまざまなセンサの計測値を利用して確率論を応用し、自車両の位置精度を向上させる手法（例えば、カルマンフィルタ等を用いる方法）を、車両間の相対位置に適用したものである。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００９−２６４８４４号公報
【特許文献２】特願平１１−２６７５号公報
【非特許文献】
【０００６】
【非特許文献１】藤井彩恵、山口弘純、東野輝夫、金田茂、高井峰生、「車車間通信を用いた協調型車両位置推定手法」、情報処理学会第４３回高度交通システム研究会（ＩＴＳ）、２０１０年１１月
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上記の特許文献１に記載の技術では、ＧＰＳの干渉測位を実施するために、受信した搬送波を通信する必要がある、という問題がある。また、上記の特許文献１では、三台以上の車両の相対位置を算出する際における通信データ量の低減手法が提案されているが、二台の場合には適用できない、という問題がある。また、搬送波の整数値バイアスの算出を利用した相対位置信頼度算出法が提案されているが、マルチパスを受けている際には、そもそも整数値バイアスの推定が困難である、という問題がある。
【０００８】
また、上記の特許文献２に記載の技術では、衛星を共通化することで、相対位置の精度を向上させることはできるが、そもそも共通している衛星が４以下の場合には測位ができない、という問題がある。
【０００９】
また、上記の非特許文献１に記載の技術では、マルチパスが発生している場合には、正しい値を推定することができない、という問題がある。
【００１０】
本発明は、上記の問題点を解決するためになされたもので、少ないデータ通信量で、他の装置との相対位置を精度良く推定することができる相対位置推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１１】
上記目的を達成するために、本発明の相対位置推定装置は、移動体と他の装置との相対位置を推定する、前記移動体に搭載される相対位置推定装置であって、複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、前記移動体の運動状態を検出する検出手段と、前記取得手段により取得された前記発信源情報、及び前記検出手段によって検出された運動状態に基づいて、前記移動体の位置と、前記移動体の速度ベクトル、又は前記移動体の速度及び前記移動体の方位角を示す速度情報とを算出する算出手段と、他の装置の位置及び前記速度情報を受信する受信手段と、前記受信手段によって受信された前記他の装置の位置と、前記算出手段により算出された前記移動体の位置との差分から、各時刻又は各地点について、前記移動体と前記他の装置との相対位置を算出する相対位置算出手段と、前記算出手段により算出された前記移動体の速度ベクトル又は前記速度情報を所定時間分積算して、前記所定時間における前記移動体の位置の軌跡を算出する第１軌跡算出手段と、前記受信手段によって受信された前記速度情報を前記所定時間分積算して、前記所定時間における前記他の装置の位置の軌跡を算出する第２軌跡算出手段と、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置とに基づいて、前記軌跡上の特定点における前記相対位置を推定する相対位置推定手段と、を含んで構成されている。
【００１２】
本発明に係る相対位置推定プログラムは、コンピュータを、移動体と他の装置との相対位置を推定する、前記移動体に搭載される相対位置推定装置の各手段として機能させるための相対位置推定プログラムであって、前記コンピュータを、複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段、前記取得手段により取得された前記発信源情報、及び前記移動体の運動状態を検出する検出手段によって検出された運動状態に基づいて、前記移動体の位置と、前記移動体の速度ベクトル、又は前記移動体の速度及び前記移動体の方位角を示す速度情報とを算出する算出手段、他の装置の位置及び前記速度情報を受信する受信手段によって受信された前記他の装置の位置と、前記算出手段により算出された前記移動体の位置との差分から、各時刻又は各地点について、前記移動体と前記他の装置との相対位置を算出する相対位置算出手段、前記算出手段により算出された前記移動体の速度ベクトル又は前記速度情報を所定時間分積算して、前記所定時間における前記移動体の位置の軌跡を算出する第１軌跡算出手段、前記受信手段によって受信された前記速度情報を前記所定時間分積算して、前記所定時間における前記他の装置の位置の軌跡を算出する第２軌跡算出手段、及び前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置とに基づいて、前記軌跡上の特定点における前記相対位置を推定する相対位置推定手段として機能させるための相対位置推定プログラムである。
【００１３】
本発明によれば、取得手段によって、複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する。検出手段によって、前記移動体の運動状態を検出する。検出手段は慣性センサであり、例えば、自車両の角速度または角加速度を検出するジャイロセンサや、方位角を検出する磁気方位計、自車両の速度を検出する速度センサを用いることができる。
【００１４】
そして、算出手段によって、取得手段により取得された前記発信源情報、及び前記検出手段によって検出された運動状態に基づいて、前記移動体の位置と、前記移動体の速度ベクトル、又は前記移動体の速度及び前記移動体の方位角を示す速度情報とを算出する。受信手段によって、他の装置の位置及び前記速度情報を受信する。相対位置算出手段によって、前記受信手段によって受信された前記他の装置の位置と、前記算出手段により算出された前記移動体の位置との差分から、各時刻又は各地点について、前記移動体と前記他の装置との相対位置を算出する。
【００１５】
そして、第１軌跡算出手段によって、前記算出手段により算出された前記移動体の速度ベクトル又は前記速度情報を所定時間分積算して、前記所定時間における前記移動体の位置の軌跡を算出する。第２軌跡算出手段によって、前記受信手段によって受信された前記速度情報を前記所定時間分積算して、前記所定時間における前記他の装置の位置の軌跡を算出する。
【００１６】
そして、相対位置推定手段によって、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置とに基づいて、前記軌跡上の特定点における前記相対位置を推定する。
【００１７】
このように、他の装置の位置及び速度情報を受信すると共に、移動体の位置の軌跡と、他の装置の位置の軌跡と、移動体と他の装置との相対位置を算出して、移動体の位置の軌跡と、他の装置の位置の軌跡と、各時刻又は各地点の相対位置とに基づいて、軌跡上の特定点における相対位置を推定することにより、少ないデータ通信量で、他の装置との相対位置を精度良く推定することができる。
【００１８】
本発明に係る相対位置は、前記移動体の位置を基準とした前記他の装置の相対位置であり、前記相対位置推定手段は、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡、及び前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置から求められる各時刻又は各地点の前記他の装置の位置との差分の和を最小とする、前記他の装置の位置の軌跡を求め、前記求められた前記他の装置の位置の軌跡と前記移動体の位置の軌跡とに基づいて、前記軌跡上の特定点における前記相対位置を推定するようにすることができる。これによって、他の装置との相対位置を精度良く推定することができる。
【００１９】
本発明に係る相対位置は、前記他の装置の位置を基準とした前記移動体の相対位置であり、前記相対位置推定手段は、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡、及び前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置から求められる各時刻又は各地点の前記移動体の位置との差分の和を最小とする、前記移動体の位置の軌跡を求め、前記求められた前記移動体の位置の軌跡と前記他の装置の位置の軌跡とに基づいて、前記軌跡上の特定点における前記相対位置を推定するようにすることができる。これによって、他の装置との相対位置を精度良く推定することができる。
【００２０】
また、上記の相対位置推定装置は、前記差分の和が最小とされたときの前記差分の各々に基づいて、前記相対位置推定手段によって推定された前記相対位置の誤差を算出する相対位置誤差算出手段を更に含むようにすることができる。これによって、推定された相対位置の誤差を精度良く算出することができる。
【００２１】
また、上記の他の装置は、複数の情報発信源の各々から発信された前記発信源情報を取得する第２取得手段と、前記他の装置の運動状態を検出する第２検出手段と、前記第２取得手段により取得された前記発信源情報、及び前記第２検出手段によって検出された運動状態に基づいて、前記他の装置の位置と、前記速度情報とを算出する第２算出手段と、前記第２算出手段によって算出された前記他の装置の位置、前記速度情報、及び前記第２取得手段によって取得した前記発信源情報を発信した前記情報発信源の識別情報を送信する送信手段を含み、前記受信手段は、前記送信手段によって送信された前記他の装置の位置、前記速度情報、及び前記情報発信源の識別情報を受信し、前記相対位置推定手段は、前記受信手段によって受信した前記識別情報が示す前記情報発信源を全て含む複数の情報発信源からの前記発信源情報に基づいて算出された前記移動体の位置に基づく前記相対位置について求められる前記差分を、他の相対位置について求められる前記差分より小さくするように、前記差分の和を最小とする軌跡を求め、前記軌跡上の特定点における前記相対位置を推定するようにすることができる。これによって、他の装置との相対位置をより精度良く推定することができる。
【００２２】
また、上記の算出手段は、前記情報発信源の各々の位置に関する情報、及び前記情報発信源の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記情報発信源の各々の方向を算出し、時系列の前記情報発信源の各々の位置に関する情報に基づいて、前記情報発信源の各々の速度を算出し、前記移動体から見た前記情報発信源の各々の方向、前記情報発信源の各々の速度、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体の情報発信源の各々の方向の速度を算出し、複数の前記移動体の情報発信源の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出することができる。
【００２３】
また、上記の情報発信源を、前記情報発信源の各々の位置に関する情報として衛星軌道情報を、前記情報発信源の各々と前記移動体との距離に関する情報として擬似距離情報を、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報としてドップラー周波数情報を発信するＧＰＳ衛星とすることができる。情報発信源としては、例えば、擬似衛星やビーコンなどのように、発信源情報を発信するものであればよいが、代表的には、ＧＰＳ衛星とすることができる。
【００２４】
なお、本発明の相対位置推定プログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２５】
以上説明したように、本発明の相対位置推定装置及びプログラムによれば、他の装置の位置及び速度情報を受信すると共に、移動体の位置の軌跡と、他の装置の位置の軌跡と、移動体と他の装置との相対位置を算出して、移動体の位置の軌跡と、他の装置の位置の軌跡と、各時刻又は各地点の相対位置とに基づいて、軌跡上の特定点における相対位置を推定することにより、少ないデータ通信量で、他の装置との相対位置を精度良く推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２６】
【図１】本発明の第１の実施の形態に係る相対位置推定装置及び車載器を示すブロック図である。
【図２】本発明の第１の実施の形態に係る相対位置推定装置の位置・速度ベクトル算出部を示すブロック図である。
【図３】三角測量の原理に従って自車両の位置が推定される様子を示すイメージ図である。
【図４】各ＧＰＳ衛星の速度ベクトル及び各ＧＰＳ衛星の方向に基づいて、ＧＰＳ衛星方向の自車両の速度を算出する様子を示すイメージ図である。
【図５】自車両と相手車両とにおけるＧＰＳ誤差を説明するための図である。
【図６】相手車両の軌跡をフィッティングする様子を示すイメージ図である。
【図７】残差が大きい相対位置を棄却してから、相手車両の軌跡を再度フィッティングする様子を示すイメージ図である。
【図８】軌跡に誤差があり、残差のばらつきが大きくなっている様子を示すイメージ図である。
【図９】マルチパスが全域で発生して、残差のばらつきが大きくなっている様子を示すイメージ図である。
【図１０】第１の実施の形態に係る相対位置推定装置のコンピュータにおける位置速度算出処理ルーチンの内容を示すフローチャートである。
【図１１】第１の実施の形態に係る相対位置推定装置のコンピュータにおけるＧＰＳ情報に基づく自車両方位角推定処理ルーチンの内容を示すフローチャートである。
【図１２】第１の実施の形態に係る相対位置推定装置のコンピュータにおける速度ベクトル算出処理ルーチンの内容を示すフローチャートである。
【図１３】第１の実施の形態に係る相対位置推定装置のコンピュータにおける相対位置推定処理ルーチンの内容を示すフローチャートである。
【図１４】本発明の第２の実施の形態に係る相対位置推定装置の位置・速度ベクトル算出部を示すブロック図である。
【図１５】（Ａ）使用するＧＰＳ衛星を共通化できない場合を説明するための図、及び（Ａ）使用するＧＰＳ衛星を共通化できる場合を説明するための図である。
【図１６】第２の実施の形態に係る相対位置推定装置のコンピュータにおける位置速度算出処理ルーチンの内容を示すフローチャートである。
【図１７】本発明の第３の実施の形態に係る路側機を示すブロック図である。
【図１８】路側機の軌跡をフィッティングする様子を示すイメージ図である。
【発明を実施するための形態】
【００２７】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、第１の実施の形態では、車両に搭載され、ＧＰＳ衛星から発信されたＧＰＳ情報を取得すると共に、他の車両と車車間通信を行い、自車両に対する他の車両の相対位置を推定する相対位置推定装置に、本発明を適用した場合を例に説明する。
【００２８】
図１に示すように、第１の実施の形態に係る相対位置推定装置１０は、ＧＰＳ衛星からの電波を受信するＧＰＳ受信部１２と、自車両のヨーレートを検出するジャイロセンサ１４と、自車両の速度を検出する速度センサ１６と、他の車両と車車間通信を行うための無線通信部１８と、ＧＰＳ受信部１２によって受信されたＧＰＳ衛星からの受信信号、無線通信部１８によって受信したデータ、並びにジャイロセンサ１４及び速度センサ１６の検出値に基づいて、自車両に対する当該他の車両の相対位置を推定する処理を実行するコンピュータ２０と、を備えている。なお、ジャイロセンサ１４及び速度センサ１６が、検出手段の一例であり、無線通信部１８が、受信手段の一例である。
【００２９】
ＧＰＳ受信部１２は、複数のＧＰＳ衛星からの電波を受信して、受信した全てのＧＰＳ衛星からの受信信号から、ＧＰＳ衛星の情報として、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報（エフェメリス）、ＧＰＳ衛星が電波を送信した時刻、受信信号の強度、周波数などを取得し、コンピュータ２０に出力する。
【００３０】
無線通信部１８は、他の車両に搭載されている車載器８０から送信されたデータを受信する。なお、以下では、当該他の車両を、相手車両と称する。
【００３１】
ここで、車載器８０は、相対位置推定装置１０と同様に、ＧＰＳ衛星からの電波を受信するＧＰＳ受信部８２と、相手車両のヨーレートを検出するジャイロセンサ８４と、相手車両の速度を検出する速度センサ８６と、車車間通信を行うための無線通信部８８と、相手車両の位置及び速度ベクトルを算出する処理を実行するコンピュータ９０と、を備えている。コンピュータ９０は、ＧＰＳ受信部８２によって受信されたＧＰＳ衛星からの受信信号、無線通信部８８によって受信したデータ、並びにジャイロセンサ８４及び速度センサ８６の検出値に基づいて、相手車両の位置及び速度ベクトルを算出する位置・速度ベクトル算出部９２を備えている。位置・速度ベクトル算出部９２は、後述する位置・速度ベクトル算出部２４と同様の構成であるため、説明を省略する。なお、ジャイロセンサ８４及び速度センサ８６が、第２検出手段の一例であり、無線通信部８８が、送信手段の一例であり、位置・速度ベクトル算出部９２が、第２取得手段及び第２算出手段の一例である。
【００３２】
車載器８０は、所定時間間隔で、相手車両の位置及び速度ベクトルを算出して、各時刻について、位置及び速度ベクトルと、位置を算出する処理で使用したＧＰＳ衛星の衛星番号とを含む計測データを、無線通信部８８によって送信する。なお、計測データには、更に、算出された位置がいつの位置であるかを示す時刻、すなわち、車載器８０でＧＰＳ衛星からの電波を受信した時刻が含まれている。無線通信部８８によって送信されるデータを表１に示す。
【００３３】
【表１】
【００３４】
コンピュータ２０は、ＣＰＵ、後述する位置速度算出処理ルーチン及び相対位置推定処理ルーチン及びを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。
【００３５】
コンピュータ２０を以下で説明する位置速度算出処理ルーチン及び相対位置推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、無線通信部１８によって受信した相手車両からの計測データを記憶する受信データ記憶部２２と、ＧＰＳ受信部１２によって受信されたＧＰＳ衛星からの受信信号、並びにジャイロセンサ１４及び速度センサ１６の検出値に基づいて、各時刻における自車両の位置及び速度ベクトルを算出する位置・速度ベクトル算出部２４と、各時刻について、算出された自車両の位置及び受信した計測データに含まれる相手車両の位置の差分を算出して自車両を基準とした相手車両の相対位置を算出する位置差分相対位置算出部２６と、各時刻で取得したＧＰＳ衛星の情報に含まれる衛星番号及び受信した計測データに含まれるＧＰＳ衛星の衛星番号に基づいて、各時刻に対する重みを算出する重み算出部２８と、各時刻について算出された、自車両の位置、速度ベクトル、相対位置、及び重みを記憶するデータ記憶部３０とを備えている。なお、位置・速度ベクトル算出部２４は、算出手段の一例であり、位置差分相対位置算出部２６が、相対位置算出手段の一例である。
【００３６】
受信データ記憶部２２は、無線通信部１８によって相手車両からの計測データを受信するたびに、計測データを記憶し、蓄積する。
【００３７】
位置・速度ベクトル算出部２４は、図２に示すように、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ擬似距離データ、ドップラー周波数、及びＧＰＳ衛星の位置座標を算出して取得するＧＰＳ情報取得部５０と、取得したＧＰＳ情報に基づいて、自車両の位置を算出する位置算出部５１と、取得したＧＰＳ情報に基づいて、自車両の方位角を推定する方位角推定部５２と、推定された方位角、ジャイロセンサ１４の検出値、及び速度センサ１６の検出値を対応させて記憶するデータ記憶部５４と、データ記憶部５４に記憶された各種データに基づいて、自車両の速度ベクトルを推定する速度ベクトル推定部５６と、を含んだ構成で表すことができる。なお、ＧＰＳ情報取得部５０が、取得手段の一例である。
【００３８】
ＧＰＳ情報取得部５０は、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ衛星が電波を送信した時刻及び自車両で電波を受信した時刻に基づいて、ＧＰＳ擬似距離データを算出する。また、ＧＰＳ情報取得部５０は、各ＧＰＳ衛星から送信される信号の既知の周波数と、各ＧＰＳ衛星から受信した受信信号の周波数とに基づいて、各ＧＰＳ衛星からの受信信号のドップラー周波数を各々算出する。なお、ドップラー周波数は、ＧＰＳ衛星と自車との相対速度による、搬送波周波数のドップラーシフト量を観測したものである。また、ＧＰＳ情報取得部５０は、ＧＰＳ衛星の軌道情報及びＧＰＳ衛星が電波を送信した時刻に基づいて、ＧＰＳ衛星の位置座標を各々算出する。また、ＧＰＳ情報取得部５０は、取得したＧＰＳ衛星の情報に含まれる各ＧＰＳ衛星の衛星番号を、重み算出部２８へ出力する。
【００３９】
位置算出部５１は、以下のように、ＧＰＳ情報取得部５０によって取得された各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。
【００４０】
ＧＰＳを用いた測位では、図３に示すように、既知であるＧＰＳ衛星の位置座標と、各ＧＰＳ衛星から受信した受信信号の伝播距離である擬似距離とに基づいて、三角測量の原理に従って、自車両の位置が推定される。
【００４１】
ここで、ＧＰＳ衛星までの真の距離ｒｊは、以下の（１）式で表され、ＧＰＳで観測される擬似距離ρｊは、以下の（２）式で表される。
【００４２】
【数１】
【００４３】
ただし、（Ｘｊ，Ｙｊ，Ｚｊ）がＧＰＳ衛星ｊの位置座標であり、（ｘ，ｙ，ｚ）が自車両の位置座標である。ｓは、ＧＰＳ受信部１２の時計誤差による距離誤差である。
【００４４】
上記（１）式、（２）式より、４つ以上のＧＰＳ衛星のＧＰＳ擬似距離データから得られる以下の（３）式の連立方程式を解くことによって、自車両の位置（ｘ、ｙ、ｚ）が算出される。
【００４５】
【数２】
【００４６】
また、方位角推定部５２は、さらに、取得した各ＧＰＳ衛星のドップラー周波数に基づいて、各ＧＰＳ衛星に対する自車両の相対速度を算出する相対速度算出部６０と、取得した各ＧＰＳ衛星の位置座標の時系列データに基づいて、各ＧＰＳ衛星の速度ベクトルを算出する衛星速度算出部６２と、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星の方向（角度の関係）を算出する衛星方向算出部６６と、算出された相対速度、各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の方向に基づいて、各ＧＰＳ衛星方向の自車両の速度を算出する衛星方向自車速算出部６８と、算出された複数の各ＧＰＳ衛星方向の自車両の速度に基づいて、自車両の速度ベクトルを算出する自車両速度算出部７０と、算出された自車両の速度ベクトルに基づいて自車両の方位角を算出する自車両方位角算出部７２と、を備えた構成で表すことができる。
【００４７】
相対速度算出部６０は、ドップラー周波数とＧＰＳ衛星に対する相対速度との関係を表わす以下の（４）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度を算出する。
【００４８】
【数３】
【００４９】
ただし、ｖｊはＧＰＳ衛星ｊに対する相対速度であり、Ｄ１ｊはＧＰＳ衛星ｊから得られるドップラー周波数（ドップラーシフト量）である。また、Ｃは光速であり、Ｆ１は、ＧＰＳ衛星から送信される信号の既知のＬ１周波数である。
【００５０】
衛星速度算出部６２は、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（３次元速度ＶＸｊ、ＶＹｊ、ＶＺｊ）を算出する。例えば、非特許文献（Ｐｒａｔａｐ Ｍｉｓｒａ ａｎｄ Ｐｅｒ Ｅｎｇｅ原著 日本航海学会ＧＰＳ研究会訳：“精説ＧＰＳ基本概念・測位原理・信号と受信機”正陽文庫，２００４．）に記載された方法を用いて、各ＧＰＳ衛星の速度ベクトルを算出することができる。
【００５１】
衛星方向算出部６６は、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係（水平方向に対する仰角θｊ、北方向に対する方位角φｊ）を、各ＧＰＳ衛星の方向として算出する。
【００５２】
衛星方向自車速算出部６８は、図４に示すように、算出された各ＧＰＳ衛星に対する自車両の相対速度ｖｊ、各ＧＰＳ衛星の速度ベクトル（ＶＸｊ、ＶＹｊ、ＶＺｊ）、及び各ＧＰＳ衛星の方向Ｒｊ（θｊ、φｊ）に基づいて、以下の（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖｊを算出する。
【００５３】
【数４】
【００５４】
ｖｊは、ＧＰＳ衛星ｊに対する自車両の相対速度（衛星方向におけるＧＰＳ衛星との相対速度）である。また、Ｖｓｊは、自車方向のＧＰＳ衛星ｊの速度であり、Ｖｓｊ＝Ｒｊ［ＶＸｊ，ＶＹｊ，ＶＺｊ］Ｔにより求まる。また、Ｖｖｊは、ＧＰＳ衛星ｊの方向の自車速であり、ｖＣｂは、クロックバイアス変動である。
【００５５】
上述したように、ＧＰＳ衛星方向の自車速は、ＧＰＳ衛星位置の三次元位置ではなく、ＧＰＳ衛星との方位関係によってのみ算出される。ＧＰＳ衛星は遥か遠方にあり、１日で地球をほぼ２周するため１分間の角度変化は０．５度である。通常、ＧＰＳ衛星とＧＰＳ受信機との時計誤差は通常１ｍｓｅｃ以下であるため、ＧＰＳ衛星との方位関係に大きな影響はない。また、同じくＧＰＳ衛星は遥か遠方にあるため、自車の位置決定に数１００ｍ程度の誤差が生じていたとしても、ＧＰＳ衛星との方位関係に大きな影響はない。このため、擬似距離に誤差が乗りやすい状況であったとしても、ＧＰＳ衛星方向の自車速は、擬似距離に比較して正確に算出され得る。
【００５６】
自車両速度算出部７０は、以下に説明するように、自車両の速度ベクトルの最適推定を行う。
【００５７】
まず、自車の速度ベクトルを（Ｖｘ，Ｖｙ，Ｖｚ）としたとき、ＧＰＳ衛星方向の自車両の速度Ｖｖｊとの関係は以下の（６）式で表される。
【００５８】
【数５】
【００５９】
各ＧＰＳ衛星ｊについて得られる上記（６）式より、Ｖｘ，Ｖｙ，Ｖｚ及びｖＣｂを推定値とした、以下の（７）式で表される連立方程式が得られる。
【００６０】
【数６】
【００６１】
電波を受信したＧＰＳ衛星が４個以上である場合に、上記（７）式の連立方程式を解くことによって、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。
【００６２】
自車両方位角算出部７２は、自車両速度算出部７０により算出された自車両の速度ベクトルから三角関数を用いて自車両の方位角を算出し、方位角推定部５２での方位角の推定結果としてデータ記憶部５４に記憶する。ここで推定された方位角は、ＧＰＳ情報に基づいて推定された方位角であるので、以下、「推定方位角」という。これに対し、ジャイロセンサ１４の検出値から求まる自車両の方位角を「観測方位角」という。ただし、ジャイロセンサ１４では、方位角の変化分しか計測できないため、ある初期値を基準に時間経過分を積分することで、ジャイロセンサ１４の検出値から観測方位角を算出する。例えば、下記（８）式では、ある時間ｔ０の初期値Ψｉｎｉｔ（ｔ０）を基準にｋ秒前の観測方位角Ψｍ（ｔ０−ｋ）を算出している。つまり、データ記憶部５４にジャイロセンサ１４の検出値Ψｇｙｒｏの微分値を記憶しておくことで、観測方位角が算出可能になる。
【００６３】
【数７】
【００６４】
なお、データ記憶部５４には、所定時間分の推定方位角と共に、対応する所定時間内に検出されたジャイロセンサ１４の検出値、及び速度センサ１６の検出値が記憶される。
【００６５】
また、速度ベクトル推定部５６は、さらに、推定方位角と観測方位角とを統合して方位角の最適値を推定する最適推定部７４と、推定された最適値と推定方位角との残差の分散に基づいて、推定方位角の精度を判定する精度判定部７６と、精度判定部７６での判定結果に基づいて自車両の方位角を推定し、推定した方位角を用いて速度ベクトルを算出する速度ベクトル算出部７８と、を含んだ構成で表すことができる。
【００６６】
最適推定部７４は、データ記憶部５４から所定時間分のジャイロセンサ１４の検出値である自車両のヨーレートを読み出し、観測毎のヨーレートを各々積分して時系列に積算して自車両の観測方位角を算出する。また、対応する所定期間内に推定された推定方位角をデータ記憶部５４から読み出す。
【００６７】
また、最適推定部７４は、最小二乗法等に代表される最適計算により、推定方位角と観測方位角とを統合した方位角の最適値を推定する。例えば、最小二乗法では、下記（９）式により推定方位角Ψｇｐｓと、観測方位角Ψｍとの一定時間における差の和が最小になるように、観測方位角Ψｍの算出の基準となる最適な初期値Ψｉｎｉｔ（ｔ０）を探索することで、方位角の最適値を推定する。
【００６８】
【数８】
【００６９】
精度判定部７６は、最適推定部７４で推定された方位角の最適値と推定方位角の各々との残差を算出し、所定時間分の残差のばらつき（分散または標準偏差）が所定の閾値以下か否かにより、方位角の最適値の精度を判定する。
【００７０】
速度ベクトル算出部７８は、最適推定部７４により推定された所定時間分の方位角の最適値が精度判定部７６により精度が高いと判定された場合には、その方位角の最適値及び速度センサ１６で検出された速度を用いて、速度ベクトルを算出する。一方、精度が低いと判定された最適値については、速度ベクトルの算出に用いない。その間の速度ベクトルは、過去に推定された方位角を用いてジャイロセンサ１４の検出値を積算して方位角を算出して、速度センサ１６で検出された速度を用いて算出する。速度ベクトル算出部７８は、算出した速度ベクトルを速度ベクトルの推定結果として出力する。
【００７１】
また、位置差分相対位置算出部２６は、自車両を、相対位置を算出する基準として、各時刻について、算出された自車両の位置及び受信した計測データに含まれる相手車両の位置の差分により、相対位置を算出する。このように、位置差分相対位置算出部２６は、単純に二台の車両の位置の差分を計算することで、相対位置の算出を行う。このとき、１００ｍ程度以下の距離にある二台の車両の相対位置を算出する場合においては、図５に示すように、ＧＰＳの成層圏、対流圏に起因する誤差が共通化されるため、各車両の絶対位置よりも相対位置が正確になる。そのため、単純に位置の差分を計算することにより、マルチパスが発生していない箇所では、ばらつきがあるが、正確な相対位置が算出することができる。
【００７２】
また、重み算出部２８は、各時刻について、受信した計測データに含まれるＧＰＳ衛星の衛星番号のうち、自車両で取得したＧＰＳ衛星の情報に含まれる各ＧＰＳ衛星の衛星番号と共通になっている衛星番号の割合が大きいほど、後述する重み付き最小二乗法で用いられる重みを大きく算出する。すなわち、自車両で取得したＧＰＳ衛星の情報に含まれる衛星番号が、受信した計測データに含まれるＧＰＳ衛星の衛星番号の全てを含む場合に、最大の重みを算出する。
【００７３】
また、コンピュータ２０は、更に、所定時間分の自車両の速度ベクトルを積算して、所定時間における自車両の位置の軌跡を算出する軌跡算出部３２と、受信した測定データから得られる所定時間分の相手車両の速度ベクトルを積算し、当該所定時間における相手車両の位置の軌跡を算出する相手軌跡算出部３４と、自車両及び相手車両の軌跡と位置の差分により算出された相対位置とに基づいて、自車両に対する相手車両の相対位置を推定する軌跡利用相対位置推定部３６と、推定された相対位置の誤差を算出する相対位置誤差算出部３８とを備えている。なお、軌跡算出部３２が、第１軌跡算出手段の一例であり、相手軌跡算出部３４が、第２軌跡算出手段の一例である。
【００７４】
軌跡算出部３２は、算出された所定時間分の自車両の速度ベクトルをデータ記憶部３０から取得し、取得した所定時間分の自車両の速度ベクトルを積算して、当該所定時間における自車両の位置の軌跡を算出する。また、相手軌跡算出部３４は、受信データ記憶部２２に記憶された測定データから、同じ所定時間分の相手車両の速度ベクトルを取得し、取得した相手車両の速度ベクトルを積算して、当該所定時間における相手車両の位置の軌跡を算出する。
【００７５】
軌跡利用相対位置推定部３６は、以下に説明するように、自車両に対する相手車両の相対位置を推定する。
【００７６】
まず、自車両及び相手車両の軌跡と、軌跡上の各時刻の位置の差分による相対位置とが既知である。ここで、軌跡は正解であるが、位置の差分による相対位置には、ノイズによるばらつきと、マルチパスによる大きな誤差が含まれている可能性がある。
【００７７】
そこで、軌跡利用相対位置推定部３６は、最小二乗法等に代表される最適計算により、軌跡と位置の差分による相対位置とに対して、軌跡のフィッティングを行って、軌跡の最適値を推定する。図６に示すように、まず、自車両の軌跡を基準にして、各時刻の位置差分による相対位置から、各時刻の相手車両の予測位置を算出する。そして、相手車両の予測位置に対して、最小二乗法により相手車両の軌跡のフィッティングを行って、相手車両の軌跡の最適値を求め、求められた相手車両の軌跡の最適値を用いて、軌跡上の特定点（例えば、終端点）における自車両に対する相手車両の相対位置を算出する。
【００７８】
ここで、特に二台の車両で共通した衛星を使用して測位を行っていた場合には、よりＧＰＳの誤差が共通化されるため、誤差が小さくなる特徴がある。この特徴を利用し、最小二乗の計算の際に、重み算出部２８によって、共通化されている衛星の割合に応じて算出された重みを、データ記憶部３０から取得し、取得した重みを適用して、以下の（１０）式に従って、重み付き最小二乗法による相手車両の軌跡のフィッティングを行う。
【００７９】
【数９】
【００８０】
ただし、Δｙは、観測量（相手車両の軌跡上の位置）と予測値（予測された相手車両の位置）との差（残差)であり、Δｘは未知数であり、Ｇは、一般行列である。また、Ｗが、各時刻の重みを示す重み行列である。
【００８１】
上記（１０）式では、観測量と予測値との差（残差)Δｙが予め設定した重み行列Ｗに応じて最小化されるように、未知数Δｘの算出を行う。重み行列Ｗは、予め既知である誤差の共分散を基に設定することで、誤差の大きさを考慮した推定が可能なことが知られており、共分散の逆数を重み行列Ｇに設定する手法が用いられることが多い。二台の車両で共通した衛星を使用して測位した場合では、相対位置の誤差が小さくなることは知られているため、位置の算出に使用したＧＰＳ衛星の衛星番号に基づいて重み行列Ｗを設定することで、より正確な相対位置を推定することができる。なお、重み行列Ｗは一致している衛星の数の割合で算出する必要はなく、完全に一致しているか、否か、の二種類で設定してもよい。例えば、衛星番号が完全に一致している時刻については重み「１」を設定し完全に一致していない時刻については重み「０．５」を設定するようにしてもよい。
【００８２】
また、軌跡利用相対位置推定部３６は、相手車両の軌跡の最適値と、軌跡上の各時刻の相手車両の位置の予測値との残差（最小二乗の際の残差）について、閾値判定を行い、図７に示すように、残差が閾値以上となる時刻についての位置差分による相対位置を棄却してから、再度、軌跡と位置の差分による相対位置とに対してフィッティングを行って、軌跡の最適値を推定することで、マルチパスの影響を除去する。
【００８３】
相対位置誤差算出部３８は、軌跡利用相対位置推定部３６で求められた相手車両の軌跡の最適値と、軌跡上の各時刻の相手車両の位置の予測値との残差の標準偏差を算出し、相対位置誤差の指標として出力する。
軌跡を利用して相対位置を算出する手法で誤差が発生する場合は、図８に示すような「軌跡に誤差がある場合」と図９に示すような「全域でマルチパスが発生している場合」とである。共に、最小二乗の際の残差のばらつきが大きくなるため、残差の標準偏差を、相対位置誤差の指標とすることができる。
【００８４】
なお、軌跡の最適値を利用して算出した相対位置と、単純に位置の差分により算出した相対位置とを利用して、算出した相対位置の誤差を算出してもよい。
【００８５】
次に、第１の実施の形態に係る相対位置推定装置１０の作用について説明する。
【００８６】
まず、無線通信部１８によって、相手車両の車載器８０から送信された、位置、速度ベクトル、及び衛星番号を含む測定データを受信する毎に、相対位置推定装置１０のコンピュータ２０は、受信した測定データを受信データ記憶部２２に記憶する。
【００８７】
また、ＧＰＳ受信部１２によって、複数のＧＰＳ衛星から電波を繰り返し受信しているときに、コンピュータ２０において、図１０に示す位置速度算出処理ルーチンが繰り返し実行される。
【００８８】
ステップ１００で、ＧＰＳ受信部１２から複数のＧＰＳ衛星の情報を取得すると共に、複数のＧＰＳ衛星のＧＰＳ擬似距離データ、ドップラー周波数、及びＧＰＳ衛星の位置座標を算出して取得する。
【００８９】
そして、ステップ１０２において、各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、上記（１）〜（３）式に従って、自車両の位置を算出する。
【００９０】
次に、ステップ１０４で、ジャイロセンサ１４の検出値である自車両のヨーレート、及び速度センサ１６の検出値である自車両の速度を取得して、データ記憶部５４に記憶する。
【００９１】
次に、ステップ１０６で、後述するＧＰＳ情報に基づく自車両方位角処理を実行して、推定方位角を推定し、次に、ステップ１０８で、後述する速度ベクトル算出処理を実行して、速度ベクトルを推定する。
【００９２】
そして、ステップ１１０において、上記ステップ１００で取得したＧＰＳ衛星の情報と同一時刻（電波の受信時刻が同一）の測定データを、受信データ記憶部２２から取得する。ステップ１１２では、上記ステップ１００で取得したＧＰＳ衛星の情報から得られる衛星番号と、上記ステップ１１０で取得した測定データから得られる衛星番号とに基づいて、重みを算出する。
【００９３】
そして、ステップ１１４において、上記ステップ１０２で算出した自車両の位置と、上記ステップ１１０で取得した測定データから得られる相手車両の位置との差分から、自車両に対する相手車両の相対位置を算出する。次のステップ１１６では、上記ステップ１０２、１０６、１１２、１１４による算出結果を、データ記憶部３０に記憶し、位置速度算出処理ルーチンを終了する。
【００９４】
上記位置速度算出処理ルーチンが、繰り返し実行されることにより、各時刻について算出された自車両の位置、速度ベクトル、重み、及び相対位置が、データ記憶部３０に記憶される。
【００９５】
次に、図１１を参照して、ＧＰＳ情報に基づく自車両方位角推定処理ルーチンについて説明する。
【００９６】
ステップ１０６０で、上記（４）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度ｖｊを算出する。
【００９７】
次に、ステップ１０６２で、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（ＶＸｊ、ＶＹｊ、ＶＺｊ）を算出する。
【００９８】
次に、ステップ１０６６で、上記ステップ１０２で算出された自車両の位置、及び上記ステップ１００で取得された各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係Ｒｊ（水平方向に対する仰角θｊ、北方向に対する方位角φｊ）を、各ＧＰＳ衛星の方向として算出する。
【００９９】
次に、ステップ１０６８で、上記ステップ１０６０で算出された各ＧＰＳ衛星に対する自車両の相対速度ｖｊ、上記ステップ１０６２で算出された各ＧＰＳ衛星の速度ベクトルＶｊ（ＶＸｊ、ＶＹｊ、ＶＺｊ）、及び上記ステップ１０６６で算出された各ＧＰＳ衛星の方向Ｒｊ（θｊ、φｊ）に基づいて、上記（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖｊを算出する。（５）式における自車方向のＧＰＳ衛星ｊの速度Ｖｓｊは、Ｖｓｊ＝Ｒｊ［ＶＸｊ，ＶＹｊ，ＶＺｊ］Ｔにより算出する。
【０１００】
次に、ステップ１０７０で、上記（６）式、及び（７）式に従って、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。
【０１０１】
次に、ステップ１０７２で、上記ステップ１０７０で算出された自車両の速度ベクトルから三角関数を用いて自車両の方位角を算出し、算出した方位角を推定方位角としてデータ記憶部５４に記憶して、リターンする。
【０１０２】
次に、図１２を参照して、速度ベクトル推定処理ルーチンについて説明する。
【０１０３】
ステップ１０８０で、データ記憶部５４から所定期間分のジャイロセンサ１４の検出値である自車両のヨーレートを読み出し、観測毎のヨーレートを各々時系列に積算して自車両の観測方位角を算出する。
【０１０４】
次に、ステップ１０８２で、所定期間内に推定された推定方位角をデータ記憶部５４から読み出し、上記ステップ１０８０で算出した観測方位角の時系列データを平行移動して探索しながら、最小二乗法等に代表される最適計算により、方位角の最適値を推定する。
【０１０５】
次に、ステップ１０８４で、上記ステップ１０８２で推定された方位角の最適値と推定方位角の各々との残差を算出し、所定期間分の残差の分散を算出する。
【０１０６】
次に、ステップ１０８６で、上記ステップ１０８４で算出した残差の分散が予め定めた閾値以下か否可を判定することにより、方位角の最適値の精度を判定する。残差の分散が閾値以下の場合には、方位角の最適値の精度が高いと判断してステップ１０８８へ移行し、上記ステップ１０８２で推定された方位角の最適値を、方位角の推定値として採用する。一方、残差の分散が閾値を超える場合には、方位角の最適値の精度が低いと判断して、ステップ１０９０へ移行し、過去に推定された方位角を用いてジャイロセンサ１４の検出値を積算して算出した方位角を、方位角の推定値として採用する。
【０１０７】
次に、ステップ１０９２で、データ記憶部５４から速度センサ１６で検出された速度を読み出し、上記ステップ１０８８または１０９０で採用された方位角の推定値及び読み出した速度を用いて速度ベクトルを算出し、算出した速度ベクトルを速度ベクトルの推定結果としてデータ記憶部３０に記憶して、リターンする。
【０１０８】
また、データ記憶部３０に、所定時間分の自車両の位置、速度ベクトル、重み、及び相対位置が記憶されると、コンピュータ２０において、図１３に示す相対位置推定処理ルーチンが実行される。
【０１０９】
ステップ１２０において、データ記憶部３０から所定時間分の速度ベクトルを取得し、所定時間分の自車両の速度ベクトルを積算して、所定時間における自車両の軌跡を算出する。次のステップ１２２では、受信データ記憶部２２から対応する所定時間分の相手車両の速度ベクトルを取得し、所定時間分の相手車両の速度ベクトルを積算して、所定期間における相手車両の軌跡を算出する。
【０１１０】
そして、ステップ１２４において、データ記憶部３０から、対応する所定時間内の各時刻について算出された相対位置を取得する。ステップ１２６では、上記ステップ１２０で算出された自車両の軌跡と、上記ステップ１２４で取得した各時刻の相対位置とに基づいて、対応する所定時間内の各時刻における相手車両の予測位置を算出する。
【０１１１】
次のステップ１２８では、データ記憶部３０から、対応する所定時間内の各時刻について算出された重みを取得し、取得した重みを適用した最小二乗法により、上記ステップ１２６で算出された相手車両の予測位置に対して、上記ステップ１２２で算出された相手車両の軌跡をフィッティングし、最適値を推定する。
【０１１２】
そして、ステップ１３０において、上記ステップ１２８で推定された相手車両の軌跡の最適値と、各時刻の相手車両の予測位置とに基づいて、最小二乗法の残差を各時刻について算出し、ステップ１３２において、上記ステップ１３０で算出された残差が閾値以上となる地点（時刻）が存在するか否かを判定する。残差が閾値以上となる地点（時刻）が存在する場合には、ステップ１３４において、該当地点（時刻）の相対位置を棄却して、ステップ１３６へ移行する。
【０１１３】
ステップ１３６では、上記ステップ１２０で算出された自車両の軌跡と、上記ステップ１２４で取得した各時刻の相対位置のうち、上記ステップ１３４で棄却された相対位置以外の相対位置とに基づいて、対応する所定時間内の各時刻における相手車両の予測位置を算出する。
【０１１４】
次のステップ１３８では、上記ステップ１２８と同様に、重みを適用した最小二乗法により、上記ステップ１３６で算出された相手車両の予測位置に対して、上記ステップ１２２で算出された相手車両の軌跡をフィッティングし、最適値を推定する。
【０１１５】
そして、ステップ１４０において、上記ステップ１３８で推定された相手車両の軌跡の最適値と、各時刻の相手車両の予測位置とに基づいて、最小二乗法の残差を各時刻について算出し、ステップ１４２へ移行する。
【０１１６】
一方、上記ステップ１３２で、残差が閾値以上となる地点（時刻）が存在しないと判定された場合には、ステップ１４２へ移行する。
【０１１７】
ステップ１４２では、上記ステップ１２８又はステップ１３８で推定された相手車両の軌跡の最適値と、上記ステップ１２０で算出された自車両の軌跡とに基づいて、軌跡上の終端点における相対位置を算出し、出力する。
【０１１８】
次のステップ１４４では、上記ステップ１３０又はステップ１４０で算出された各時刻の残差に基づいて、上記ステップ１４２で算出された相対位置の誤差を算出して出力し、相対位置推定処理ルーチンを終了する。
【０１１９】
以上説明したように、第１の実施の形態に係る相対位置推定装置によれば、相手車両の位置及び速度ベクトルを受信すると共に、自車両の位置の軌跡と、相手車両の位置の軌跡と、自車両に対する相手車両の相対位置とを算出して、自車両の位置の軌跡と、相手車両の位置の軌跡と、各時刻の相対位置とに基づいて、相手車両の軌跡の最適値を推定し、軌跡上の終端点における相対位置を推定することにより、ＧＰＳの観測データを通信する特許文献１よりも、少ないデータ通信量で、相手車両との相対位置を精度良く推定することができる。ＧＰＳの観測データは、各衛星の擬似距離、ドップラー周波数、搬送波位相等から構成され、その通信容量は衛星に比例する。ＧＰＳで測位を実施するためには４つ以上の衛星が必要なため、少なくとも４×３（擬似距離、ドップラー周波数、搬送波位相）＝１２個のデータを通信する必要がある。一方、本発明では、通信データは上記表１に示すデータに限られ、位置（３次元）、速度ベクトル（３次元）、時刻、及び衛星番号でよい。衛星番号を、各衛星の使用状況を示すビットで表せば、ＧＰＳであれば最大３２個の衛星が想定されるため、３２ｂｉｔ＝４ｂｙｔｅの容量でよい。よって、従来手法と比較して、少なく、かつ一定の通信容量で相対位置の推定が可能である。
【０１２０】
また、ＧＰＳドップラーと慣性センサを組み合わせることで、精度が高い軌跡が算出可能である。マルチパスが発生している場所においても、正確な軌跡を算出することができる。加えて、１００ｍ程度の相対位置では、共に測位に用いている衛星が共通化されていることが多いため、マルチパスが無い場所では単純にＧＰＳ測位結果の差分を用いたとしても、ばらつきはあるが精度が高い相対位置を計測することができる。以上の技術的な背景により、位置の差分による相対位置と二台の車両の軌跡に基づいて、最小二乗法によるフィッティングにより、ばらつきが抑えられた相対位置を推定することができ、残差で閾値を設定することでマルチパスの棄却ができる。加えて、衛星が共通化されている箇所に対して、重み付き最小二乗法で用いる重みを大きくすることで、推定した相対位置の精度を高めることができる。
【０１２１】
また、二台の車両の軌跡を利用して相対位置を算出することで、残差を利用したマルチパスの影響の除去することができると共に、ＧＰＳが受信できない場所でも相対位置を計測することができる。
【０１２２】
また、ＧＰＳの観測データ（擬似距離・ドップラー・搬送波位相）等を無線により通信する必要がないため、通信データ量の少なくすることができる。
【０１２３】
次に、第２の実施の形態に係る相対位置推定装置について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【０１２４】
第２の実施の形態では、自車両の位置を算出する際に、相手車両の位置の算出において使用したＧＰＳ衛星と共通となるように、使用するＧＰＳ衛星を選択している点が、第１の実施の形態と異なっている。
【０１２５】
第２の実施の形態に係る相対位置推定装置では、図１４に示すように、位置・速度ベクトル算出部２２４を、ＧＰＳ情報取得部５０と、取得したＧＰＳ衛星の情報に含まれる衛星番号、及び受信した測定データから得られる衛星番号に基づいて、位置の算出で使用するＧＰＳ衛星を選択する衛星選択部２５０と、位置算出部５１と、方位角推定部５２と、データ記憶部５４と、速度ベクトル推定部５６と、を含んだ構成で表すことができる。
【０１２６】
衛星選択部２５０は、ＧＰＳ情報取得部５０によって取得したＧＰＳ衛星の情報から衛星番号を取得すると共に、受信データ記憶部２２に記憶されている同時刻の測定データから、相手車両の位置の計算で使用したＧＰＳ衛星の衛星番号を取得する。そして、衛星選択部２５０は、相手車両の位置の計算で使用したＧＰＳ衛星の衛星番号の全てが、自車両で取得したＧＰＳ衛星の情報に含まれる衛星番号に含まれている場合には、完全に衛星を共通化させるように、相手車両の位置の計算で使用したＧＰＳ衛星の全てを、位置の算出において使用するＧＰＳ衛星として選択する。一方、衛星選択部２５０は、相手車両の位置の計算で使用したＧＰＳ衛星の衛星番号の少なくとも１つが、自車両で取得したＧＰＳ衛星の情報に含まれる衛星番号に含まれていない場合には、ＧＰＳ衛星の選択を行わない。
【０１２７】
例えば、図１５（Ａ）に示すように、相手車両の位置の計算で使用したＧＰＳ衛星の衛星番号の少なくとも１つが、自車両で取得したＧＰＳ衛星の情報の衛星番号に含まれていない場合には、位置算出部５１において、自車両で電波を受信した全てのＧＰＳ衛星を使用して、自車両の位置を算出する。
【０１２８】
一方、図１５（Ｂ）に示すように、相手車両の位置の計算で使用したＧＰＳ衛星の衛星番号の全てが、自車両で取得したＧＰＳ衛星の情報の衛星番号に含まれている場合には、位置算出部５１において、相手車両の位置の計算で使用したＧＰＳ衛星と同じＧＰＳ衛星を用いて、自車両の位置を算出する。
【０１２９】
これによって、自車両と相手車両で共通したＧＰＳ衛星を使用して測位を行っていた場合を増やすことができる。
【０１３０】
位置算出部５１は、ＧＰＳ情報取得部５０によって取得された各ＧＰＳ衛星のＧＰＳ擬似距離データ、又は衛星選択部２５０によって選択された各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。
【０１３１】
次に、第２の実施の形態における位置速度算出処理ルーチンについて図１６を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【０１３２】
ステップ１００で、ＧＰＳ受信部１２から複数のＧＰＳ衛星の情報を取得すると共に、複数のＧＰＳ衛星のＧＰＳ擬似距離データ、ドップラー周波数、ＧＰＳ衛星の位置座標を算出して取得する。次のステップ１１０で、取得したＧＰＳ衛星の情報と同一時刻の測定データを、受信データ記憶部２２から取得する。
【０１３３】
そして、ステップ２７０において、上記ステップ１００で取得したＧＰＳ衛星の情報に含まれる衛星番号が、上記ステップ１１０で取得した測定データに含まれる衛星番号を全て含むか否かを判定する。自車両で取得したＧＰＳ衛星の情報に含まれる衛星番号が、測定データに含まれる衛星番号の少なくとも１つを含まない場合には、ステップ１０２へ移行する。一方、自車両で取得したＧＰＳ衛星の情報に含まれる衛星番号が、測定データに含まれる衛星番号を全て含む場合には、ステップ２７２において、自車両の位置の算出で使用するＧＰＳ衛星として、上記ステップ１１０で取得した測定データに含まれる全ての衛星番号のＧＰＳ衛星を選択する。
【０１３４】
そして、ステップ１０２において、上記ステップ２７２で選択された各ＧＰＳ衛星のＧＰＳ擬似距離データ、又はＧＰＳ情報を取得した全てのＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。
【０１３５】
次に、ステップ１０４で、自車両のヨーレート、及び自車両の速度を取得して、データ記憶部５４に記憶する。次に、ステップ１０６で、ＧＰＳ情報に基づく自車両方位角処理を実行して、推定方位角を推定し、次に、ステップ１０８で、速度ベクトル算出処理を実行して、速度ベクトルを推定する。
【０１３６】
そして、ステップ１１２では、上記で取得したＧＰＳ衛星の情報から得られる衛星番号と、上記で取得した測定データから得られる衛星番号とに基づいて、最小二乗法で用いられる重みを算出する。
【０１３７】
そして、ステップ１１４において、自車両に対する相手車両の相対位置を算出する。次のステップ１１６では、上記ステップ１０２、１０６、１１２、１１４における算出結果を、データ記憶部３０に記憶し、位置速度算出処理ルーチンを終了する。
【０１３８】
なお、第２の実施の形態に係る相対位置推定装置の他の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。
【０１３９】
以上説明したように、第２の実施の形態に係る相対位置推定装置によれば、自車両と相手車両で共通したＧＰＳ衛星を使用して測位を行っていた場合を増やすことができるため、より精度よく相対位置を推定することができる。
【０１４０】
なお、上記の第１の実施の形態及び第２の実施の形態では、自車両及び相手車両が移動中である場合について説明したが、これに限定されるものではなく、自車両及び相手車両の何れか一方が停車していてもよい。この場合には、停止している車両の速度ベクトルが０となるため、算出される軌跡が点となるが、上述した軌跡利用相対位置推定部３６と同様に、自車両に対する相手車両の相対位置を推定することができる。
【０１４１】
次に、第３の実施の形態に係る相対位置推定装置について説明する。なお、第３の実施の形態に係る相対位置推定装置の構成は、第１の実施の形態と同様であるため、同一符号を付して説明を省略する。
【０１４２】
第３の実施の形態では、路車間通信により、路側機と通信して、自車両に対する路側機の相対位置を推定している点が、第１の実施の形態と異なっている。
【０１４３】
第３の実施の形態では、相対位置推定装置１０が、図１７に示すような路側機３８０と路車間通信を行う。路側機３８０は、ＧＰＳ受信部８２と、無線通信部８８と、ＧＰＳ受信部８２によって受信されたＧＰＳ衛星からの受信信号に基づいて、路側機３８０の位置を算出する処理を実行するコンピュータ３９０と、を備えている。
【０１４４】
コンピュータ３９０は、位置算出部３９２を備えており、位置算出部３９２は、ＧＰＳ受信部８２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ衛星が電波を送信した時刻及び路側機３８０で電波を受信した時刻に基づいて、ＧＰＳ擬似距離データを算出する。位置算出部３９２は、取得した各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、路側機３８０の位置を算出して、無線通信部８８へ出力すると共に、車速を０とした速度ベクトルを無線通信部８８へ出力する。
【０１４５】
無線通信部８８は、所定時間間隔で、各時刻について、位置及び速度ベクトルと、位置を算出する処理で使用したＧＰＳ衛星の衛星番号とを含む計測データを送信する。なお、計測データには、更に、算出された位置がいつの位置であるかを示す時刻、すなわち、路側機３８０でＧＰＳ衛星から電波を受信した時刻が含まれている。
【０１４６】
なお、第３の実施の形態に係る相対位置推定装置１０の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。
【０１４７】
第３の実施の形態に係る相対位置推定装置１０が、軌跡を利用した相対位置を推定する場合には、図１８に示すように、通信相手である路側機３８０の軌跡が点になる。
【０１４８】
そして、自車両の軌跡を基準にして、各時刻の位置差分による相対位置から、各時刻の路側機３８０の予測位置を算出する。そして、路側機３８０の予測位置に対して、最小二乗法により路側機３８０の軌跡（点）のフィッティングを行うことで、路側機３８０の軌跡（点）の最適値を求め、求められた路側機３８０の軌跡（点）の最適値を用いて、軌跡上の終端点における自車両に対する路側機３８０の相対位置を算出する。
【０１４９】
このように、自車両を基準とした相対位置を算出する対象が、静止物であっても、精度よく相対位置を推定することができる。
【０１５０】
なお、上記の第１の実施の形態〜第３の実施の形態では、ドップラー周波数を用いて、速度ベクトルを算出して、自車両の方位角を算出し、所定期間分の自車両の方位角の算出結果を用いて最適推定を行って、速度ベクトルを算出する場合を例に説明したが、これに限定されるものではない。ドップラー周波数を用いた他の手法により、速度ベクトルを算出してもよい。例えば、自車両速度算出部７０による算出結果を、そのまま自車両の速度ベクトルとして用いてもよい。
【０１５１】
また、上記実施の形態では、速度ベクトルを算出する場合を例に説明したが、これに限定されるものではなく、自車両の方位角を算出すると共に、自車両の速度を検出し、自車両の方位角と速度からなる速度情報を用いてもよい。この場合には、所定時間分の自車両の方位角と速度とから軌跡を算出するようにすればよい。
【０１５２】
また、上記実施の形態では、所定時間間隔で、相対位置及び速度ベクトルを算出して、各時刻の相対位置及び速度ベクトルを求める場合を例に説明したが、これに限定されるものではなく、所定の距離間隔で、相対位置及び速度ベクトルを算出して、各地点の相対位置及び速度ベクトルを求めるようにしてもよい。
【０１５３】
また、上記実施の形態では、自車両を基準とした相対位置を推定する場合を例に説明したが、これに限定されるものではなく、相手車両又は路側機を基準とした自車両の相対位置を推定してもよい。この場合には、相手車両又は路側機の軌跡を基準にして、各時刻の位置差分による相対位置から、各時刻の自車両の予測位置を算出する。そして、自車両の予測位置に対して、最小二乗法により自車両の軌跡のフィッティングを行うことで、自車両の軌跡の最適値を求め、求められた自車両の軌跡の最適値を用いて、相手車両又は路側機に対する自車両の相対位置を推定するようにすればよい。
【０１５４】
また、上記実施の形態では、重み付き最小二乗法を用いて、相手車両の軌跡の最適値を求める場合を例に説明したが、これに限定されるものではなく、重みなしの最小二乗法を用いて相手車両の軌跡の最適値を求めてもよい。この場合には、以下の（１１）式、（１２）式を用いて、相手車両の軌跡の最適値を求めればよい。
【０１５５】
【数１０】
ただし、Δｙは、観測量（相手車両の軌跡上の位置）と予測値（予測された相手車両の位置）との差（残差）であり、Δｘは未知数であり、Ｇは、一般行列である。上記（１１）式は観測量と予測値との差（残差）Δｙと未知数Δｘ、一般行列Ｇの関係を示している。上記（１２）式は、一般的な未知数Δｘを推定する方法である。
【０１５６】
また、ＧＰＳの計測値とジャイロ、速度センサの組み合わせによる複合航法結果を用いて、自車両の位置を算出するようにしてもよい。
【０１５７】
また、上記実施の形態では、ＧＰＳ情報に基づいて自車両の方位角を推定して精度の判定に用いる場合について説明したが、精度の判定に速度を用いてもよい。具体的には、自車両速度算出部７０で算出された速度ベクトルから自車両の速度の絶対値を算出し、最適推定部７４において、速度センサ１６で検出された速度と算出された速度の絶対値とを統合して、速度の最適値を推定し、この最適値と算出された速度の絶対値との残差の分散に基づいて、精度を判定することができる。そして、精度が高いと判定された所定期間の速度ベクトルから推定方位角を算出して、観測方位角と統合した方位角を推定するようにするとよい。
【０１５８】
また、上記実施の形態では、最適値と推定方位角との残差の分散に基づいて、精度を判定する場合について説明したが、これに限定されない。例えば、残差の分布がガウス分布に適合しているか否かにより精度を判定してもよい。
【０１５９】
また、上記実施の形態では、観測方位角を得るために、ジャイロセンサを用いる場合について説明したが、慣性航法系のセンサであればよく、磁気方位計でもよい。
【０１６０】
また、上記実施の形態では、ＧＰＳ衛星のＧＰＳ情報を用いる場合について説明したが、情報発信源の位置に関する情報、情報発信源と移動体との距離に関する情報、及び情報発信源と移動体との相対速度に関する情報を含む発信源情報を発信する情報発信源からの情報が取得できればよい。例えば、擬似衛星から発信される情報を受信するようにしてもよい。
【０１６１】
また、衛星測位システムは、一般的にＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｉｔｅ Ｓｙｓｔｅｍ）と呼ばれ、ＧＰＳだけでなく、ロシアのグロナス（ＧＬＯＮＡＳＳ）、欧州のガリレオ（Ｇａｌｉｌｅｏ）、日本の準天頂衛星システムも予定され、一部は既に適用されている。本発明はＧＰＳに限るものではなく、広くＧＮＳＳに適用可能な技術である。そのため、上記ＧＮＳＳから発信される情報を受信するようにしてもよい。
【０１６２】
また、上記実施の形態では、車両に搭載される相対位置推定装置について説明したが、本発明の相対位置推定装置が搭載される移動体は車両に限定されない。例えば、相対位置推定装置をロボットに搭載してもよいし、歩行者が携帯できるように相対位置推定装置をポータブル端末として構成するようにしてもよい。
【符号の説明】
【０１６３】
１０ 相対位置推定装置
１２、８２ ＧＰＳ受信部
１４、８４ ジャイロセンサ
１６、８６ 速度センサ
１８、８８ 無線通信部
２０、９０、３９０ コンピュータ
２４、５６、９２、２２４ 位置・速度ベクトル算出部
２６ 位置差分相対位置算出部
２８ 重み算出部
３０ データ記憶部
３２ 軌跡算出部
３４ 相手軌跡算出部
３６ 軌跡利用相対位置推定部
３８ 相対位置誤差算出部
５０ 情報取得部
５１ 位置算出部
５２ 方位角推定部
８０ 車載器
２５０ 衛星選択部
３８０ 路側機
３９２ 位置算出部
【技術分野】
【０００１】
本発明は、相対位置推定装置及びプログラムに係り、特に、他の装置と移動体との相対位置を推定するための相対位置推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、３台以上の移動体間の相対位置をＧＰＳの干渉測位によって求めると共に、干渉測位に関する信頼性判定を効率的に行う相対位置検知装置が知られている（特許文献１）。この相対位置検知装置では、３台以上の移動体が互いに通信可能な所定の場所において、基準移動体が観測データを含むデータを非基準移動体に送信し、非基準移動体は観測データ及び基準移動体から受信した観測データを含むデータを用いた干渉測位を行なって基準移動体との相対位置を算出する。そして、他の非基準移動体から整数値バイアスを含むデータを受信して干渉測位に関する信頼性判定を行なっている。
【０００３】
また、測位衛星受信手段および無線通信手段を利用して、任意の移動体と他の移動体間の相対位置を検出する相対位置検出システムが知られている（特許文献２）。この相対位置検出システムでは、衛星の組み合わせがより一致するように、過去に使用している衛星の状態から衛星の使用度を算出し、そのデータを基に衛星が一致しやすいように共通に使用する衛星を選択している。
【０００４】
また、複数の車両が保持するＧＰＳ受信機、測域センサ、および車車間通信を利用した、車両の安全走行支援アプリケーションのための協調型位置推定手法が提案されている（非特許文献１）。この協調型位置推定手法では、車載器非搭載車両が混在する環境において、自車両および周辺車両の位置を高精度に推定するために、異なる車両が保持するＧＰＳおよび測域センサの異なる時間の観測結果を推定に利用している。また、中心極限定理に基づき各推定結果の精度を求めることにより、最も高精度な推定結果を車両間で共有している。本手法の考え方は、基本的に、さまざまなセンサの計測値を利用して確率論を応用し、自車両の位置精度を向上させる手法（例えば、カルマンフィルタ等を用いる方法）を、車両間の相対位置に適用したものである。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００９−２６４８４４号公報
【特許文献２】特願平１１−２６７５号公報
【非特許文献】
【０００６】
【非特許文献１】藤井彩恵、山口弘純、東野輝夫、金田茂、高井峰生、「車車間通信を用いた協調型車両位置推定手法」、情報処理学会第４３回高度交通システム研究会（ＩＴＳ）、２０１０年１１月
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上記の特許文献１に記載の技術では、ＧＰＳの干渉測位を実施するために、受信した搬送波を通信する必要がある、という問題がある。また、上記の特許文献１では、三台以上の車両の相対位置を算出する際における通信データ量の低減手法が提案されているが、二台の場合には適用できない、という問題がある。また、搬送波の整数値バイアスの算出を利用した相対位置信頼度算出法が提案されているが、マルチパスを受けている際には、そもそも整数値バイアスの推定が困難である、という問題がある。
【０００８】
また、上記の特許文献２に記載の技術では、衛星を共通化することで、相対位置の精度を向上させることはできるが、そもそも共通している衛星が４以下の場合には測位ができない、という問題がある。
【０００９】
また、上記の非特許文献１に記載の技術では、マルチパスが発生している場合には、正しい値を推定することができない、という問題がある。
【００１０】
本発明は、上記の問題点を解決するためになされたもので、少ないデータ通信量で、他の装置との相対位置を精度良く推定することができる相対位置推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１１】
上記目的を達成するために、本発明の相対位置推定装置は、移動体と他の装置との相対位置を推定する、前記移動体に搭載される相対位置推定装置であって、複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、前記移動体の運動状態を検出する検出手段と、前記取得手段により取得された前記発信源情報、及び前記検出手段によって検出された運動状態に基づいて、前記移動体の位置と、前記移動体の速度ベクトル、又は前記移動体の速度及び前記移動体の方位角を示す速度情報とを算出する算出手段と、他の装置の位置及び前記速度情報を受信する受信手段と、前記受信手段によって受信された前記他の装置の位置と、前記算出手段により算出された前記移動体の位置との差分から、各時刻又は各地点について、前記移動体と前記他の装置との相対位置を算出する相対位置算出手段と、前記算出手段により算出された前記移動体の速度ベクトル又は前記速度情報を所定時間分積算して、前記所定時間における前記移動体の位置の軌跡を算出する第１軌跡算出手段と、前記受信手段によって受信された前記速度情報を前記所定時間分積算して、前記所定時間における前記他の装置の位置の軌跡を算出する第２軌跡算出手段と、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置とに基づいて、前記軌跡上の特定点における前記相対位置を推定する相対位置推定手段と、を含んで構成されている。
【００１２】
本発明に係る相対位置推定プログラムは、コンピュータを、移動体と他の装置との相対位置を推定する、前記移動体に搭載される相対位置推定装置の各手段として機能させるための相対位置推定プログラムであって、前記コンピュータを、複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段、前記取得手段により取得された前記発信源情報、及び前記移動体の運動状態を検出する検出手段によって検出された運動状態に基づいて、前記移動体の位置と、前記移動体の速度ベクトル、又は前記移動体の速度及び前記移動体の方位角を示す速度情報とを算出する算出手段、他の装置の位置及び前記速度情報を受信する受信手段によって受信された前記他の装置の位置と、前記算出手段により算出された前記移動体の位置との差分から、各時刻又は各地点について、前記移動体と前記他の装置との相対位置を算出する相対位置算出手段、前記算出手段により算出された前記移動体の速度ベクトル又は前記速度情報を所定時間分積算して、前記所定時間における前記移動体の位置の軌跡を算出する第１軌跡算出手段、前記受信手段によって受信された前記速度情報を前記所定時間分積算して、前記所定時間における前記他の装置の位置の軌跡を算出する第２軌跡算出手段、及び前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置とに基づいて、前記軌跡上の特定点における前記相対位置を推定する相対位置推定手段として機能させるための相対位置推定プログラムである。
【００１３】
本発明によれば、取得手段によって、複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する。検出手段によって、前記移動体の運動状態を検出する。検出手段は慣性センサであり、例えば、自車両の角速度または角加速度を検出するジャイロセンサや、方位角を検出する磁気方位計、自車両の速度を検出する速度センサを用いることができる。
【００１４】
そして、算出手段によって、取得手段により取得された前記発信源情報、及び前記検出手段によって検出された運動状態に基づいて、前記移動体の位置と、前記移動体の速度ベクトル、又は前記移動体の速度及び前記移動体の方位角を示す速度情報とを算出する。受信手段によって、他の装置の位置及び前記速度情報を受信する。相対位置算出手段によって、前記受信手段によって受信された前記他の装置の位置と、前記算出手段により算出された前記移動体の位置との差分から、各時刻又は各地点について、前記移動体と前記他の装置との相対位置を算出する。
【００１５】
そして、第１軌跡算出手段によって、前記算出手段により算出された前記移動体の速度ベクトル又は前記速度情報を所定時間分積算して、前記所定時間における前記移動体の位置の軌跡を算出する。第２軌跡算出手段によって、前記受信手段によって受信された前記速度情報を前記所定時間分積算して、前記所定時間における前記他の装置の位置の軌跡を算出する。
【００１６】
そして、相対位置推定手段によって、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置とに基づいて、前記軌跡上の特定点における前記相対位置を推定する。
【００１７】
このように、他の装置の位置及び速度情報を受信すると共に、移動体の位置の軌跡と、他の装置の位置の軌跡と、移動体と他の装置との相対位置を算出して、移動体の位置の軌跡と、他の装置の位置の軌跡と、各時刻又は各地点の相対位置とに基づいて、軌跡上の特定点における相対位置を推定することにより、少ないデータ通信量で、他の装置との相対位置を精度良く推定することができる。
【００１８】
本発明に係る相対位置は、前記移動体の位置を基準とした前記他の装置の相対位置であり、前記相対位置推定手段は、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡、及び前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置から求められる各時刻又は各地点の前記他の装置の位置との差分の和を最小とする、前記他の装置の位置の軌跡を求め、前記求められた前記他の装置の位置の軌跡と前記移動体の位置の軌跡とに基づいて、前記軌跡上の特定点における前記相対位置を推定するようにすることができる。これによって、他の装置との相対位置を精度良く推定することができる。
【００１９】
本発明に係る相対位置は、前記他の装置の位置を基準とした前記移動体の相対位置であり、前記相対位置推定手段は、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡、及び前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置から求められる各時刻又は各地点の前記移動体の位置との差分の和を最小とする、前記移動体の位置の軌跡を求め、前記求められた前記移動体の位置の軌跡と前記他の装置の位置の軌跡とに基づいて、前記軌跡上の特定点における前記相対位置を推定するようにすることができる。これによって、他の装置との相対位置を精度良く推定することができる。
【００２０】
また、上記の相対位置推定装置は、前記差分の和が最小とされたときの前記差分の各々に基づいて、前記相対位置推定手段によって推定された前記相対位置の誤差を算出する相対位置誤差算出手段を更に含むようにすることができる。これによって、推定された相対位置の誤差を精度良く算出することができる。
【００２１】
また、上記の他の装置は、複数の情報発信源の各々から発信された前記発信源情報を取得する第２取得手段と、前記他の装置の運動状態を検出する第２検出手段と、前記第２取得手段により取得された前記発信源情報、及び前記第２検出手段によって検出された運動状態に基づいて、前記他の装置の位置と、前記速度情報とを算出する第２算出手段と、前記第２算出手段によって算出された前記他の装置の位置、前記速度情報、及び前記第２取得手段によって取得した前記発信源情報を発信した前記情報発信源の識別情報を送信する送信手段を含み、前記受信手段は、前記送信手段によって送信された前記他の装置の位置、前記速度情報、及び前記情報発信源の識別情報を受信し、前記相対位置推定手段は、前記受信手段によって受信した前記識別情報が示す前記情報発信源を全て含む複数の情報発信源からの前記発信源情報に基づいて算出された前記移動体の位置に基づく前記相対位置について求められる前記差分を、他の相対位置について求められる前記差分より小さくするように、前記差分の和を最小とする軌跡を求め、前記軌跡上の特定点における前記相対位置を推定するようにすることができる。これによって、他の装置との相対位置をより精度良く推定することができる。
【００２２】
また、上記の算出手段は、前記情報発信源の各々の位置に関する情報、及び前記情報発信源の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記情報発信源の各々の方向を算出し、時系列の前記情報発信源の各々の位置に関する情報に基づいて、前記情報発信源の各々の速度を算出し、前記移動体から見た前記情報発信源の各々の方向、前記情報発信源の各々の速度、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体の情報発信源の各々の方向の速度を算出し、複数の前記移動体の情報発信源の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出することができる。
【００２３】
また、上記の情報発信源を、前記情報発信源の各々の位置に関する情報として衛星軌道情報を、前記情報発信源の各々と前記移動体との距離に関する情報として擬似距離情報を、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報としてドップラー周波数情報を発信するＧＰＳ衛星とすることができる。情報発信源としては、例えば、擬似衛星やビーコンなどのように、発信源情報を発信するものであればよいが、代表的には、ＧＰＳ衛星とすることができる。
【００２４】
なお、本発明の相対位置推定プログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２５】
以上説明したように、本発明の相対位置推定装置及びプログラムによれば、他の装置の位置及び速度情報を受信すると共に、移動体の位置の軌跡と、他の装置の位置の軌跡と、移動体と他の装置との相対位置を算出して、移動体の位置の軌跡と、他の装置の位置の軌跡と、各時刻又は各地点の相対位置とに基づいて、軌跡上の特定点における相対位置を推定することにより、少ないデータ通信量で、他の装置との相対位置を精度良く推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２６】
【図１】本発明の第１の実施の形態に係る相対位置推定装置及び車載器を示すブロック図である。
【図２】本発明の第１の実施の形態に係る相対位置推定装置の位置・速度ベクトル算出部を示すブロック図である。
【図３】三角測量の原理に従って自車両の位置が推定される様子を示すイメージ図である。
【図４】各ＧＰＳ衛星の速度ベクトル及び各ＧＰＳ衛星の方向に基づいて、ＧＰＳ衛星方向の自車両の速度を算出する様子を示すイメージ図である。
【図５】自車両と相手車両とにおけるＧＰＳ誤差を説明するための図である。
【図６】相手車両の軌跡をフィッティングする様子を示すイメージ図である。
【図７】残差が大きい相対位置を棄却してから、相手車両の軌跡を再度フィッティングする様子を示すイメージ図である。
【図８】軌跡に誤差があり、残差のばらつきが大きくなっている様子を示すイメージ図である。
【図９】マルチパスが全域で発生して、残差のばらつきが大きくなっている様子を示すイメージ図である。
【図１０】第１の実施の形態に係る相対位置推定装置のコンピュータにおける位置速度算出処理ルーチンの内容を示すフローチャートである。
【図１１】第１の実施の形態に係る相対位置推定装置のコンピュータにおけるＧＰＳ情報に基づく自車両方位角推定処理ルーチンの内容を示すフローチャートである。
【図１２】第１の実施の形態に係る相対位置推定装置のコンピュータにおける速度ベクトル算出処理ルーチンの内容を示すフローチャートである。
【図１３】第１の実施の形態に係る相対位置推定装置のコンピュータにおける相対位置推定処理ルーチンの内容を示すフローチャートである。
【図１４】本発明の第２の実施の形態に係る相対位置推定装置の位置・速度ベクトル算出部を示すブロック図である。
【図１５】（Ａ）使用するＧＰＳ衛星を共通化できない場合を説明するための図、及び（Ａ）使用するＧＰＳ衛星を共通化できる場合を説明するための図である。
【図１６】第２の実施の形態に係る相対位置推定装置のコンピュータにおける位置速度算出処理ルーチンの内容を示すフローチャートである。
【図１７】本発明の第３の実施の形態に係る路側機を示すブロック図である。
【図１８】路側機の軌跡をフィッティングする様子を示すイメージ図である。
【発明を実施するための形態】
【００２７】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、第１の実施の形態では、車両に搭載され、ＧＰＳ衛星から発信されたＧＰＳ情報を取得すると共に、他の車両と車車間通信を行い、自車両に対する他の車両の相対位置を推定する相対位置推定装置に、本発明を適用した場合を例に説明する。
【００２８】
図１に示すように、第１の実施の形態に係る相対位置推定装置１０は、ＧＰＳ衛星からの電波を受信するＧＰＳ受信部１２と、自車両のヨーレートを検出するジャイロセンサ１４と、自車両の速度を検出する速度センサ１６と、他の車両と車車間通信を行うための無線通信部１８と、ＧＰＳ受信部１２によって受信されたＧＰＳ衛星からの受信信号、無線通信部１８によって受信したデータ、並びにジャイロセンサ１４及び速度センサ１６の検出値に基づいて、自車両に対する当該他の車両の相対位置を推定する処理を実行するコンピュータ２０と、を備えている。なお、ジャイロセンサ１４及び速度センサ１６が、検出手段の一例であり、無線通信部１８が、受信手段の一例である。
【００２９】
ＧＰＳ受信部１２は、複数のＧＰＳ衛星からの電波を受信して、受信した全てのＧＰＳ衛星からの受信信号から、ＧＰＳ衛星の情報として、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報（エフェメリス）、ＧＰＳ衛星が電波を送信した時刻、受信信号の強度、周波数などを取得し、コンピュータ２０に出力する。
【００３０】
無線通信部１８は、他の車両に搭載されている車載器８０から送信されたデータを受信する。なお、以下では、当該他の車両を、相手車両と称する。
【００３１】
ここで、車載器８０は、相対位置推定装置１０と同様に、ＧＰＳ衛星からの電波を受信するＧＰＳ受信部８２と、相手車両のヨーレートを検出するジャイロセンサ８４と、相手車両の速度を検出する速度センサ８６と、車車間通信を行うための無線通信部８８と、相手車両の位置及び速度ベクトルを算出する処理を実行するコンピュータ９０と、を備えている。コンピュータ９０は、ＧＰＳ受信部８２によって受信されたＧＰＳ衛星からの受信信号、無線通信部８８によって受信したデータ、並びにジャイロセンサ８４及び速度センサ８６の検出値に基づいて、相手車両の位置及び速度ベクトルを算出する位置・速度ベクトル算出部９２を備えている。位置・速度ベクトル算出部９２は、後述する位置・速度ベクトル算出部２４と同様の構成であるため、説明を省略する。なお、ジャイロセンサ８４及び速度センサ８６が、第２検出手段の一例であり、無線通信部８８が、送信手段の一例であり、位置・速度ベクトル算出部９２が、第２取得手段及び第２算出手段の一例である。
【００３２】
車載器８０は、所定時間間隔で、相手車両の位置及び速度ベクトルを算出して、各時刻について、位置及び速度ベクトルと、位置を算出する処理で使用したＧＰＳ衛星の衛星番号とを含む計測データを、無線通信部８８によって送信する。なお、計測データには、更に、算出された位置がいつの位置であるかを示す時刻、すなわち、車載器８０でＧＰＳ衛星からの電波を受信した時刻が含まれている。無線通信部８８によって送信されるデータを表１に示す。
【００３３】
【表１】
【００３４】
コンピュータ２０は、ＣＰＵ、後述する位置速度算出処理ルーチン及び相対位置推定処理ルーチン及びを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。
【００３５】
コンピュータ２０を以下で説明する位置速度算出処理ルーチン及び相対位置推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、無線通信部１８によって受信した相手車両からの計測データを記憶する受信データ記憶部２２と、ＧＰＳ受信部１２によって受信されたＧＰＳ衛星からの受信信号、並びにジャイロセンサ１４及び速度センサ１６の検出値に基づいて、各時刻における自車両の位置及び速度ベクトルを算出する位置・速度ベクトル算出部２４と、各時刻について、算出された自車両の位置及び受信した計測データに含まれる相手車両の位置の差分を算出して自車両を基準とした相手車両の相対位置を算出する位置差分相対位置算出部２６と、各時刻で取得したＧＰＳ衛星の情報に含まれる衛星番号及び受信した計測データに含まれるＧＰＳ衛星の衛星番号に基づいて、各時刻に対する重みを算出する重み算出部２８と、各時刻について算出された、自車両の位置、速度ベクトル、相対位置、及び重みを記憶するデータ記憶部３０とを備えている。なお、位置・速度ベクトル算出部２４は、算出手段の一例であり、位置差分相対位置算出部２６が、相対位置算出手段の一例である。
【００３６】
受信データ記憶部２２は、無線通信部１８によって相手車両からの計測データを受信するたびに、計測データを記憶し、蓄積する。
【００３７】
位置・速度ベクトル算出部２４は、図２に示すように、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ擬似距離データ、ドップラー周波数、及びＧＰＳ衛星の位置座標を算出して取得するＧＰＳ情報取得部５０と、取得したＧＰＳ情報に基づいて、自車両の位置を算出する位置算出部５１と、取得したＧＰＳ情報に基づいて、自車両の方位角を推定する方位角推定部５２と、推定された方位角、ジャイロセンサ１４の検出値、及び速度センサ１６の検出値を対応させて記憶するデータ記憶部５４と、データ記憶部５４に記憶された各種データに基づいて、自車両の速度ベクトルを推定する速度ベクトル推定部５６と、を含んだ構成で表すことができる。なお、ＧＰＳ情報取得部５０が、取得手段の一例である。
【００３８】
ＧＰＳ情報取得部５０は、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ衛星が電波を送信した時刻及び自車両で電波を受信した時刻に基づいて、ＧＰＳ擬似距離データを算出する。また、ＧＰＳ情報取得部５０は、各ＧＰＳ衛星から送信される信号の既知の周波数と、各ＧＰＳ衛星から受信した受信信号の周波数とに基づいて、各ＧＰＳ衛星からの受信信号のドップラー周波数を各々算出する。なお、ドップラー周波数は、ＧＰＳ衛星と自車との相対速度による、搬送波周波数のドップラーシフト量を観測したものである。また、ＧＰＳ情報取得部５０は、ＧＰＳ衛星の軌道情報及びＧＰＳ衛星が電波を送信した時刻に基づいて、ＧＰＳ衛星の位置座標を各々算出する。また、ＧＰＳ情報取得部５０は、取得したＧＰＳ衛星の情報に含まれる各ＧＰＳ衛星の衛星番号を、重み算出部２８へ出力する。
【００３９】
位置算出部５１は、以下のように、ＧＰＳ情報取得部５０によって取得された各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。
【００４０】
ＧＰＳを用いた測位では、図３に示すように、既知であるＧＰＳ衛星の位置座標と、各ＧＰＳ衛星から受信した受信信号の伝播距離である擬似距離とに基づいて、三角測量の原理に従って、自車両の位置が推定される。
【００４１】
ここで、ＧＰＳ衛星までの真の距離ｒｊは、以下の（１）式で表され、ＧＰＳで観測される擬似距離ρｊは、以下の（２）式で表される。
【００４２】
【数１】
【００４３】
ただし、（Ｘｊ，Ｙｊ，Ｚｊ）がＧＰＳ衛星ｊの位置座標であり、（ｘ，ｙ，ｚ）が自車両の位置座標である。ｓは、ＧＰＳ受信部１２の時計誤差による距離誤差である。
【００４４】
上記（１）式、（２）式より、４つ以上のＧＰＳ衛星のＧＰＳ擬似距離データから得られる以下の（３）式の連立方程式を解くことによって、自車両の位置（ｘ、ｙ、ｚ）が算出される。
【００４５】
【数２】
【００４６】
また、方位角推定部５２は、さらに、取得した各ＧＰＳ衛星のドップラー周波数に基づいて、各ＧＰＳ衛星に対する自車両の相対速度を算出する相対速度算出部６０と、取得した各ＧＰＳ衛星の位置座標の時系列データに基づいて、各ＧＰＳ衛星の速度ベクトルを算出する衛星速度算出部６２と、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星の方向（角度の関係）を算出する衛星方向算出部６６と、算出された相対速度、各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の方向に基づいて、各ＧＰＳ衛星方向の自車両の速度を算出する衛星方向自車速算出部６８と、算出された複数の各ＧＰＳ衛星方向の自車両の速度に基づいて、自車両の速度ベクトルを算出する自車両速度算出部７０と、算出された自車両の速度ベクトルに基づいて自車両の方位角を算出する自車両方位角算出部７２と、を備えた構成で表すことができる。
【００４７】
相対速度算出部６０は、ドップラー周波数とＧＰＳ衛星に対する相対速度との関係を表わす以下の（４）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度を算出する。
【００４８】
【数３】
【００４９】
ただし、ｖｊはＧＰＳ衛星ｊに対する相対速度であり、Ｄ１ｊはＧＰＳ衛星ｊから得られるドップラー周波数（ドップラーシフト量）である。また、Ｃは光速であり、Ｆ１は、ＧＰＳ衛星から送信される信号の既知のＬ１周波数である。
【００５０】
衛星速度算出部６２は、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（３次元速度ＶＸｊ、ＶＹｊ、ＶＺｊ）を算出する。例えば、非特許文献（Ｐｒａｔａｐ Ｍｉｓｒａ ａｎｄ Ｐｅｒ Ｅｎｇｅ原著 日本航海学会ＧＰＳ研究会訳：“精説ＧＰＳ基本概念・測位原理・信号と受信機”正陽文庫，２００４．）に記載された方法を用いて、各ＧＰＳ衛星の速度ベクトルを算出することができる。
【００５１】
衛星方向算出部６６は、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係（水平方向に対する仰角θｊ、北方向に対する方位角φｊ）を、各ＧＰＳ衛星の方向として算出する。
【００５２】
衛星方向自車速算出部６８は、図４に示すように、算出された各ＧＰＳ衛星に対する自車両の相対速度ｖｊ、各ＧＰＳ衛星の速度ベクトル（ＶＸｊ、ＶＹｊ、ＶＺｊ）、及び各ＧＰＳ衛星の方向Ｒｊ（θｊ、φｊ）に基づいて、以下の（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖｊを算出する。
【００５３】
【数４】
【００５４】
ｖｊは、ＧＰＳ衛星ｊに対する自車両の相対速度（衛星方向におけるＧＰＳ衛星との相対速度）である。また、Ｖｓｊは、自車方向のＧＰＳ衛星ｊの速度であり、Ｖｓｊ＝Ｒｊ［ＶＸｊ，ＶＹｊ，ＶＺｊ］Ｔにより求まる。また、Ｖｖｊは、ＧＰＳ衛星ｊの方向の自車速であり、ｖＣｂは、クロックバイアス変動である。
【００５５】
上述したように、ＧＰＳ衛星方向の自車速は、ＧＰＳ衛星位置の三次元位置ではなく、ＧＰＳ衛星との方位関係によってのみ算出される。ＧＰＳ衛星は遥か遠方にあり、１日で地球をほぼ２周するため１分間の角度変化は０．５度である。通常、ＧＰＳ衛星とＧＰＳ受信機との時計誤差は通常１ｍｓｅｃ以下であるため、ＧＰＳ衛星との方位関係に大きな影響はない。また、同じくＧＰＳ衛星は遥か遠方にあるため、自車の位置決定に数１００ｍ程度の誤差が生じていたとしても、ＧＰＳ衛星との方位関係に大きな影響はない。このため、擬似距離に誤差が乗りやすい状況であったとしても、ＧＰＳ衛星方向の自車速は、擬似距離に比較して正確に算出され得る。
【００５６】
自車両速度算出部７０は、以下に説明するように、自車両の速度ベクトルの最適推定を行う。
【００５７】
まず、自車の速度ベクトルを（Ｖｘ，Ｖｙ，Ｖｚ）としたとき、ＧＰＳ衛星方向の自車両の速度Ｖｖｊとの関係は以下の（６）式で表される。
【００５８】
【数５】
【００５９】
各ＧＰＳ衛星ｊについて得られる上記（６）式より、Ｖｘ，Ｖｙ，Ｖｚ及びｖＣｂを推定値とした、以下の（７）式で表される連立方程式が得られる。
【００６０】
【数６】
【００６１】
電波を受信したＧＰＳ衛星が４個以上である場合に、上記（７）式の連立方程式を解くことによって、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。
【００６２】
自車両方位角算出部７２は、自車両速度算出部７０により算出された自車両の速度ベクトルから三角関数を用いて自車両の方位角を算出し、方位角推定部５２での方位角の推定結果としてデータ記憶部５４に記憶する。ここで推定された方位角は、ＧＰＳ情報に基づいて推定された方位角であるので、以下、「推定方位角」という。これに対し、ジャイロセンサ１４の検出値から求まる自車両の方位角を「観測方位角」という。ただし、ジャイロセンサ１４では、方位角の変化分しか計測できないため、ある初期値を基準に時間経過分を積分することで、ジャイロセンサ１４の検出値から観測方位角を算出する。例えば、下記（８）式では、ある時間ｔ０の初期値Ψｉｎｉｔ（ｔ０）を基準にｋ秒前の観測方位角Ψｍ（ｔ０−ｋ）を算出している。つまり、データ記憶部５４にジャイロセンサ１４の検出値Ψｇｙｒｏの微分値を記憶しておくことで、観測方位角が算出可能になる。
【００６３】
【数７】
【００６４】
なお、データ記憶部５４には、所定時間分の推定方位角と共に、対応する所定時間内に検出されたジャイロセンサ１４の検出値、及び速度センサ１６の検出値が記憶される。
【００６５】
また、速度ベクトル推定部５６は、さらに、推定方位角と観測方位角とを統合して方位角の最適値を推定する最適推定部７４と、推定された最適値と推定方位角との残差の分散に基づいて、推定方位角の精度を判定する精度判定部７６と、精度判定部７６での判定結果に基づいて自車両の方位角を推定し、推定した方位角を用いて速度ベクトルを算出する速度ベクトル算出部７８と、を含んだ構成で表すことができる。
【００６６】
最適推定部７４は、データ記憶部５４から所定時間分のジャイロセンサ１４の検出値である自車両のヨーレートを読み出し、観測毎のヨーレートを各々積分して時系列に積算して自車両の観測方位角を算出する。また、対応する所定期間内に推定された推定方位角をデータ記憶部５４から読み出す。
【００６７】
また、最適推定部７４は、最小二乗法等に代表される最適計算により、推定方位角と観測方位角とを統合した方位角の最適値を推定する。例えば、最小二乗法では、下記（９）式により推定方位角Ψｇｐｓと、観測方位角Ψｍとの一定時間における差の和が最小になるように、観測方位角Ψｍの算出の基準となる最適な初期値Ψｉｎｉｔ（ｔ０）を探索することで、方位角の最適値を推定する。
【００６８】
【数８】
【００６９】
精度判定部７６は、最適推定部７４で推定された方位角の最適値と推定方位角の各々との残差を算出し、所定時間分の残差のばらつき（分散または標準偏差）が所定の閾値以下か否かにより、方位角の最適値の精度を判定する。
【００７０】
速度ベクトル算出部７８は、最適推定部７４により推定された所定時間分の方位角の最適値が精度判定部７６により精度が高いと判定された場合には、その方位角の最適値及び速度センサ１６で検出された速度を用いて、速度ベクトルを算出する。一方、精度が低いと判定された最適値については、速度ベクトルの算出に用いない。その間の速度ベクトルは、過去に推定された方位角を用いてジャイロセンサ１４の検出値を積算して方位角を算出して、速度センサ１６で検出された速度を用いて算出する。速度ベクトル算出部７８は、算出した速度ベクトルを速度ベクトルの推定結果として出力する。
【００７１】
また、位置差分相対位置算出部２６は、自車両を、相対位置を算出する基準として、各時刻について、算出された自車両の位置及び受信した計測データに含まれる相手車両の位置の差分により、相対位置を算出する。このように、位置差分相対位置算出部２６は、単純に二台の車両の位置の差分を計算することで、相対位置の算出を行う。このとき、１００ｍ程度以下の距離にある二台の車両の相対位置を算出する場合においては、図５に示すように、ＧＰＳの成層圏、対流圏に起因する誤差が共通化されるため、各車両の絶対位置よりも相対位置が正確になる。そのため、単純に位置の差分を計算することにより、マルチパスが発生していない箇所では、ばらつきがあるが、正確な相対位置が算出することができる。
【００７２】
また、重み算出部２８は、各時刻について、受信した計測データに含まれるＧＰＳ衛星の衛星番号のうち、自車両で取得したＧＰＳ衛星の情報に含まれる各ＧＰＳ衛星の衛星番号と共通になっている衛星番号の割合が大きいほど、後述する重み付き最小二乗法で用いられる重みを大きく算出する。すなわち、自車両で取得したＧＰＳ衛星の情報に含まれる衛星番号が、受信した計測データに含まれるＧＰＳ衛星の衛星番号の全てを含む場合に、最大の重みを算出する。
【００７３】
また、コンピュータ２０は、更に、所定時間分の自車両の速度ベクトルを積算して、所定時間における自車両の位置の軌跡を算出する軌跡算出部３２と、受信した測定データから得られる所定時間分の相手車両の速度ベクトルを積算し、当該所定時間における相手車両の位置の軌跡を算出する相手軌跡算出部３４と、自車両及び相手車両の軌跡と位置の差分により算出された相対位置とに基づいて、自車両に対する相手車両の相対位置を推定する軌跡利用相対位置推定部３６と、推定された相対位置の誤差を算出する相対位置誤差算出部３８とを備えている。なお、軌跡算出部３２が、第１軌跡算出手段の一例であり、相手軌跡算出部３４が、第２軌跡算出手段の一例である。
【００７４】
軌跡算出部３２は、算出された所定時間分の自車両の速度ベクトルをデータ記憶部３０から取得し、取得した所定時間分の自車両の速度ベクトルを積算して、当該所定時間における自車両の位置の軌跡を算出する。また、相手軌跡算出部３４は、受信データ記憶部２２に記憶された測定データから、同じ所定時間分の相手車両の速度ベクトルを取得し、取得した相手車両の速度ベクトルを積算して、当該所定時間における相手車両の位置の軌跡を算出する。
【００７５】
軌跡利用相対位置推定部３６は、以下に説明するように、自車両に対する相手車両の相対位置を推定する。
【００７６】
まず、自車両及び相手車両の軌跡と、軌跡上の各時刻の位置の差分による相対位置とが既知である。ここで、軌跡は正解であるが、位置の差分による相対位置には、ノイズによるばらつきと、マルチパスによる大きな誤差が含まれている可能性がある。
【００７７】
そこで、軌跡利用相対位置推定部３６は、最小二乗法等に代表される最適計算により、軌跡と位置の差分による相対位置とに対して、軌跡のフィッティングを行って、軌跡の最適値を推定する。図６に示すように、まず、自車両の軌跡を基準にして、各時刻の位置差分による相対位置から、各時刻の相手車両の予測位置を算出する。そして、相手車両の予測位置に対して、最小二乗法により相手車両の軌跡のフィッティングを行って、相手車両の軌跡の最適値を求め、求められた相手車両の軌跡の最適値を用いて、軌跡上の特定点（例えば、終端点）における自車両に対する相手車両の相対位置を算出する。
【００７８】
ここで、特に二台の車両で共通した衛星を使用して測位を行っていた場合には、よりＧＰＳの誤差が共通化されるため、誤差が小さくなる特徴がある。この特徴を利用し、最小二乗の計算の際に、重み算出部２８によって、共通化されている衛星の割合に応じて算出された重みを、データ記憶部３０から取得し、取得した重みを適用して、以下の（１０）式に従って、重み付き最小二乗法による相手車両の軌跡のフィッティングを行う。
【００７９】
【数９】
【００８０】
ただし、Δｙは、観測量（相手車両の軌跡上の位置）と予測値（予測された相手車両の位置）との差（残差)であり、Δｘは未知数であり、Ｇは、一般行列である。また、Ｗが、各時刻の重みを示す重み行列である。
【００８１】
上記（１０）式では、観測量と予測値との差（残差)Δｙが予め設定した重み行列Ｗに応じて最小化されるように、未知数Δｘの算出を行う。重み行列Ｗは、予め既知である誤差の共分散を基に設定することで、誤差の大きさを考慮した推定が可能なことが知られており、共分散の逆数を重み行列Ｇに設定する手法が用いられることが多い。二台の車両で共通した衛星を使用して測位した場合では、相対位置の誤差が小さくなることは知られているため、位置の算出に使用したＧＰＳ衛星の衛星番号に基づいて重み行列Ｗを設定することで、より正確な相対位置を推定することができる。なお、重み行列Ｗは一致している衛星の数の割合で算出する必要はなく、完全に一致しているか、否か、の二種類で設定してもよい。例えば、衛星番号が完全に一致している時刻については重み「１」を設定し完全に一致していない時刻については重み「０．５」を設定するようにしてもよい。
【００８２】
また、軌跡利用相対位置推定部３６は、相手車両の軌跡の最適値と、軌跡上の各時刻の相手車両の位置の予測値との残差（最小二乗の際の残差）について、閾値判定を行い、図７に示すように、残差が閾値以上となる時刻についての位置差分による相対位置を棄却してから、再度、軌跡と位置の差分による相対位置とに対してフィッティングを行って、軌跡の最適値を推定することで、マルチパスの影響を除去する。
【００８３】
相対位置誤差算出部３８は、軌跡利用相対位置推定部３６で求められた相手車両の軌跡の最適値と、軌跡上の各時刻の相手車両の位置の予測値との残差の標準偏差を算出し、相対位置誤差の指標として出力する。
軌跡を利用して相対位置を算出する手法で誤差が発生する場合は、図８に示すような「軌跡に誤差がある場合」と図９に示すような「全域でマルチパスが発生している場合」とである。共に、最小二乗の際の残差のばらつきが大きくなるため、残差の標準偏差を、相対位置誤差の指標とすることができる。
【００８４】
なお、軌跡の最適値を利用して算出した相対位置と、単純に位置の差分により算出した相対位置とを利用して、算出した相対位置の誤差を算出してもよい。
【００８５】
次に、第１の実施の形態に係る相対位置推定装置１０の作用について説明する。
【００８６】
まず、無線通信部１８によって、相手車両の車載器８０から送信された、位置、速度ベクトル、及び衛星番号を含む測定データを受信する毎に、相対位置推定装置１０のコンピュータ２０は、受信した測定データを受信データ記憶部２２に記憶する。
【００８７】
また、ＧＰＳ受信部１２によって、複数のＧＰＳ衛星から電波を繰り返し受信しているときに、コンピュータ２０において、図１０に示す位置速度算出処理ルーチンが繰り返し実行される。
【００８８】
ステップ１００で、ＧＰＳ受信部１２から複数のＧＰＳ衛星の情報を取得すると共に、複数のＧＰＳ衛星のＧＰＳ擬似距離データ、ドップラー周波数、及びＧＰＳ衛星の位置座標を算出して取得する。
【００８９】
そして、ステップ１０２において、各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、上記（１）〜（３）式に従って、自車両の位置を算出する。
【００９０】
次に、ステップ１０４で、ジャイロセンサ１４の検出値である自車両のヨーレート、及び速度センサ１６の検出値である自車両の速度を取得して、データ記憶部５４に記憶する。
【００９１】
次に、ステップ１０６で、後述するＧＰＳ情報に基づく自車両方位角処理を実行して、推定方位角を推定し、次に、ステップ１０８で、後述する速度ベクトル算出処理を実行して、速度ベクトルを推定する。
【００９２】
そして、ステップ１１０において、上記ステップ１００で取得したＧＰＳ衛星の情報と同一時刻（電波の受信時刻が同一）の測定データを、受信データ記憶部２２から取得する。ステップ１１２では、上記ステップ１００で取得したＧＰＳ衛星の情報から得られる衛星番号と、上記ステップ１１０で取得した測定データから得られる衛星番号とに基づいて、重みを算出する。
【００９３】
そして、ステップ１１４において、上記ステップ１０２で算出した自車両の位置と、上記ステップ１１０で取得した測定データから得られる相手車両の位置との差分から、自車両に対する相手車両の相対位置を算出する。次のステップ１１６では、上記ステップ１０２、１０６、１１２、１１４による算出結果を、データ記憶部３０に記憶し、位置速度算出処理ルーチンを終了する。
【００９４】
上記位置速度算出処理ルーチンが、繰り返し実行されることにより、各時刻について算出された自車両の位置、速度ベクトル、重み、及び相対位置が、データ記憶部３０に記憶される。
【００９５】
次に、図１１を参照して、ＧＰＳ情報に基づく自車両方位角推定処理ルーチンについて説明する。
【００９６】
ステップ１０６０で、上記（４）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度ｖｊを算出する。
【００９７】
次に、ステップ１０６２で、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（ＶＸｊ、ＶＹｊ、ＶＺｊ）を算出する。
【００９８】
次に、ステップ１０６６で、上記ステップ１０２で算出された自車両の位置、及び上記ステップ１００で取得された各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係Ｒｊ（水平方向に対する仰角θｊ、北方向に対する方位角φｊ）を、各ＧＰＳ衛星の方向として算出する。
【００９９】
次に、ステップ１０６８で、上記ステップ１０６０で算出された各ＧＰＳ衛星に対する自車両の相対速度ｖｊ、上記ステップ１０６２で算出された各ＧＰＳ衛星の速度ベクトルＶｊ（ＶＸｊ、ＶＹｊ、ＶＺｊ）、及び上記ステップ１０６６で算出された各ＧＰＳ衛星の方向Ｒｊ（θｊ、φｊ）に基づいて、上記（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖｊを算出する。（５）式における自車方向のＧＰＳ衛星ｊの速度Ｖｓｊは、Ｖｓｊ＝Ｒｊ［ＶＸｊ，ＶＹｊ，ＶＺｊ］Ｔにより算出する。
【０１００】
次に、ステップ１０７０で、上記（６）式、及び（７）式に従って、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。
【０１０１】
次に、ステップ１０７２で、上記ステップ１０７０で算出された自車両の速度ベクトルから三角関数を用いて自車両の方位角を算出し、算出した方位角を推定方位角としてデータ記憶部５４に記憶して、リターンする。
【０１０２】
次に、図１２を参照して、速度ベクトル推定処理ルーチンについて説明する。
【０１０３】
ステップ１０８０で、データ記憶部５４から所定期間分のジャイロセンサ１４の検出値である自車両のヨーレートを読み出し、観測毎のヨーレートを各々時系列に積算して自車両の観測方位角を算出する。
【０１０４】
次に、ステップ１０８２で、所定期間内に推定された推定方位角をデータ記憶部５４から読み出し、上記ステップ１０８０で算出した観測方位角の時系列データを平行移動して探索しながら、最小二乗法等に代表される最適計算により、方位角の最適値を推定する。
【０１０５】
次に、ステップ１０８４で、上記ステップ１０８２で推定された方位角の最適値と推定方位角の各々との残差を算出し、所定期間分の残差の分散を算出する。
【０１０６】
次に、ステップ１０８６で、上記ステップ１０８４で算出した残差の分散が予め定めた閾値以下か否可を判定することにより、方位角の最適値の精度を判定する。残差の分散が閾値以下の場合には、方位角の最適値の精度が高いと判断してステップ１０８８へ移行し、上記ステップ１０８２で推定された方位角の最適値を、方位角の推定値として採用する。一方、残差の分散が閾値を超える場合には、方位角の最適値の精度が低いと判断して、ステップ１０９０へ移行し、過去に推定された方位角を用いてジャイロセンサ１４の検出値を積算して算出した方位角を、方位角の推定値として採用する。
【０１０７】
次に、ステップ１０９２で、データ記憶部５４から速度センサ１６で検出された速度を読み出し、上記ステップ１０８８または１０９０で採用された方位角の推定値及び読み出した速度を用いて速度ベクトルを算出し、算出した速度ベクトルを速度ベクトルの推定結果としてデータ記憶部３０に記憶して、リターンする。
【０１０８】
また、データ記憶部３０に、所定時間分の自車両の位置、速度ベクトル、重み、及び相対位置が記憶されると、コンピュータ２０において、図１３に示す相対位置推定処理ルーチンが実行される。
【０１０９】
ステップ１２０において、データ記憶部３０から所定時間分の速度ベクトルを取得し、所定時間分の自車両の速度ベクトルを積算して、所定時間における自車両の軌跡を算出する。次のステップ１２２では、受信データ記憶部２２から対応する所定時間分の相手車両の速度ベクトルを取得し、所定時間分の相手車両の速度ベクトルを積算して、所定期間における相手車両の軌跡を算出する。
【０１１０】
そして、ステップ１２４において、データ記憶部３０から、対応する所定時間内の各時刻について算出された相対位置を取得する。ステップ１２６では、上記ステップ１２０で算出された自車両の軌跡と、上記ステップ１２４で取得した各時刻の相対位置とに基づいて、対応する所定時間内の各時刻における相手車両の予測位置を算出する。
【０１１１】
次のステップ１２８では、データ記憶部３０から、対応する所定時間内の各時刻について算出された重みを取得し、取得した重みを適用した最小二乗法により、上記ステップ１２６で算出された相手車両の予測位置に対して、上記ステップ１２２で算出された相手車両の軌跡をフィッティングし、最適値を推定する。
【０１１２】
そして、ステップ１３０において、上記ステップ１２８で推定された相手車両の軌跡の最適値と、各時刻の相手車両の予測位置とに基づいて、最小二乗法の残差を各時刻について算出し、ステップ１３２において、上記ステップ１３０で算出された残差が閾値以上となる地点（時刻）が存在するか否かを判定する。残差が閾値以上となる地点（時刻）が存在する場合には、ステップ１３４において、該当地点（時刻）の相対位置を棄却して、ステップ１３６へ移行する。
【０１１３】
ステップ１３６では、上記ステップ１２０で算出された自車両の軌跡と、上記ステップ１２４で取得した各時刻の相対位置のうち、上記ステップ１３４で棄却された相対位置以外の相対位置とに基づいて、対応する所定時間内の各時刻における相手車両の予測位置を算出する。
【０１１４】
次のステップ１３８では、上記ステップ１２８と同様に、重みを適用した最小二乗法により、上記ステップ１３６で算出された相手車両の予測位置に対して、上記ステップ１２２で算出された相手車両の軌跡をフィッティングし、最適値を推定する。
【０１１５】
そして、ステップ１４０において、上記ステップ１３８で推定された相手車両の軌跡の最適値と、各時刻の相手車両の予測位置とに基づいて、最小二乗法の残差を各時刻について算出し、ステップ１４２へ移行する。
【０１１６】
一方、上記ステップ１３２で、残差が閾値以上となる地点（時刻）が存在しないと判定された場合には、ステップ１４２へ移行する。
【０１１７】
ステップ１４２では、上記ステップ１２８又はステップ１３８で推定された相手車両の軌跡の最適値と、上記ステップ１２０で算出された自車両の軌跡とに基づいて、軌跡上の終端点における相対位置を算出し、出力する。
【０１１８】
次のステップ１４４では、上記ステップ１３０又はステップ１４０で算出された各時刻の残差に基づいて、上記ステップ１４２で算出された相対位置の誤差を算出して出力し、相対位置推定処理ルーチンを終了する。
【０１１９】
以上説明したように、第１の実施の形態に係る相対位置推定装置によれば、相手車両の位置及び速度ベクトルを受信すると共に、自車両の位置の軌跡と、相手車両の位置の軌跡と、自車両に対する相手車両の相対位置とを算出して、自車両の位置の軌跡と、相手車両の位置の軌跡と、各時刻の相対位置とに基づいて、相手車両の軌跡の最適値を推定し、軌跡上の終端点における相対位置を推定することにより、ＧＰＳの観測データを通信する特許文献１よりも、少ないデータ通信量で、相手車両との相対位置を精度良く推定することができる。ＧＰＳの観測データは、各衛星の擬似距離、ドップラー周波数、搬送波位相等から構成され、その通信容量は衛星に比例する。ＧＰＳで測位を実施するためには４つ以上の衛星が必要なため、少なくとも４×３（擬似距離、ドップラー周波数、搬送波位相）＝１２個のデータを通信する必要がある。一方、本発明では、通信データは上記表１に示すデータに限られ、位置（３次元）、速度ベクトル（３次元）、時刻、及び衛星番号でよい。衛星番号を、各衛星の使用状況を示すビットで表せば、ＧＰＳであれば最大３２個の衛星が想定されるため、３２ｂｉｔ＝４ｂｙｔｅの容量でよい。よって、従来手法と比較して、少なく、かつ一定の通信容量で相対位置の推定が可能である。
【０１２０】
また、ＧＰＳドップラーと慣性センサを組み合わせることで、精度が高い軌跡が算出可能である。マルチパスが発生している場所においても、正確な軌跡を算出することができる。加えて、１００ｍ程度の相対位置では、共に測位に用いている衛星が共通化されていることが多いため、マルチパスが無い場所では単純にＧＰＳ測位結果の差分を用いたとしても、ばらつきはあるが精度が高い相対位置を計測することができる。以上の技術的な背景により、位置の差分による相対位置と二台の車両の軌跡に基づいて、最小二乗法によるフィッティングにより、ばらつきが抑えられた相対位置を推定することができ、残差で閾値を設定することでマルチパスの棄却ができる。加えて、衛星が共通化されている箇所に対して、重み付き最小二乗法で用いる重みを大きくすることで、推定した相対位置の精度を高めることができる。
【０１２１】
また、二台の車両の軌跡を利用して相対位置を算出することで、残差を利用したマルチパスの影響の除去することができると共に、ＧＰＳが受信できない場所でも相対位置を計測することができる。
【０１２２】
また、ＧＰＳの観測データ（擬似距離・ドップラー・搬送波位相）等を無線により通信する必要がないため、通信データ量の少なくすることができる。
【０１２３】
次に、第２の実施の形態に係る相対位置推定装置について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【０１２４】
第２の実施の形態では、自車両の位置を算出する際に、相手車両の位置の算出において使用したＧＰＳ衛星と共通となるように、使用するＧＰＳ衛星を選択している点が、第１の実施の形態と異なっている。
【０１２５】
第２の実施の形態に係る相対位置推定装置では、図１４に示すように、位置・速度ベクトル算出部２２４を、ＧＰＳ情報取得部５０と、取得したＧＰＳ衛星の情報に含まれる衛星番号、及び受信した測定データから得られる衛星番号に基づいて、位置の算出で使用するＧＰＳ衛星を選択する衛星選択部２５０と、位置算出部５１と、方位角推定部５２と、データ記憶部５４と、速度ベクトル推定部５６と、を含んだ構成で表すことができる。
【０１２６】
衛星選択部２５０は、ＧＰＳ情報取得部５０によって取得したＧＰＳ衛星の情報から衛星番号を取得すると共に、受信データ記憶部２２に記憶されている同時刻の測定データから、相手車両の位置の計算で使用したＧＰＳ衛星の衛星番号を取得する。そして、衛星選択部２５０は、相手車両の位置の計算で使用したＧＰＳ衛星の衛星番号の全てが、自車両で取得したＧＰＳ衛星の情報に含まれる衛星番号に含まれている場合には、完全に衛星を共通化させるように、相手車両の位置の計算で使用したＧＰＳ衛星の全てを、位置の算出において使用するＧＰＳ衛星として選択する。一方、衛星選択部２５０は、相手車両の位置の計算で使用したＧＰＳ衛星の衛星番号の少なくとも１つが、自車両で取得したＧＰＳ衛星の情報に含まれる衛星番号に含まれていない場合には、ＧＰＳ衛星の選択を行わない。
【０１２７】
例えば、図１５（Ａ）に示すように、相手車両の位置の計算で使用したＧＰＳ衛星の衛星番号の少なくとも１つが、自車両で取得したＧＰＳ衛星の情報の衛星番号に含まれていない場合には、位置算出部５１において、自車両で電波を受信した全てのＧＰＳ衛星を使用して、自車両の位置を算出する。
【０１２８】
一方、図１５（Ｂ）に示すように、相手車両の位置の計算で使用したＧＰＳ衛星の衛星番号の全てが、自車両で取得したＧＰＳ衛星の情報の衛星番号に含まれている場合には、位置算出部５１において、相手車両の位置の計算で使用したＧＰＳ衛星と同じＧＰＳ衛星を用いて、自車両の位置を算出する。
【０１２９】
これによって、自車両と相手車両で共通したＧＰＳ衛星を使用して測位を行っていた場合を増やすことができる。
【０１３０】
位置算出部５１は、ＧＰＳ情報取得部５０によって取得された各ＧＰＳ衛星のＧＰＳ擬似距離データ、又は衛星選択部２５０によって選択された各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。
【０１３１】
次に、第２の実施の形態における位置速度算出処理ルーチンについて図１６を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【０１３２】
ステップ１００で、ＧＰＳ受信部１２から複数のＧＰＳ衛星の情報を取得すると共に、複数のＧＰＳ衛星のＧＰＳ擬似距離データ、ドップラー周波数、ＧＰＳ衛星の位置座標を算出して取得する。次のステップ１１０で、取得したＧＰＳ衛星の情報と同一時刻の測定データを、受信データ記憶部２２から取得する。
【０１３３】
そして、ステップ２７０において、上記ステップ１００で取得したＧＰＳ衛星の情報に含まれる衛星番号が、上記ステップ１１０で取得した測定データに含まれる衛星番号を全て含むか否かを判定する。自車両で取得したＧＰＳ衛星の情報に含まれる衛星番号が、測定データに含まれる衛星番号の少なくとも１つを含まない場合には、ステップ１０２へ移行する。一方、自車両で取得したＧＰＳ衛星の情報に含まれる衛星番号が、測定データに含まれる衛星番号を全て含む場合には、ステップ２７２において、自車両の位置の算出で使用するＧＰＳ衛星として、上記ステップ１１０で取得した測定データに含まれる全ての衛星番号のＧＰＳ衛星を選択する。
【０１３４】
そして、ステップ１０２において、上記ステップ２７２で選択された各ＧＰＳ衛星のＧＰＳ擬似距離データ、又はＧＰＳ情報を取得した全てのＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。
【０１３５】
次に、ステップ１０４で、自車両のヨーレート、及び自車両の速度を取得して、データ記憶部５４に記憶する。次に、ステップ１０６で、ＧＰＳ情報に基づく自車両方位角処理を実行して、推定方位角を推定し、次に、ステップ１０８で、速度ベクトル算出処理を実行して、速度ベクトルを推定する。
【０１３６】
そして、ステップ１１２では、上記で取得したＧＰＳ衛星の情報から得られる衛星番号と、上記で取得した測定データから得られる衛星番号とに基づいて、最小二乗法で用いられる重みを算出する。
【０１３７】
そして、ステップ１１４において、自車両に対する相手車両の相対位置を算出する。次のステップ１１６では、上記ステップ１０２、１０６、１１２、１１４における算出結果を、データ記憶部３０に記憶し、位置速度算出処理ルーチンを終了する。
【０１３８】
なお、第２の実施の形態に係る相対位置推定装置の他の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。
【０１３９】
以上説明したように、第２の実施の形態に係る相対位置推定装置によれば、自車両と相手車両で共通したＧＰＳ衛星を使用して測位を行っていた場合を増やすことができるため、より精度よく相対位置を推定することができる。
【０１４０】
なお、上記の第１の実施の形態及び第２の実施の形態では、自車両及び相手車両が移動中である場合について説明したが、これに限定されるものではなく、自車両及び相手車両の何れか一方が停車していてもよい。この場合には、停止している車両の速度ベクトルが０となるため、算出される軌跡が点となるが、上述した軌跡利用相対位置推定部３６と同様に、自車両に対する相手車両の相対位置を推定することができる。
【０１４１】
次に、第３の実施の形態に係る相対位置推定装置について説明する。なお、第３の実施の形態に係る相対位置推定装置の構成は、第１の実施の形態と同様であるため、同一符号を付して説明を省略する。
【０１４２】
第３の実施の形態では、路車間通信により、路側機と通信して、自車両に対する路側機の相対位置を推定している点が、第１の実施の形態と異なっている。
【０１４３】
第３の実施の形態では、相対位置推定装置１０が、図１７に示すような路側機３８０と路車間通信を行う。路側機３８０は、ＧＰＳ受信部８２と、無線通信部８８と、ＧＰＳ受信部８２によって受信されたＧＰＳ衛星からの受信信号に基づいて、路側機３８０の位置を算出する処理を実行するコンピュータ３９０と、を備えている。
【０１４４】
コンピュータ３９０は、位置算出部３９２を備えており、位置算出部３９２は、ＧＰＳ受信部８２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ衛星が電波を送信した時刻及び路側機３８０で電波を受信した時刻に基づいて、ＧＰＳ擬似距離データを算出する。位置算出部３９２は、取得した各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、路側機３８０の位置を算出して、無線通信部８８へ出力すると共に、車速を０とした速度ベクトルを無線通信部８８へ出力する。
【０１４５】
無線通信部８８は、所定時間間隔で、各時刻について、位置及び速度ベクトルと、位置を算出する処理で使用したＧＰＳ衛星の衛星番号とを含む計測データを送信する。なお、計測データには、更に、算出された位置がいつの位置であるかを示す時刻、すなわち、路側機３８０でＧＰＳ衛星から電波を受信した時刻が含まれている。
【０１４６】
なお、第３の実施の形態に係る相対位置推定装置１０の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。
【０１４７】
第３の実施の形態に係る相対位置推定装置１０が、軌跡を利用した相対位置を推定する場合には、図１８に示すように、通信相手である路側機３８０の軌跡が点になる。
【０１４８】
そして、自車両の軌跡を基準にして、各時刻の位置差分による相対位置から、各時刻の路側機３８０の予測位置を算出する。そして、路側機３８０の予測位置に対して、最小二乗法により路側機３８０の軌跡（点）のフィッティングを行うことで、路側機３８０の軌跡（点）の最適値を求め、求められた路側機３８０の軌跡（点）の最適値を用いて、軌跡上の終端点における自車両に対する路側機３８０の相対位置を算出する。
【０１４９】
このように、自車両を基準とした相対位置を算出する対象が、静止物であっても、精度よく相対位置を推定することができる。
【０１５０】
なお、上記の第１の実施の形態〜第３の実施の形態では、ドップラー周波数を用いて、速度ベクトルを算出して、自車両の方位角を算出し、所定期間分の自車両の方位角の算出結果を用いて最適推定を行って、速度ベクトルを算出する場合を例に説明したが、これに限定されるものではない。ドップラー周波数を用いた他の手法により、速度ベクトルを算出してもよい。例えば、自車両速度算出部７０による算出結果を、そのまま自車両の速度ベクトルとして用いてもよい。
【０１５１】
また、上記実施の形態では、速度ベクトルを算出する場合を例に説明したが、これに限定されるものではなく、自車両の方位角を算出すると共に、自車両の速度を検出し、自車両の方位角と速度からなる速度情報を用いてもよい。この場合には、所定時間分の自車両の方位角と速度とから軌跡を算出するようにすればよい。
【０１５２】
また、上記実施の形態では、所定時間間隔で、相対位置及び速度ベクトルを算出して、各時刻の相対位置及び速度ベクトルを求める場合を例に説明したが、これに限定されるものではなく、所定の距離間隔で、相対位置及び速度ベクトルを算出して、各地点の相対位置及び速度ベクトルを求めるようにしてもよい。
【０１５３】
また、上記実施の形態では、自車両を基準とした相対位置を推定する場合を例に説明したが、これに限定されるものではなく、相手車両又は路側機を基準とした自車両の相対位置を推定してもよい。この場合には、相手車両又は路側機の軌跡を基準にして、各時刻の位置差分による相対位置から、各時刻の自車両の予測位置を算出する。そして、自車両の予測位置に対して、最小二乗法により自車両の軌跡のフィッティングを行うことで、自車両の軌跡の最適値を求め、求められた自車両の軌跡の最適値を用いて、相手車両又は路側機に対する自車両の相対位置を推定するようにすればよい。
【０１５４】
また、上記実施の形態では、重み付き最小二乗法を用いて、相手車両の軌跡の最適値を求める場合を例に説明したが、これに限定されるものではなく、重みなしの最小二乗法を用いて相手車両の軌跡の最適値を求めてもよい。この場合には、以下の（１１）式、（１２）式を用いて、相手車両の軌跡の最適値を求めればよい。
【０１５５】
【数１０】
ただし、Δｙは、観測量（相手車両の軌跡上の位置）と予測値（予測された相手車両の位置）との差（残差）であり、Δｘは未知数であり、Ｇは、一般行列である。上記（１１）式は観測量と予測値との差（残差）Δｙと未知数Δｘ、一般行列Ｇの関係を示している。上記（１２）式は、一般的な未知数Δｘを推定する方法である。
【０１５６】
また、ＧＰＳの計測値とジャイロ、速度センサの組み合わせによる複合航法結果を用いて、自車両の位置を算出するようにしてもよい。
【０１５７】
また、上記実施の形態では、ＧＰＳ情報に基づいて自車両の方位角を推定して精度の判定に用いる場合について説明したが、精度の判定に速度を用いてもよい。具体的には、自車両速度算出部７０で算出された速度ベクトルから自車両の速度の絶対値を算出し、最適推定部７４において、速度センサ１６で検出された速度と算出された速度の絶対値とを統合して、速度の最適値を推定し、この最適値と算出された速度の絶対値との残差の分散に基づいて、精度を判定することができる。そして、精度が高いと判定された所定期間の速度ベクトルから推定方位角を算出して、観測方位角と統合した方位角を推定するようにするとよい。
【０１５８】
また、上記実施の形態では、最適値と推定方位角との残差の分散に基づいて、精度を判定する場合について説明したが、これに限定されない。例えば、残差の分布がガウス分布に適合しているか否かにより精度を判定してもよい。
【０１５９】
また、上記実施の形態では、観測方位角を得るために、ジャイロセンサを用いる場合について説明したが、慣性航法系のセンサであればよく、磁気方位計でもよい。
【０１６０】
また、上記実施の形態では、ＧＰＳ衛星のＧＰＳ情報を用いる場合について説明したが、情報発信源の位置に関する情報、情報発信源と移動体との距離に関する情報、及び情報発信源と移動体との相対速度に関する情報を含む発信源情報を発信する情報発信源からの情報が取得できればよい。例えば、擬似衛星から発信される情報を受信するようにしてもよい。
【０１６１】
また、衛星測位システムは、一般的にＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｉｔｅ Ｓｙｓｔｅｍ）と呼ばれ、ＧＰＳだけでなく、ロシアのグロナス（ＧＬＯＮＡＳＳ）、欧州のガリレオ（Ｇａｌｉｌｅｏ）、日本の準天頂衛星システムも予定され、一部は既に適用されている。本発明はＧＰＳに限るものではなく、広くＧＮＳＳに適用可能な技術である。そのため、上記ＧＮＳＳから発信される情報を受信するようにしてもよい。
【０１６２】
また、上記実施の形態では、車両に搭載される相対位置推定装置について説明したが、本発明の相対位置推定装置が搭載される移動体は車両に限定されない。例えば、相対位置推定装置をロボットに搭載してもよいし、歩行者が携帯できるように相対位置推定装置をポータブル端末として構成するようにしてもよい。
【符号の説明】
【０１６３】
１０ 相対位置推定装置
１２、８２ ＧＰＳ受信部
１４、８４ ジャイロセンサ
１６、８６ 速度センサ
１８、８８ 無線通信部
２０、９０、３９０ コンピュータ
２４、５６、９２、２２４ 位置・速度ベクトル算出部
２６ 位置差分相対位置算出部
２８ 重み算出部
３０ データ記憶部
３２ 軌跡算出部
３４ 相手軌跡算出部
３６ 軌跡利用相対位置推定部
３８ 相対位置誤差算出部
５０ 情報取得部
５１ 位置算出部
５２ 方位角推定部
８０ 車載器
２５０ 衛星選択部
３８０ 路側機
３９２ 位置算出部
【特許請求の範囲】
【請求項１】
移動体と他の装置との相対位置を推定する、前記移動体に搭載される相対位置推定装置であって、
複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、
前記移動体の運動状態を検出する検出手段と、
前記取得手段により取得された前記発信源情報、及び前記検出手段によって検出された運動状態に基づいて、前記移動体の位置と、前記移動体の速度ベクトル、又は前記移動体の速度及び前記移動体の方位角を示す速度情報とを算出する算出手段と、
他の装置の位置及び前記速度情報を受信する受信手段と、
前記受信手段によって受信された前記他の装置の位置と、前記算出手段により算出された前記移動体の位置との差分から、各時刻又は各地点について、前記移動体と前記他の装置との相対位置を算出する相対位置算出手段と、
前記算出手段により算出された前記移動体の速度ベクトル又は前記速度情報を所定時間分積算して、前記所定時間における前記移動体の位置の軌跡を算出する第１軌跡算出手段と、
前記受信手段によって受信された前記速度情報を前記所定時間分積算して、前記所定時間における前記他の装置の位置の軌跡を算出する第２軌跡算出手段と、
前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置とに基づいて、前記軌跡上の特定点における前記相対位置を推定する相対位置推定手段と、
を含む相対位置推定装置。
【請求項２】
前記相対位置は、前記移動体の位置を基準とした前記他の装置の相対位置であり、
前記相対位置推定手段は、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡、及び前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置から求められる各時刻又は各地点の前記他の装置の位置との差分の和を最小とする、前記他の装置の位置の軌跡を求め、前記求められた前記他の装置の位置の軌跡と前記移動体の位置の軌跡とに基づいて、前記軌跡上の特定点における前記相対位置を推定する請求項１記載の相対位置推定装置。
【請求項３】
前記相対位置は、前記他の装置の位置を基準とした前記移動体の相対位置であり、
前記相対位置推定手段は、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡、及び前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置から求められる各時刻又は各地点の前記移動体の位置との差分の和を最小とする、前記移動体の位置の軌跡を求め、前記求められた前記移動体の位置の軌跡と前記他の装置の位置の軌跡とに基づいて、前記軌跡上の特定点における前記相対位置を推定する請求項１記載の相対位置推定装置。
【請求項４】
前記差分の和が最小とされたときの前記差分の各々に基づいて、前記相対位置推定手段によって推定された前記相対位置の誤差を算出する相対位置誤差算出手段を更に含む請求項２又は３記載の相対位置推定装置。
【請求項５】
前記他の装置は、複数の情報発信源の各々から発信された前記発信源情報を取得する第２取得手段と、前記他の装置の運動状態を検出する第２検出手段と、前記第２取得手段により取得された前記発信源情報、及び前記第２検出手段によって検出された運動状態に基づいて、前記他の装置の位置と、前記速度情報とを算出する第２算出手段と、前記第２算出手段によって算出された前記他の装置の位置、前記速度情報、及び前記第２取得手段によって取得した前記発信源情報を発信した前記情報発信源の識別情報を送信する送信手段を含み、
前記受信手段は、前記送信手段によって送信された前記他の装置の位置、前記速度情報、及び前記情報発信源の識別情報を受信し、
前記相対位置推定手段は、前記受信手段によって受信した前記識別情報が示す前記情報発信源を全て含む複数の情報発信源からの前記発信源情報に基づいて算出された前記移動体の位置に基づく前記相対位置について求められる前記差分を、他の相対位置について求められる前記差分より小さくするように、前記差分の和を最小とする軌跡を求め、前記軌跡上の特定点における前記相対位置を推定する請求項２〜請求項４の何れか１項記載の相対位置推定装置。
【請求項６】
前記算出手段は、前記情報発信源の各々の位置に関する情報、及び前記情報発信源の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記情報発信源の各々の方向を算出し、時系列の前記情報発信源の各々の位置に関する情報に基づいて、前記情報発信源の各々の速度を算出し、前記移動体から見た前記情報発信源の各々の方向、前記情報発信源の各々の速度、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体の情報発信源の各々の方向の速度を算出し、複数の前記移動体の情報発信源の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出する請求項１〜請求項５の何れか１項記載の相対位置推定装置。
【請求項７】
前記情報発信源を、前記情報発信源の各々の位置に関する情報として衛星軌道情報を、前記情報発信源の各々と前記移動体との距離に関する情報として擬似距離情報を、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報としてドップラー周波数情報を発信するＧＰＳ衛星とした請求項１〜請求項６の何れか１項記載の相対位置推定装置。
【請求項８】
コンピュータを、移動体と他の装置との相対位置を推定する、前記移動体に搭載される相対位置推定装置の各手段として機能させるための相対位置推定プログラムであって、
前記コンピュータを、
複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段、
前記取得手段により取得された前記発信源情報、及び前記移動体の運動状態を検出する検出手段によって検出された運動状態に基づいて、前記移動体の位置と、前記移動体の速度ベクトル、又は前記移動体の速度及び前記移動体の方位角を示す速度情報とを算出する算出手段、
他の装置の位置及び前記速度情報を受信する受信手段によって受信された前記他の装置の位置と、前記算出手段により算出された前記移動体の位置との差分から、各時刻又は各地点について、前記移動体と前記他の装置との相対位置を算出する相対位置算出手段、
前記算出手段により算出された前記移動体の速度ベクトル又は前記速度情報を所定時間分積算して、前記所定時間における前記移動体の位置の軌跡を算出する第１軌跡算出手段、
前記受信手段によって受信された前記速度情報を前記所定時間分積算して、前記所定時間における前記他の装置の位置の軌跡を算出する第２軌跡算出手段、及び
前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置とに基づいて、前記軌跡上の特定点における前記相対位置を推定する相対位置推定手段
として機能させるための相対位置推定プログラム。
【請求項９】
コンピュータを、請求項１〜請求項７の何れか１項記載の相対位置推定装置の各手段として機能させるための相対位置推定プログラム。
【請求項１】
移動体と他の装置との相対位置を推定する、前記移動体に搭載される相対位置推定装置であって、
複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、
前記移動体の運動状態を検出する検出手段と、
前記取得手段により取得された前記発信源情報、及び前記検出手段によって検出された運動状態に基づいて、前記移動体の位置と、前記移動体の速度ベクトル、又は前記移動体の速度及び前記移動体の方位角を示す速度情報とを算出する算出手段と、
他の装置の位置及び前記速度情報を受信する受信手段と、
前記受信手段によって受信された前記他の装置の位置と、前記算出手段により算出された前記移動体の位置との差分から、各時刻又は各地点について、前記移動体と前記他の装置との相対位置を算出する相対位置算出手段と、
前記算出手段により算出された前記移動体の速度ベクトル又は前記速度情報を所定時間分積算して、前記所定時間における前記移動体の位置の軌跡を算出する第１軌跡算出手段と、
前記受信手段によって受信された前記速度情報を前記所定時間分積算して、前記所定時間における前記他の装置の位置の軌跡を算出する第２軌跡算出手段と、
前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置とに基づいて、前記軌跡上の特定点における前記相対位置を推定する相対位置推定手段と、
を含む相対位置推定装置。
【請求項２】
前記相対位置は、前記移動体の位置を基準とした前記他の装置の相対位置であり、
前記相対位置推定手段は、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡、及び前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置から求められる各時刻又は各地点の前記他の装置の位置との差分の和を最小とする、前記他の装置の位置の軌跡を求め、前記求められた前記他の装置の位置の軌跡と前記移動体の位置の軌跡とに基づいて、前記軌跡上の特定点における前記相対位置を推定する請求項１記載の相対位置推定装置。
【請求項３】
前記相対位置は、前記他の装置の位置を基準とした前記移動体の相対位置であり、
前記相対位置推定手段は、前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡、及び前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置から求められる各時刻又は各地点の前記移動体の位置との差分の和を最小とする、前記移動体の位置の軌跡を求め、前記求められた前記移動体の位置の軌跡と前記他の装置の位置の軌跡とに基づいて、前記軌跡上の特定点における前記相対位置を推定する請求項１記載の相対位置推定装置。
【請求項４】
前記差分の和が最小とされたときの前記差分の各々に基づいて、前記相対位置推定手段によって推定された前記相対位置の誤差を算出する相対位置誤差算出手段を更に含む請求項２又は３記載の相対位置推定装置。
【請求項５】
前記他の装置は、複数の情報発信源の各々から発信された前記発信源情報を取得する第２取得手段と、前記他の装置の運動状態を検出する第２検出手段と、前記第２取得手段により取得された前記発信源情報、及び前記第２検出手段によって検出された運動状態に基づいて、前記他の装置の位置と、前記速度情報とを算出する第２算出手段と、前記第２算出手段によって算出された前記他の装置の位置、前記速度情報、及び前記第２取得手段によって取得した前記発信源情報を発信した前記情報発信源の識別情報を送信する送信手段を含み、
前記受信手段は、前記送信手段によって送信された前記他の装置の位置、前記速度情報、及び前記情報発信源の識別情報を受信し、
前記相対位置推定手段は、前記受信手段によって受信した前記識別情報が示す前記情報発信源を全て含む複数の情報発信源からの前記発信源情報に基づいて算出された前記移動体の位置に基づく前記相対位置について求められる前記差分を、他の相対位置について求められる前記差分より小さくするように、前記差分の和を最小とする軌跡を求め、前記軌跡上の特定点における前記相対位置を推定する請求項２〜請求項４の何れか１項記載の相対位置推定装置。
【請求項６】
前記算出手段は、前記情報発信源の各々の位置に関する情報、及び前記情報発信源の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記情報発信源の各々の方向を算出し、時系列の前記情報発信源の各々の位置に関する情報に基づいて、前記情報発信源の各々の速度を算出し、前記移動体から見た前記情報発信源の各々の方向、前記情報発信源の各々の速度、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体の情報発信源の各々の方向の速度を算出し、複数の前記移動体の情報発信源の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出する請求項１〜請求項５の何れか１項記載の相対位置推定装置。
【請求項７】
前記情報発信源を、前記情報発信源の各々の位置に関する情報として衛星軌道情報を、前記情報発信源の各々と前記移動体との距離に関する情報として擬似距離情報を、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報としてドップラー周波数情報を発信するＧＰＳ衛星とした請求項１〜請求項６の何れか１項記載の相対位置推定装置。
【請求項８】
コンピュータを、移動体と他の装置との相対位置を推定する、前記移動体に搭載される相対位置推定装置の各手段として機能させるための相対位置推定プログラムであって、
前記コンピュータを、
複数の情報発信源の各々から発信された前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記移動体との距離に関する情報、及び前記情報発信源の各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段、
前記取得手段により取得された前記発信源情報、及び前記移動体の運動状態を検出する検出手段によって検出された運動状態に基づいて、前記移動体の位置と、前記移動体の速度ベクトル、又は前記移動体の速度及び前記移動体の方位角を示す速度情報とを算出する算出手段、
他の装置の位置及び前記速度情報を受信する受信手段によって受信された前記他の装置の位置と、前記算出手段により算出された前記移動体の位置との差分から、各時刻又は各地点について、前記移動体と前記他の装置との相対位置を算出する相対位置算出手段、
前記算出手段により算出された前記移動体の速度ベクトル又は前記速度情報を所定時間分積算して、前記所定時間における前記移動体の位置の軌跡を算出する第１軌跡算出手段、
前記受信手段によって受信された前記速度情報を前記所定時間分積算して、前記所定時間における前記他の装置の位置の軌跡を算出する第２軌跡算出手段、及び
前記第１軌跡算出手段によって算出された前記所定時間における前記移動体の位置の軌跡と、前記第２軌跡算出手段によって算出された前記所定時間における前記他の装置の位置の軌跡と、前記相対位置算出手段によって前記所定時間内に各時刻又は各地点について算出された前記相対位置とに基づいて、前記軌跡上の特定点における前記相対位置を推定する相対位置推定手段
として機能させるための相対位置推定プログラム。
【請求項９】
コンピュータを、請求項１〜請求項７の何れか１項記載の相対位置推定装置の各手段として機能させるための相対位置推定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【公開番号】特開２０１２−２０３７２１（Ｐ２０１２−２０３７２１Ａ）
【公開日】平成２４年１０月２２日（２０１２．１０．２２）
【国際特許分類】
【出願番号】特願２０１１−６８８１４（Ｐ２０１１−６８８１４）
【出願日】平成２３年３月２５日（２０１１．３．２５）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２４年１０月２２日（２０１２．１０．２２）
【国際特許分類】
【出願日】平成２３年３月２５日（２０１１．３．２５）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]