JP5994237B2

JP5994237B2 - 測位装置及びプログラム

Info

Publication number: JP5994237B2
Application number: JP2011256649A
Authority: JP
Inventors: 竜也岩瀬; 洪二郎武山
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2011-11-24
Filing date: 2011-11-24
Publication date: 2016-09-21
Anticipated expiration: 2031-11-24
Also published as: JP2013108961A

Description

本発明は、測位装置及びプログラムに係り、特に、測位解と共にその信頼度を出力する測位装置及びプログラムに関する。

従来、ＧＰＳ（Global Positioning System、全地球測位システム）受信機またはＩＮＳ（Inertial Navigation System、慣性航法システム）と高度計を用いて飛翔体の位置を３次元（緯度、経度、高度）で検出し、その検出位置の誤差を検出に用いた機器の誤差特性に従って推定する空間位置表示装置が提案されている（例えば、特許文献１参照）。

また、ＧＰＳ内部の誤差情報、車内センサから得られる誤差情報、地図情報を用いて内部で補正を行った際に計算される補正量、過去の位置情報を蓄積し、ＧＰＳから送信される位置情報に含まれている誤差情報を用い、表示ディスプレイ上に現在の位置精度の誤差範囲を表示する事故車両表示方式が提案されている（例えば、特許文献２参照）。

また、ＧＰＳレシーバから受信したＧＰＳ位置推定値と慣性航法装置から受信したＩＮＵ位置推定値とを統合するモービル機械に配置された統合位置決めシステムが提案されている（例えば、特許文献３参照）。

特開２００１−９３１００号公報特開２０００−２９３７８４号公報特開平１０−３１１７３５号公報

しかし、特許文献１に記載の技術では、機器の誤差特性から計算される信頼度の範囲を単純な円で表現しているが、その具体的な計算方法は明記されていない。また、特許文献２においても、具体的な誤差範囲の計算方法は記載されていない。

また、特許文献３に記載の技術では、ＧＰＳ情報とＩＮＳ情報とをカルマンフィルタを用いて統合し、カルマンフィルタの共分散を信頼度としている。しかし、カルマンフィルタを適用した場合において、観測値に非ガウスノイズが存在する場合、信頼度の精度が低くなる可能性がある。

また、特許文献１〜３に記載の技術はいずれも、複数エポックの観測値を用いる方式ではないため、複数エポックのＧＰＳ情報とＩＮＳ情報とを統合する場合の信頼度については記載されていない。

本発明は、上記の問題点を解決するためになされたもので、ＧＰＳ情報とＩＮＳ情報とを統合して測位する場合に、測位解の信頼度を精度良く計算することができる測位装置及びプログラムを提供することを目的とする。

上記目的を達成するために、本発明の測位装置は、ＧＰＳ衛星から送信された信号を受信して擬似距離及びドップラー周波数を含むＧＰＳ情報を出力するＧＰＳ受信機から該ＧＰＳ情報をエポック毎に取得すると共に、自装置が搭載された移動体の移動量を含むＩＮＳ情報を検出するＩＮＳ装置から該ＩＮＳ情報をエポック毎に取得する取得手段と、
前記取得手段により取得された複数エポック分のＧＰＳ情報とＩＮＳ情報とを統合して、前記移動体の位置を測位する測位手段と、前記ＩＮＳ装置で検出された移動体の移動量の観測誤差を、各エポックにおける前記移動体から見た前記ＧＰＳ衛星の方向に投影して前記擬似距離の誤差に加算することにより、前記ＩＮＳ装置で検出された移動体の移動量の観測誤差と、前記擬似距離の誤差とを統合し、統合した誤差に基づいて、前記測位手段による測位の信頼度を計算する計算手段と、を含んで構成されている。

本発明の測位装置によれば、取得手段が、ＧＰＳ衛星から送信された信号を受信して擬似距離及びドップラー周波数を含むＧＰＳ情報を出力するＧＰＳ受信機からＧＰＳ情報をエポック毎に取得すると共に、自装置が搭載された移動体の移動量を含むＩＮＳ情報を検出するＩＮＳ装置からＩＮＳ情報をエポック毎に取得する。そして、測位手段が、取得手段により取得された複数エポック分のＧＰＳ情報とＩＮＳ情報とを統合して、移動体の位置を測位し、計算手段が、ＩＮＳ装置で検出された移動体の移動量の観測誤差を、各エポックにおける移動体から見たＧＰＳ衛星の方向に投影して擬似距離の誤差に加算することにより、ＩＮＳ装置で検出された移動体の移動量の観測誤差と、擬似距離の誤差とを統合し、統合した誤差に基づいて、測位手段による測位の信頼度を計算する。

このように、複数エポック分のＧＰＳ情報とＩＮＳ情報とを統合して、移動体の位置を測位する際に、ＩＮＳ装置の観測誤差をＧＰＳ受信機の観測誤差に換算して、ＩＮＳ装置の観測誤差とＧＰＳ受信機の観測誤差とを統合するため、ＩＮＳ装置の観測誤差も加味して、測位解の信頼度を精度良く計算することができる。

また、前記測位手段は、前記取得手段により取得された複数エポック分のＩＮＳ情報から前記移動体の移動軌跡を算出し、該移動軌跡を前記擬似距離を用いて位置あわせすることにより、前記移動体の位置を測位することができる。このような測位は、各エポックの位置ではなく、移動軌跡の位置のみを計算するだけでよいため、演算量を削減することができる。ＧＰＳ情報のみの観測方程式においても、上記のように、ＩＮＳ装置の観測誤差をＧＰＳ受信機の観測誤差に換算して、ＩＮＳ装置の観測誤差とＧＰＳ受信機の観測誤差とを統合するため、ＩＮＳ装置の観測誤差も加味して、測位解の信頼度を精度良く計算することができる。

また、本発明の測位装置は、前記測位手段により測位された移動体の位置との残差が予め定めた閾値以上となる擬似距離を含むＧＰＳ情報、または該ＧＰＳ情報を送信したＧＰＳ衛星から送信された全てのＧＰＳ情報を、前記取得手段により取得したＧＰＳ情報から排除して、前記測位手段による測位、及び前記計算手段による信頼度の計算が再度行われるように制御する制御手段を含んで構成することができる。これにより、マルチパス等の影響を受けたつじつまの合わないＧＰＳ情報を排除することができるため、測位精度がより向上する。

また、本発明の測位プログラムは、コンピュータを、ＧＰＳ衛星から送信された信号を受信して擬似距離及びドップラー周波数を含むＧＰＳ情報を出力するＧＰＳ受信機から該ＧＰＳ情報をエポック毎に取得すると共に、自装置が搭載された移動体の移動量を含むＩＮＳ情報を検出するＩＮＳ装置から該ＩＮＳ情報をエポック毎に取得する取得手段、前記取得手段により取得された複数エポック分のＧＰＳ情報とＩＮＳ情報とを統合して、前記移動体の位置を測位する測位手段、及び前記ＩＮＳ装置で検出された移動体の移動量の観測誤差を、各エポックにおける前記移動体から見た前記ＧＰＳ衛星の方向に投影して前記擬似距離の誤差に加算することにより、前記ＩＮＳ装置で検出された移動体の移動量の観測誤差と、前記擬似距離の誤差とを統合し、統合した誤差に基づいて、前記測位手段による測位の信頼度を計算する計算手段として機能させるためのプログラムである。

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

以上説明したように、本発明の測位装置及びプログラムによれば、複数エポック分のＧＰＳ情報とＩＮＳ情報とを統合して、移動体の位置を測位する際に、ＩＮＳ装置の観測誤差をＧＰＳ受信機の観測誤差に換算して、ＩＮＳ装置の観測誤差とＧＰＳ受信機の観測誤差とを統合するため、ＩＮＳ装置の観測誤差も加味して、測位解の信頼度を精度良く計算することができる、という効果が得られる。

本実施の形態に係る測位装置の機能的構成を示すブロック図である。ＧＰＳ情報とＩＮＳ情報との統合方式として、タイトカップリング方式を説明するための図である。ＧＰＳ情報とＩＮＳ情報との統合方式として、バンドルアジャストメント方式を説明するための図である。本実施の形態の原理を説明するための図である。つじつまの合わないＧＰＳ衛星を示すイメージ図である。本実施の形態における測位処理ルーチンの内容を示すフローチャートである。

以下、図面を参照して本発明の実施の形態を詳細に説明する。本実施の形態では、車両に搭載され、自車両の位置を測位する測位装置に本発明を適用した例について説明する。

図１に示すように、本実施の形態の測位装置１０は、ＧＰＳ衛星から送信されたＧＰＳ情報を受信するＧＰＳ受信機１２と、自車両の移動量を含むＩＮＳ情報を取得するＩＮＳ装置１４と、ＧＰＳ情報及びＩＮＳ情報に基づいて測位解及びその信頼度を算出するコンピュータ１６とを含んで構成されている。

ＧＰＳ受信機１２で受信されるＧＰＳ情報は、擬似距離、ドップラー周波数、及びエフェメリスを含む。また、ＩＮＳ装置１４は、速度センサ、ヨーレイトセンサ、及びジャイロセンサを備え、各センサにより検出された検出値に基づいて、自車両の移動量を算出する。

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

このコンピュータ１６を以下で説明する測位処理ルーチンに従って機能ブロックで表すと、図１に示すように、Ｍエポック分のＧＰＳ情報及びＩＮＳ情報に基づいて自車両の走行軌跡を算出する走行軌跡算出部２０と、エポック毎にＧＰＳ受信機１２で受信したＧＰＳ情報及びＩＮＳ装置１４で取得したＩＮＳ情報を記憶するＭエポックデータ記憶部２２と、算出した走行軌跡に基づいて、Ｍエポック分の自車両の位置の初期値を算出する初期値算出部２４と、ＩＮＳ装置１４の観測誤差（以下、ＩＮＳ誤差という）を擬似距離誤差に換算すると共に、初期値に基づくＧＰＳ情報の観測方程式を導出する誤差換算・方程式導出部２６と、観測方程式から測位解を計算する測位解計算部２８と、残差に基づいて整合性のないＧＰＳ衛星を選択する衛星選択部３０とを含んだ構成で表すことができる。

ここで、本実施の形態の原理について説明する。

ＧＰＳ情報とＩＮＳ情報とを統合した測位の方式として、図２に示すようなタイトカップリング方式がある。この方式では、ＧＰＳ受信機及びＩＮＳ装置の観測を元に、Ｍエポックの拘束条件を求める。すなわち、下記（１）式に示すように、擬似距離ρ（ＧＰＳ情報）と走行軌跡Δｘ（ＩＮＳ情報）との連立方程式を解くことにより、各エポックでの自車両の位置ｘ_１，ｘ_２，・・・，ｘ_Ｍを求める。

タイトカップリング方式では、エポック数Ｍが増加すると、（１）式内の行列Ｇのサイズも増大し、演算コストが大きくなるという問題がある。

また、ＧＰＳ情報とＩＮＳ情報とを統合した測位の別の方式として、図３に示すようなバンドルアジャストメント方式がある。この方式では、予め方位を定めて、ＩＮＳ情報から走行軌跡を算出しておき、その走行軌跡をＧＰＳ情報で位置合わせすることにより、走行軌跡の始点の位置を求める。すなわち、下記（２）式に示すように、擬似距離ρ（ＧＰＳ情報）と走行軌跡の位置あわせ量ｄｘとの連立方程式を解くことにより、各エポックでの自車両の位置ｘ_１，ｘ_２，・・・，ｘ_Ｍを求める。

バンドルアジャストメント方式では、ＩＮＳ情報から算出した走行軌跡が正しいと仮定しているため、（２）式に示すようにパラメータを減らして演算量を削減できる。しかし、方程式にＩＮＳ情報の変数が含まれていないため、ＩＮＳ誤差を信頼度に反映できないという問題がある。

そこで、本実施の形態では、図４に示すように、バンドルアジャストメント方式において、ＩＮＳ誤差を擬似距離誤差に換算することで、演算量を増加させることなく、ＩＮＳ誤差も含んだ信頼度を算出する。

以下、ｉをＧＰＳ衛星番号（１〜ｎ）、ｊをエポック番号（１〜Ｍ）とし、推定する状態［ｘ_Ｍ，ｙ_Ｍ，ｚ_Ｍ，Ｃｂ_Ｍ］（Ｍエポック目の位置及びクロックバイアス）として、各部について詳述する。

走行軌跡算出部２０は、ＩＮＳ装置１４から自車両の移動量を取得し、ＧＰＳ受信機１２からドップラー周波数を取得する。そして、自車両の走行軌跡として、１エポック目からｊエポック目までの移動量ベクトルΔｘ_ｊ、１エポック目からｊエポック目までの時間変化Δｔ_ｊ、クロックドリフトＣｄ、及び方位角θの値を求める。これらの値は、例えば、参考文献「ＧＰＳドップラーと慣性センサの統合による車両軌跡推定手法第４３回情報処理学会ＩＴＳ研究会」の手法を用いることができる。

初期値算出部２４は、１エポック目の自車両の位置の初期値を定め、走行軌跡算出部２０で算出された各値に基づいて、下記（３）式により、各エポックでの自車両の位置の初期値を算出し、下記（４）式により、各エポックでのＩＮＳ誤差を算出し、下記（５）式により、各エポックのクロックバイアスを算出し、下記（６）式により、各エポックでのクロックバイアス誤差を算出する。

ここで、Ｒは方位角θの回転行列、σ_ｖはＩＮＳ装置１４で検出される速度のばらつき、及びσ_ｃｄはクロックドリフトのばらつきである。なお、σ_ｖ及びσ_ｃｄは固定値を用いることができる。

誤差換算・方程式導出部２６は、下記（７）式により、ＩＮＳ誤差を擬似距離誤差に換算する。

ここで、σ_ρは擬似距離誤差、ｇ_ｉ，ｊはＧＰＳ受信機１２からｊエポック目のＧＰＳ衛星ｉへの単位方向ベクトルである。従って、（７）式は、ＩＮＳ誤差を、ＧＰＳ衛星の方向に投影して、ＧＰＳ受信機１２の観測誤差（以下、ＧＰＳ誤差という。ここでは、擬似距離誤差_σρ及びクロックバイアス誤差σ_ｃｂ）に加算することで、ＧＰＳ誤差とＩＮＳ誤差とを統合した観測誤差を計算していることを表している。なお、（７）式では、擬似距離誤差と換算したＩＮＳ誤差との二乗和の平方根により、統合した観測誤差を計算する例を示しているが、各誤差の絶対値の和などを用いて計算してもよく、観測誤差の統合方法は限定されない。

また、誤差換算・方程式導出部２６は、下記（８）式及び（９）式に示す初期値算出部２４で算出された初期値と真の値との関係に基づいて、下記（１０）式及び（１１）式の観測方程式を導出する。

なお、（１０）式は、１つのＧＰＳ衛星に関する観測方程式であり、（１１）式は、Ｍエポック分の全ＧＰＳ衛星に関する観測方程式である。また、このときの観測値の共分散行列は、下記（１２）式となる。

測位解計算部２８は、誤差換算・方程式導出部２６で導出された観測方程式を解いて、下記（１３）式により自車両の位置ｘの測位解を計算する。また、その共分散Ｑ_ｘを下記（１４）により計算する。このＱ_ｘが測位解の信頼度である。

衛星選択部３０は、計算された測位解とのつじつまが合わないＧＰＳ衛星からのＧＰＳ情報を排除する。つじつまが合わないとは、図５に示すように、各エポックでの測位解が示す位置とそのエポックでの衛星位置との距離と、そのエポックでの擬似距離との差が大きいことをいう。具体的には、（１１）式の観測方程式で推定される状態と、実際の観測値との残差を、下記（１５）式により計算する。

残差ｒが所定の閾値以上となるＧＰＳ衛星をつじつまの合わないＧＰＳ衛星として排除し、その衛星番号を誤差換算・方程式導出部２６に出力する。誤差換算・方程式導出部２６では、衛星選択部３０から出力された衛星番号のＧＰＳ衛星から受信したＧＰＳ情報を排除して、再度、観測方程式を導出し、測位解計算部２８で、再度、測位解を計算する。

次に、図６を参照して、本実施の形態の測位装置１０において実行される測位処理ルーチンについて説明する。

ステップ１００で、エポック毎にＧＰＳ受信機１２で受信したＧＰＳ情報及びＩＮＳ装置１４で取得したＩＮＳ情報を取得して、Ｍエポックデータ記憶部２２記憶する。

次に、ステップ１０２で、ＩＮＳ装置１４から自車両の移動量を取得し、ＧＰＳ受信機１２からドップラー周波数を取得する。そして、自車両の走行軌跡として、１エポック目からｊエポック目までの移動量ベクトルΔｘ_ｊ、１エポック目からｊエポック目までの時間変化Δｔ_ｊ、クロックドリフトＣｄ、及び方位角θの値を求める。

次に、ステップ１０４で、１エポック目の自車両の位置の初期値を定め、上記ステップ１０２で求めた各値に基づいて、（３）式により、各エポックでの自車両の位置の初期値を算出し、（４）式により、各エポックでのＩＮＳ装置１４の観測誤差を算出し、（５）式により、各エポックのクロックバイアスを算出し、（６）式により、各エポックでのクロックバイアス誤差を算出する。

次に、ステップ１０６で、（７）式により、ＩＮＳ誤差を擬似距離誤差に換算する。次に、ステップ１０８で、上記ステップ１０４で求めた初期値と真の値との関係に基づいて、（１１）式の観測方程式を導出する。

次に、ステップ１１０で、上記ステップ１０８で導出された観測方程式を解いて、（１３）式により自車両の位置ｘの測位解を計算し、（１４）式により、その信頼度（共分散）Ｑ_ｘを計算する。

次に、ステップ１１２で、（１５）式により残差ｒを計算し、残差ｒが所定の閾値以上となる擬似距離を送信したＧＰＳ衛星、すなわち、つじつまの合わないＧＰＳ衛星の有無を判定する。次に、ステップ１１４で、上記ステップ１１２での判定結果に基づいて、上記ステップ１１０で計算された測位解に整合性があるか否かを判定する。上記ステップ１１２で、つじつまの合わないＧＰＳ衛星が存在すると判定された場合には、本ステップで整合性なしと判定されて、ステップ１１６へ移行する。

ステップ１１６では、取得したＭエポック分のＧＰＳ情報のうち、上記ステップ１１２でつじつまが合わない判定されたＧＰＳ衛星から受信したＧＰＳ情報を排除して、ステップ１０６へ戻る。これにより、つじつまの合わないＧＰＳ衛星からのＧＰＳ情報が排除された状態で、再度、観測方程式が導出され、測位解が計算される。

上記ステップ１１４で、測位解に整合性あり、すなわち、つじつまの合わないＧＰＳ衛星が存在しないと判定された場合には、ステップ１１８へ移行し、上記ステップ１１２で計算された測位解及び信頼度を出力して、測位処理ルーチンを終了する。

以上説明したように、本実施の形態の測位装置によれば、複数エポック分のＧＰＳ情報及びＩＮＳ情報を統合して測位する際に、ＩＮＳ誤差をＧＰＳ衛星の方向に投影してＧＰＳ誤差に換算することで、測位解の信頼度として、ＩＮＳ誤差も加味した精度の高い信頼度を得ることができる。

また、ＧＰＳ情報とＩＮＳ情報との統合にバンドルアジャストメント方式を用いることで、方程式のパラメータを少なくすることができ、演算量を削減することができ、このような方式でも、誤差換算を行うことにより、ＩＮＳ誤差も加味した信頼度を得ることができる。

なお、上記実施の形態では、衛星選択部でつじつまの合わないＧＰＳ衛星を選択し、そのＧＰＳ衛星からのＧＰＳ情報を排除する場合について説明したが、そのＧＰＳ衛星からの全てのＧＰＳ情報を排除するのではなく、つじつまの合わないＧＰＳ情報のみを排除するようにしてもよい。

１０測位装置
１２ＧＰＳ受信機
１４ＩＮＳ装置
１６コンピュータ
２０走行軌跡算出部
２２Ｍエポックデータ記憶部
２４初期値算出部
２６誤差換算・方程式導出部
２８測位解計算部
３０衛星選択部

Claims

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