説明

測位装置及びプログラム

【課題】GPS情報とINS情報とを統合して測位する場合に、測位解の信頼度を精度良く計算する。
【解決手段】走行軌跡算出部20で、Mエポック分のGPS情報及びINS情報に基づいて自車両の走行軌跡を算出し、初期値算出部24で、算出した走行軌跡に基づいて、Mエポック分の自車両の位置の初期値を算出する。誤差換算・方程式導出部26で、バンドルアジャストメント方式により、初期値に基づくGPS情報の観測方程式を導出すると共に、INS誤差を、各エポックにおける自車両から見たGPS衛星の方向に投影して擬似距離誤差に換算して、GPS誤差と統合する。測位解計算部28で、観測方程式から測位解を計算すると共に、統合された観測誤差に基づいて信頼度を計算する。

【発明の詳細な説明】
【技術分野】
【0001】
本発明は、測位装置及びプログラムに係り、特に、測位解と共にその信頼度を出力する測位装置及びプログラムに関する。
【背景技術】
【0002】
従来、GPS(Global Positioning System、全地球測位システム)受信機またはINS(Inertial Navigation System、慣性航法システム)と高度計を用いて飛翔体の位置を3次元(緯度、経度、高度)で検出し、その検出位置の誤差を検出に用いた機器の誤差特性に従って推定する空間位置表示装置が提案されている(例えば、特許文献1参照)。
【0003】
また、GPS内部の誤差情報、車内センサから得られる誤差情報、地図情報を用いて内部で補正を行った際に計算される補正量、過去の位置情報を蓄積し、GPSから送信される位置情報に含まれている誤差情報を用い、表示ディスプレイ上に現在の位置精度の誤差範囲を表示する事故車両表示方式が提案されている(例えば、特許文献2参照)。
【0004】
また、GPSレシーバから受信したGPS位置推定値と慣性航法装置から受信したINU位置推定値とを統合するモービル機械に配置された統合位置決めシステムが提案されている(例えば、特許文献3参照)。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特開2001−93100号公報
【特許文献2】特開2000−293784号公報
【特許文献3】特開平10−311735号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
しかし、特許文献1に記載の技術では、機器の誤差特性から計算される信頼度の範囲を単純な円で表現しているが、その具体的な計算方法は明記されていない。また、特許文献2においても、具体的な誤差範囲の計算方法は記載されていない。
【0007】
また、特許文献3に記載の技術では、GPS情報とINS情報とをカルマンフィルタを用いて統合し、カルマンフィルタの共分散を信頼度としている。しかし、カルマンフィルタを適用した場合において、観測値に非ガウスノイズが存在する場合、信頼度の精度が低くなる可能性がある。
【0008】
また、特許文献1〜3に記載の技術はいずれも、複数エポックの観測値を用いる方式ではないため、複数エポックのGPS情報とINS情報とを統合する場合の信頼度については記載されていない。
【0009】
本発明は、上記の問題点を解決するためになされたもので、GPS情報とINS情報とを統合して測位する場合に、測位解の信頼度を精度良く計算することができる測位装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【0010】
上記目的を達成するために、本発明の測位装置は、GPS衛星から送信された擬似距離及びドップラー周波数を含むGPS情報を受信するGPS受信機から該GPS情報をエポック毎に取得すると共に、自装置が搭載された移動体の移動量を含むINS情報を検出するINS装置から該INS情報をエポック毎に取得する取得手段と、前記取得手段により取得された複数エポック分のGPS情報とINS情報とを統合して、前記移動体の位置を測位する測位手段と、前記INS装置の観測誤差を、各エポックにおける前記移動体から見た前記GPS衛星の方向を用いて前記GPS受信機の観測誤差に換算して該GPS受信機の観測誤差と統合し、統合した観測誤差に基づいて、前記測位手段による測位の信頼度を計算する計算手段と、を含んで構成されている。
【0011】
本発明の測位装置によれば、取得手段が、GPS衛星から送信された擬似距離及びドップラー周波数を含むGPS情報を受信するGPS受信機からGPS情報をエポック毎に取得すると共に、自装置が搭載された移動体の移動量を含むINS情報を検出するINS装置からINS情報をエポック毎に取得する。そして、測位手段が、取得手段により取得された複数エポック分のGPS情報とINS情報とを統合して、移動体の位置を測位し、計算手段が、INS装置の観測誤差を、各エポックにおける移動体から見たGPS衛星の方向を用いてGPS受信機の観測誤差に換算してGPS受信機の観測誤差と統合し、統合した観測誤差に基づいて、測位手段による測位の信頼度を計算する。
【0012】
このように、複数エポック分のGPS情報とINS情報とを統合して、移動体の位置を測位する際に、INS装置の観測誤差をGPS受信機の観測誤差に換算して、INS装置の観測誤差とGPS受信機の観測誤差とを統合するため、INS装置の観測誤差も加味して、測位解の信頼度を精度良く計算することができる。
【0013】
また、前記測位手段は、前記取得手段により取得された複数エポック分のINS情報から前記移動体の移動軌跡を算出し、該移動軌跡を前記擬似距離を用いて位置あわせすることにより、前記移動体の位置を測位することができる。このような測位は、各エポックの位置ではなく、移動軌跡の位置のみを計算するだけでよいため、演算量を削減することができる。GPS情報のみの観測方程式においても、上記のように、INS装置の観測誤差をGPS受信機の観測誤差に換算して、INS装置の観測誤差とGPS受信機の観測誤差とを統合するため、INS装置の観測誤差も加味して、測位解の信頼度を精度良く計算することができる。
【0014】
また、本発明の測位装置は、前記測位手段により測位された移動体の位置との残差が予め定めた閾値以上となる擬似距離を含むGPS情報、または該GPS情報を送信したGPS衛星から送信された全てのGPS情報を、前記取得手段により取得したGPS情報から排除して、前記測位手段による測位、及び前記計算手段による信頼度の計算が再度行われるように制御する制御手段を含んで構成することができる。これにより、マルチパス等の影響を受けたつじつまの合わないGPS情報を排除することができるため、測位精度がより向上する。
【0015】
また、本発明の測位プログラムは、コンピュータを、GPS衛星から送信された擬似距離及びドップラー周波数を含むGPS情報を受信するGPS受信機から該GPS情報をエポック毎に取得すると共に、自装置が搭載された移動体の移動量を含むINS情報を検出するINS装置から該INS情報をエポック毎に取得する取得手段、前記取得手段により取得された複数エポック分のGPS情報とINS情報とを統合して、前記移動体の位置を測位する測位手段、及び前記INS装置の観測誤差を、各エポックにおける前記移動体から見た前記GPS衛星の方向を用いて前記GPS受信機の観測誤差に換算して該GPS受信機の観測誤差と統合し、統合した観測誤差に基づいて、前記測位手段による測位の信頼度を計算する計算手段として機能させるためのプログラムである。
【0016】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ROMであってもよい。また、CD−ROMやDVDディスク、光磁気ディスクやICカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【0017】
以上説明したように、本発明の測位装置及びプログラムによれば、複数エポック分のGPS情報とINS情報とを統合して、移動体の位置を測位する際に、INS装置の観測誤差をGPS受信機の観測誤差に換算して、INS装置の観測誤差とGPS受信機の観測誤差とを統合するため、INS装置の観測誤差も加味して、測位解の信頼度を精度良く計算することができる、という効果が得られる。
【図面の簡単な説明】
【0018】
【図1】本実施の形態に係る測位装置の機能的構成を示すブロック図である。
【図2】GPS情報とINS情報との統合方式として、タイトカップリング方式を説明するための図である。
【図3】GPS情報とINS情報との統合方式として、バンドルアジャストメント方式を説明するための図である。
【図4】本実施の形態の原理を説明するための図である。
【図5】つじつまの合わないGPS衛星を示すイメージ図である。
【図6】本実施の形態における測位処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【0019】
以下、図面を参照して本発明の実施の形態を詳細に説明する。本実施の形態では、車両に搭載され、自車両の位置を測位する測位装置に本発明を適用した例について説明する。
【0020】
図1に示すように、本実施の形態の測位装置10は、GPS衛星から送信されたGPS情報を受信するGPS受信機12と、自車両の移動量を含むINS情報を取得するINS装置14と、GPS情報及びINS情報に基づいて測位解及びその信頼度を算出するコンピュータ16とを含んで構成されている。
【0021】
GPS受信機12で受信されるGPS情報は、擬似距離、ドップラー周波数、及びエフェメリスを含む。また、INS装置14は、速度センサ、ヨーレイトセンサ、及びジャイロセンサを備え、各センサにより検出された検出値に基づいて、自車両の移動量を算出する。
【0022】
コンピュータ16は、CPU、後述する測位処理ルーチンを実現するためのプログラムを記憶したROM、データを一時的に記憶するRAM、及びHDD等の記憶装置を含んで構成されている。
【0023】
このコンピュータ16を以下で説明する測位処理ルーチンに従って機能ブロックで表すと、図1に示すように、Mエポック分のGPS情報及びINS情報に基づいて自車両の走行軌跡を算出する走行軌跡算出部20と、エポック毎にGPS受信機12で受信したGPS情報及びINS装置14で取得したINS情報を記憶するMエポックデータ記憶部22と、算出した走行軌跡に基づいて、Mエポック分の自車両の位置の初期値を算出する初期値算出部24と、INS装置14の観測誤差(以下、INS誤差という)を擬似距離誤差に換算すると共に、初期値に基づくGPS情報の観測方程式を導出する誤差換算・方程式導出部26と、観測方程式から測位解を計算する測位解計算部28と、残差に基づいて整合性のないGPS衛星を選択する衛星選択部30とを含んだ構成で表すことができる。
【0024】
ここで、本実施の形態の原理について説明する。
【0025】
GPS情報とINS情報とを統合した測位の方式として、図2に示すようなタイトカップリング方式がある。この方式では、GPS受信機及びINS装置の観測を元に、Mエポックの拘束条件を求める。すなわち、下記(1)式に示すように、擬似距離ρ(GPS情報)と走行軌跡Δx(INS情報)との連立方程式を解くことにより、各エポックでの自車両の位置x,x,・・・,xを求める。
【0026】
【数1】

【0027】
タイトカップリング方式では、エポック数Mが増加すると、(1)式内の行列Gのサイズも増大し、演算コストが大きくなるという問題がある。
【0028】
また、GPS情報とINS情報とを統合した測位の別の方式として、図3に示すようなバンドルアジャストメント方式がある。この方式では、予め方位を定めて、INS情報から走行軌跡を算出しておき、その走行軌跡をGPS情報で位置合わせすることにより、走行軌跡の始点の位置を求める。すなわち、下記(2)式に示すように、擬似距離ρ(GPS情報)と走行軌跡の位置あわせ量dxとの連立方程式を解くことにより、各エポックでの自車両の位置x,x,・・・,xを求める。
【0029】
【数2】

【0030】
バンドルアジャストメント方式では、INS情報から算出した走行軌跡が正しいと仮定しているため、(2)式に示すようにパラメータを減らして演算量を削減できる。しかし、方程式にINS情報の変数が含まれていないため、INS誤差を信頼度に反映できないという問題がある。
【0031】
そこで、本実施の形態では、図4に示すように、バンドルアジャストメント方式において、INS誤差を擬似距離誤差に換算することで、演算量を増加させることなく、INS誤差も含んだ信頼度を算出する。
【0032】
以下、iをGPS衛星番号(1〜n)、jをエポック番号(1〜M)とし、推定する状態[x,y,z,Cb](Mエポック目の位置及びクロックバイアス)として、各部について詳述する。
【0033】
走行軌跡算出部20は、INS装置14から自車両の移動量を取得し、GPS受信機12からドップラー周波数を取得する。そして、自車両の走行軌跡として、1エポック目からjエポック目までの移動量ベクトルΔx、1エポック目からjエポック目までの時間変化Δt、クロックドリフトCd、及び方位角θの値を求める。これらの値は、例えば、参考文献「GPSドップラーと慣性センサの統合による車両軌跡推定手法 第43回 情報処理学会 ITS研究会」の手法を用いることができる。
【0034】
初期値算出部24は、1エポック目の自車両の位置の初期値を定め、走行軌跡算出部20で算出された各値に基づいて、下記(3)式により、各エポックでの自車両の位置の初期値を算出し、下記(4)式により、各エポックでのINS誤差を算出し、下記(5)式により、各エポックのクロックバイアスを算出し、下記(6)式により、各エポックでのクロックバイアス誤差を算出する。
【0035】
【数3】

【0036】
ここで、Rは方位角θの回転行列、σはINS装置14で検出される速度のばらつき、及びσcdはクロックドリフトのばらつきである。なお、σ及びσcdは固定値を用いることができる。
【0037】
誤差換算・方程式導出部26は、下記(7)式により、INS誤差を擬似距離誤差に換算する。
【0038】
【数4】

【0039】
ここで、σρは擬似距離誤差、gi,jはGPS受信機12からjエポック目のGPS衛星iへの単位方向ベクトルである。従って、(7)式は、INS誤差を、GPS衛星の方向に投影して、GPS受信機12の観測誤差(以下、GPS誤差という。ここでは、擬似距離誤差σρ及びクロックバイアス誤差σcb)に加算することで、GPS誤差とINS誤差とを統合した観測誤差を計算していることを表している。なお、(7)式では、擬似距離誤差と換算したINS誤差との二乗和の平方根により、統合した観測誤差を計算する例を示しているが、各誤差の絶対値の和などを用いて計算してもよく、観測誤差の統合方法は限定されない。
【0040】
また、誤差換算・方程式導出部26は、下記(8)式及び(9)式に示す初期値算出部24で算出された初期値と真の値との関係に基づいて、下記(10)式及び(11)式の観測方程式を導出する。
【0041】
【数5】

【0042】
なお、(10)式は、1つのGPS衛星に関する観測方程式であり、(11)式は、Mエポック分の全GPS衛星に関する観測方程式である。また、このときの観測値の共分散行列は、下記(12)式となる。
【0043】
【数6】

【0044】
測位解計算部28は、誤差換算・方程式導出部26で導出された観測方程式を解いて、下記(13)式により自車両の位置xの測位解を計算する。また、その共分散Qを下記(14)により計算する。このQが測位解の信頼度である。
【0045】
【数7】

【0046】
衛星選択部30は、計算された測位解とのつじつまが合わないGPS衛星からのGPS情報を排除する。つじつまが合わないとは、図5に示すように、各エポックでの測位解が示す位置とそのエポックでの衛星位置との距離と、そのエポックでの擬似距離との差が大きいことをいう。具体的には、(11)式の観測方程式で推定される状態と、実際の観測値との残差を、下記(15)式により計算する。
【0047】
【数8】

【0048】
残差rが所定の閾値以上となるGPS衛星をつじつまの合わないGPS衛星として排除し、その衛星番号を誤差換算・方程式導出部26に出力する。誤差換算・方程式導出部26では、衛星選択部30から出力された衛星番号のGPS衛星から受信したGPS情報を排除して、再度、観測方程式を導出し、測位解計算部28で、再度、測位解を計算する。
【0049】
次に、図6を参照して、本実施の形態の測位装置10において実行される測位処理ルーチンについて説明する。
【0050】
ステップ100で、エポック毎にGPS受信機12で受信したGPS情報及びINS装置14で取得したINS情報を取得して、Mエポックデータ記憶部22記憶する。
【0051】
次に、ステップ102で、INS装置14から自車両の移動量を取得し、GPS受信機12からドップラー周波数を取得する。そして、自車両の走行軌跡として、1エポック目からjエポック目までの移動量ベクトルΔx、1エポック目からjエポック目までの時間変化Δt、クロックドリフトCd、及び方位角θの値を求める。
【0052】
次に、ステップ104で、1エポック目の自車両の位置の初期値を定め、上記ステップ102で求めた各値に基づいて、(3)式により、各エポックでの自車両の位置の初期値を算出し、(4)式により、各エポックでのINS装置14の観測誤差を算出し、(5)式により、各エポックのクロックバイアスを算出し、(6)式により、各エポックでのクロックバイアス誤差を算出する。
【0053】
次に、ステップ106で、(7)式により、INS誤差を擬似距離誤差に換算する。次に、ステップ108で、上記ステップ104で求めた初期値と真の値との関係に基づいて、(11)式の観測方程式を導出する。
【0054】
次に、ステップ110で、上記ステップ108で導出された観測方程式を解いて、(13)式により自車両の位置xの測位解を計算し、(14)式により、その信頼度(共分散)Qを計算する。
【0055】
次に、ステップ112で、(15)式により残差rを計算し、残差rが所定の閾値以上となる擬似距離を送信したGPS衛星、すなわち、つじつまの合わないGPS衛星の有無を判定する。次に、ステップ114で、上記ステップ112での判定結果に基づいて、上記ステップ110で計算された測位解に整合性があるか否かを判定する。上記ステップ112で、つじつまの合わないGPS衛星が存在すると判定された場合には、本ステップで整合性なしと判定されて、ステップ116へ移行する。
【0056】
ステップ116では、取得したMエポック分のGPS情報のうち、上記ステップ112でつじつまが合わない判定されたGPS衛星から受信したGPS情報を排除して、ステップ106へ戻る。これにより、つじつまの合わないGPS衛星からのGPS情報が排除された状態で、再度、観測方程式が導出され、測位解が計算される。
【0057】
上記ステップ114で、測位解に整合性あり、すなわち、つじつまの合わないGPS衛星が存在しないと判定された場合には、ステップ118へ移行し、上記ステップ112で計算された測位解及び信頼度を出力して、測位処理ルーチンを終了する。
【0058】
以上説明したように、本実施の形態の測位装置によれば、複数エポック分のGPS情報及びINS情報を統合して測位する際に、INS誤差をGPS衛星の方向に投影してGPS誤差に換算することで、測位解の信頼度として、INS誤差も加味した精度の高い信頼度を得ることができる。
【0059】
また、GPS情報とINS情報との統合にバンドルアジャストメント方式を用いることで、方程式のパラメータを少なくすることができ、演算量を削減することができ、このような方式でも、誤差換算を行うことにより、INS誤差も加味した信頼度を得ることができる。
【0060】
なお、上記実施の形態では、衛星選択部でつじつまの合わないGPS衛星を選択し、そのGPS衛星からのGPS情報を排除する場合について説明したが、そのGPS衛星からの全てのGPS情報を排除するのではなく、つじつまの合わないGPS情報のみを排除するようにしてもよい。
【符号の説明】
【0061】
10 測位装置
12 GPS受信機
14 INS装置
16 コンピュータ
20 走行軌跡算出部
22 Mエポックデータ記憶部
24 初期値算出部
26 誤差換算・方程式導出部
28 測位解計算部
30 衛星選択部

【特許請求の範囲】
【請求項1】
GPS衛星から送信された擬似距離及びドップラー周波数を含むGPS情報を受信するGPS受信機から該GPS情報をエポック毎に取得すると共に、自装置が搭載された移動体の移動量を含むINS情報を検出するINS装置から該INS情報をエポック毎に取得する取得手段と、
前記取得手段により取得された複数エポック分のGPS情報とINS情報とを統合して、前記移動体の位置を測位する測位手段と、
前記INS装置の観測誤差を、各エポックにおける前記移動体から見た前記GPS衛星の方向を用いて前記GPS受信機の観測誤差に換算して該GPS受信機の観測誤差と統合し、統合した観測誤差に基づいて、前記測位手段による測位の信頼度を計算する計算手段と、
を含む測位装置。
【請求項2】
前記測位手段は、前記取得手段により取得された複数エポック分のINS情報から前記移動体の移動軌跡を算出し、該移動軌跡を前記擬似距離を用いて位置あわせすることにより、前記移動体の位置を測位する請求項1記載の測位装置。
【請求項3】
前記測位手段により測位された移動体の位置との残差が予め定めた閾値以上となる擬似距離を含むGPS情報、または該GPS情報を送信したGPS衛星から送信された全てのGPS情報を、前記取得手段により取得したGPS情報から排除して、前記測位手段による測位、及び前記計算手段による信頼度の計算が再度行われるように制御する制御手段を含む請求項1または請求項2記載の測位装置。
【請求項4】
コンピュータを、
GPS衛星から送信された擬似距離及びドップラー周波数を含むGPS情報を受信するGPS受信機から該GPS情報をエポック毎に取得すると共に、自装置が搭載された移動体の移動量を含むINS情報を検出するINS装置から該INS情報をエポック毎に取得する取得手段、
前記取得手段により取得された複数エポック分のGPS情報とINS情報とを統合して、前記移動体の位置を測位する測位手段、及び
前記INS装置の観測誤差を、各エポックにおける前記移動体から見た前記GPS衛星の方向を用いて前記GPS受信機の観測誤差に換算して該GPS受信機の観測誤差と統合し、統合した観測誤差に基づいて、前記測位手段による測位の信頼度を計算する計算手段
として機能させるための測位プログラム。

【図1】
image rotate

【図5】
image rotate

【図6】
image rotate

【図2】
image rotate

【図3】
image rotate

【図4】
image rotate


【公開番号】特開2013−108961(P2013−108961A)
【公開日】平成25年6月6日(2013.6.6)
【国際特許分類】
【出願番号】特願2011−256649(P2011−256649)
【出願日】平成23年11月24日(2011.11.24)
【出願人】(000003609)株式会社豊田中央研究所 (4,200)
【Fターム(参考)】