位置算出方法及び位置算出装置
【課題】移動体に備えられた衛星測位用ユニット及び慣性測位用ユニットの計測結果を併用した位置算出手法の提案。
【解決手段】GPSユニット3とINSユニット5とを備えた位置算出装置1において、影響度合設定部7は、位置算出を開始してから所与の条件が成立するまでの間は、GPS計測結果のINS計測結果に対する影響度合を第1の度合に設定し、所与の条件が成立した後は、影響度合を第1の度合よりも低い第2の度合に設定する。そして、カップリング処理部9は、影響度合設定部7により設定された影響度合に基づいてGPS計測結果とINS計測結果とのカップリング処理を実行して、移動体の位置を算出する。
【解決手段】GPSユニット3とINSユニット5とを備えた位置算出装置1において、影響度合設定部7は、位置算出を開始してから所与の条件が成立するまでの間は、GPS計測結果のINS計測結果に対する影響度合を第1の度合に設定し、所与の条件が成立した後は、影響度合を第1の度合よりも低い第2の度合に設定する。そして、カップリング処理部9は、影響度合設定部7により設定された影響度合に基づいてGPS計測結果とINS計測結果とのカップリング処理を実行して、移動体の位置を算出する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体に備えられた衛星測位用ユニット及び慣性測位用ユニットの計測結果を用いた位置算出方法等に関する。
【背景技術】
【0002】
いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、慣性センサーの活用が注目されている。慣性センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。慣性センサーの検出結果を利用して慣性航法演算を行う慣性航法システム(以下、「INS(Inertial Navigation System)」と称する。)も考案されている。
【0003】
INSでは、慣性センサーの検出結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献1には、INS演算結果を、GPS(Global Positioning System)を利用して補正する技術が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】米国特許出願公開第2010/0019963号明細書
【発明の概要】
【発明が解決しようとする課題】
【0005】
GPSを利用してINS演算結果を補正する技術は、GPS演算結果が正しいことを前提としている。特許文献1に開示された技術も同様である。しかし、GPS演算結果は、GPS衛星から受信したGPS衛星信号の信号強度、受信環境、GPS衛星の天空配置、マルチパスといった種々の要因により、演算結果の精度が低下する場合がある。このため、常にGPS演算結果を利用してINS演算結果を補正することが、必ずしも位置算出の精度改善に繋がるとは限らなかった。
【0006】
本発明は上述した課題に鑑みて為されたものであり、移動体に備えられた衛星測位用ユニット及び慣性測位用ユニットの計測結果を併用して、移動体の位置をより正確に算出するための手法を提案することを目的とする。
【課題を解決するための手段】
【0007】
以上の課題を解決するための第1の形態は、移動体に備えられた衛星測位用ユニットの第1の計測結果と、前記移動体に備えられた慣性測位用ユニットの第2の計測結果とを用いて、前記移動体の位置を算出する位置算出方法であって、位置算出を開始してから所与の条件が成立するまでの間は、前記第1の計測結果の前記第2の計測結果に対する影響度合を第1の度合に設定し、前記条件が成立した後は、前記影響度合を前記第1の度合よりも低い第2の度合に設定することと、前記影響度合に基づき、前記第1の計測結果と前記第2の計測結果とのカップリング処理を実行して前記移動体の位置を算出することと、を含む位置算出方法である。
【0008】
また、他の形態として、移動体に備えられた衛星測位用ユニットの第1の計測結果と、前記移動体に備えられた慣性測位用ユニットの第2の計測結果とを用いて、前記移動体の位置を算出する位置算出装置であって、位置算出を開始してから所与の条件が成立するまでの間は、前記第1の計測結果の前記第2の計測結果に対する影響度合を第1の度合に設定し、前記条件が成立した後は、前記影響度合を前記第1の度合よりも低い第2の度合に設定する影響度合設定部と、前記影響度合に基づき、前記第1の計測結果と前記第2の計測結果とのカップリング処理を実行して前記移動体の位置を算出するカップリング処理部と、を備えた位置算出装置を構成してもよい。
【0009】
この第1の形態等によれば、位置算出を開始してから所与の条件が成立するまでの間は、第1の計測結果の第2の計測結果に対する影響度合を第1の度合に設定する。所与の条件が成立した後は、影響度合を第1の度合よりも低い第2の度合に設定する。つまり、所与の条件の成立前後で、慣性測位用ユニットの計測結果に対する衛星測位用ユニットの計測結果の影響度合を変更する。これにより、特に初期動作時における影響度合の適正化を実現することができる。適正化した影響度合に基づき第1の計測結果と第2の計測結果とのカップリング処理を実行することで、位置算出の正確性を向上させることができる。
【0010】
また、第2の形態として、第1の形態の位置算出方法において、前記影響度合には、前記第1の計測結果を採用して前記カップリング処理を実行する実行頻度が含まれており、前記第1の度合に設定することは、前記実行頻度を第1の頻度に設定することを含み、前記第2の度合に設定することは、前記実行頻度を前記第1の頻度よりも低い第2の頻度に設定することを含み、前記位置を算出することは、前記実行頻度に従って前記第1の計測結果を採用した前記カップリング処理を実行することを含む、位置算出方法を構成することとしてもよい。
【0011】
この第2の形態によれば、所与の条件が成立するまでの間は、第1の計測結果を採用してカップリング処理を実行する実行頻度を第1の頻度に設定する。所与の条件が成立した後は、実行頻度を第1の頻度よりも低い第2の頻度に設定する。つまり、所与の条件が成立した後は、第1の計測結果をカップリング処理に採用する頻度を下げて、第1の計測結果の第2の計測結果に対する影響度合を低くする。これにより、衛星測位用ユニットの計測結果をカップリング処理に採用する頻度を適正化し、カップリング処理の実効性を高めることができる。
【0012】
また、第3の形態として、第1の形態の位置算出方法において、前記カップリング処理は、前記第1の計測結果を観測量とするカルマンフィルター処理を含み、前記影響度合には、前記カルマンフィルター処理に用いる誤差パラメーター値が含まれており、前記第1の度合に設定することは、前記誤差パラメーター値を第1のパラメーター値に設定することを含み、前記第2の度合に設定することは、前記誤差パラメーター値を前記第1のパラメーター値よりも大きい第2のパラメーター値に設定することを含み、前記位置を算出することは、前記第1の計測結果及び前記誤差パラメーター値を用いて前記カルマンフィルター処理を実行することを含む、位置算出方法を構成することとしてもよい。
【0013】
この第3の形態によれば、所与の条件が成立するまでの間は、カルマンフィルター処理に用いる誤差パラメーター値を第1のパラメーター値に設定する。所与の条件が成立した後は、誤差パラメーター値を第1のパラメーター値よりも大きい第2のパラメーター値に設定する。誤差パラメーター値は、例えば、カルマンフィルター処理において第1の計測結果を重視する程度を決定付ける値である。誤差パラメーター値を大きく設定することは、第1の計測結果の第2の計測結果に対する影響度合を下げることに相当する。誤差パラメーター値を適正化することで、算出される移動体の位置の正確性が向上する。
【0014】
また、第4の形態として、第1の形態の位置算出方法において、前記位置を算出することは、前記影響度合が前記第1の度合の場合に前記第1の計測結果を用いた所定の位置算出処理を実行して位置を算出し、前記影響度合が前記第2の度合の場合に前記カップリング処理を実行して位置を算出することである、位置算出方法を構成することとしてもよい。
【0015】
この第4の形態によれば、影響度合が第1の度合の場合は、第1の計測結果を用いた所定の位置算出処理を実行して位置を算出する。他方、影響度合が第2の度合の場合は、カップリング処理を実行して位置を算出する。これにより、影響度合に見合った位置算出の方式を適用して移動体の位置を算出することが可能となる。
【0016】
また、第5の形態として、第1〜第3の何れかの形態の位置算出方法において、前記影響度合を設定することは、位置算出を開始してからの経過時間又は位置算出回数が、前記位置算出の結果の精度が安定するための時間的条件として定められた精度安定条件を満たす場合に、前記所与の条件が成立したと判定することを含む、位置算出方法を構成することとしてもよい。
【0017】
この第5の形態によれば、位置算出を開始してからの経過時間又は位置算出回数が、位置算出の結果の精度が安定するための時間的条件として定められた精度安定条件を満たす場合に、第1の計測結果の第2の計測結果に対する影響度合を、第1の度合から第2の度合に低下させる。これにより、位置算出の結果の精度が安定したと判断した後は、衛星測位用ユニットの計測結果への依存度を下げて位置算出を行うことが可能となる。
【0018】
また、第6の形態として、第1〜第3の何れかの形態の位置算出方法において、前記位置を算出することは、位置算出の開始時に、前記第1の計測結果から求められた位置を以降の位置算出の基準位置とすることを含み、前記影響度合を設定することは、位置算出の開始時の前記第1の計測結果が所定の精度良好条件を満たす場合に、前記所与の条件が成立したと判定することを含む、位置算出方法を構成することとしてもよい。
【0019】
この第6の形態によれば、位置算出の開始時に、第1の計測結果から求められた位置を基準位置として以降の位置算出を行う。つまり、第1の計測結果の精度が良好であれば、真の位置に近い位置を基準位置として以降の位置算出を行うことができる。そこで、位置算出の開始時の第1の計測結果が所定の精度良好条件を満たす場合には、影響度合を第1の度合から第2の度合に低下させる。これにより、信頼性の高い基準位置が得られたと判断できる場合には、衛星測位用ユニットの計測結果への依存度を下げて位置算出を行うことが可能となる。
【0020】
また、第7の形態として、第1〜第3の何れかの形態の位置算出方法において、前記影響度合を設定することは、前記カップリング処理の結果が所定の精度良好条件を満たす場合に、前記所与の条件が成立したと判定することを含む、位置算出方法を構成することとしてもよい。
【0021】
この第7の形態によれば、カップリング処理の結果が所定の精度良好条件を満たす場合は、影響度合を第1の度合から第2の度合に低下させる。これにより、カップリング処理の結果の精度が良好であると判断できる場合には、衛星測位用ユニットの計測結果への依存度を下げて位置算出を行うことが可能となる。
【図面の簡単な説明】
【0022】
【図1】位置算出装置の主要構成図。
【図2】第1の位置算出装置の構成図。
【図3】カップリング処理の入出力データの説明図。
【図4】第1のモード設定条件の説明図。
【図5】第2のモード設定条件の説明図。
【図6】影響モード設定処理の流れを示すフローチャート。
【図7】位置算出を行った実験結果の一例を示す図。
【図8】位置算出を行った実験結果の一例を示す図。
【図9】位置算出を行った実験結果の一例を示す図。
【図10】位置算出を行った実験結果の一例を示す図。
【図11】ナビゲーションシステムのシステム構成図。
【図12】カーナビゲーション装置の機能構成を示すブロック図。
【図13】演算設定テーブルのテーブル構成図。
【図14】第1のナビゲーション処理の流れを示すフローチャート。
【図15】カップリング処理の流れを示すフローチャート。
【図16】第2のナビゲーション処理の流れを示すフローチャート。
【発明を実施するための形態】
【0023】
以下、図面を参照して、本発明の好適な実施形態の一例について説明する。本実施形態は、衛星測位システムの一種であるGPS(Global Positioning System)と、慣性航法演算を行うシステムであるINS(Inertial Navigation System)とを併用して位置算出を行う実施形態である。
【0024】
1.原理
1−1.構成
図1は、本実施形態における位置算出装置1の主要構成図である。位置算出装置1は、移動体に備えられて、当該移動体の位置を算出する装置(位置算出システム)である。移動体は、自動車やオートバイ、自転車、船、電車といった物の他、人間自体であってもよい。人間が位置算出装置1を携帯し、人間自身が位置算出装置1を備えることとしてもよい。図1では、ユニット(モジュール)を二重線で図示し、ユニットの計測結果を利用して演算処理を行う処理ブロックを一重線で図示することで、両者を区別する。図2についても同様である。
【0025】
位置算出装置1は、ユニット(モジュール)として、GPSユニット3と、INSユニット5とを備える。また、位置算出装置1は、主要な処理ブロックとして、影響度合設定部7と、カップリング処理部9とを有する。
【0026】
GPSユニット3は、衛星測位システムを利用して測位を行うためのユニット(衛星測位用ユニット)である。GPSユニット3は、GPS衛星から発信されているGPS衛星信号を受信するアンテナや、受信したGPS衛星信号を処理するプロセッサー等を有して構成される。
【0027】
GPSユニット3は、GPS衛星信号のコード位相やドップラー周波数、擬似距離、擬似距離変化率等のGPSメジャメント情報を計測可能に構成されている。また、GPSユニット3は、GPSメジャメント情報を用いたGPS演算を行って、移動体の位置や速度(速度ベクトル)等を計測可能に構成されている。GPSメジャメント情報やGPS演算結果は、GPS計測結果(第1の計測結果)としてカップリング処理部9に出力される。
【0028】
INSユニット5は、慣性航法を利用して測位を行うためのユニット(慣性測位用ユニット)である。INSユニット5は、加速度センサーやジャイロセンサー等の慣性センサーや、慣性センサーをパッケージ化した慣性計測ユニット(IMU(Inertial Measurement Unit))、慣性センサーの計測結果を処理するプロセッサー等を有して構成される。
【0029】
INSユニット5は、慣性センサーの計測結果を用いて、移動体の加速度(加速度ベクトル)や角速度等をINSメジャメント情報として計測可能に構成されている。また、INSユニット5は、INSメジャメント情報を用いた慣性航法演算(INS演算)を行って、移動体の位置や速度(速度ベクトル)、姿勢角等を計測可能に構成されている。INSメジャメント情報やINS演算結果は、INS計測結果(第2の計測結果)としてカップリング処理部9に出力される。
【0030】
影響度合設定部7は、GPS計測結果(第1の計測結果)のINS計測結果(第2の計測結果)に対する影響度合を設定する。影響度合設定部7は、位置算出を開始してから所与の条件が成立したか否かを判定する。そして、所与の条件が成立するまでの間は、GPS計測結果のINS計測結果に対する影響度合を第1の度合に設定し、条件が成立した後は、影響度合を第1の度合よりも低い第2の度合に設定する。
【0031】
カップリング処理部9は、影響度合設定部7により設定された影響度合に基づいて、GPS計測結果(第1の計測結果)とINS計測結果(第2の計測結果)とのカップリング処理を実行して移動体の位置等を算出する。
【0032】
図2は、図1の位置算出装置1を適用した第1の位置算出装置1Aの構成図である。第1の位置算出装置1Aでは、図1の影響度合設定部7が影響モード設定部7Aを有し、カップリング処理部9がカルマンフィルター処理部9Aを有する。
【0033】
影響モード設定部7Aは、後述するモード設定条件に従ってGPS影響モードを設定する。GPS影響モードは、GPSの影響度合を決定づけるモードである。本実施形態では、GPS計測結果のINS計測結果に対する影響度合が比較的高いモードである「高影響モード」と、GPS計測結果のINS計測結果に対する影響度合が比較的低いモードである「低影響モード」との2種類のモードを択一的に設定する場合を想定する。なお、初期設定では、高影響モードを設定することとする。
【0034】
カルマンフィルター処理部9Aは、GPS計測結果を観測量“Z”とするカルマンフィルター処理を実行して、GPS計測結果とINS計測結果とをカップリング(結び付け)する。具体的には、カルマンフィルターの理論に基づき、予測演算(時刻更新)と補正演算(観測更新)とを行って、移動体の状態“X”を推定する。
【0035】
本実施形態において、移動体の状態“X”には、少なくとも移動体の位置が含まれる。予測演算では、例えば、INSユニット5から入力したINS計測結果を制御入力“U”として、1つ前の時刻(前回時刻)における状態補正値“X+”から、現在の時刻(今回時刻)における状態“X”を予測する演算を行って、状態予測値“X−”を算出する。
【0036】
補正演算では、例えば、GPSユニット3から入力したGPS計測結果を観測量“Z”として用いて、予測演算で算出した状態予測値“X−”を補正する演算を行って、状態補正値“X+”を算出する。そして、算出した状態補正値“X+”を、カップリング結果として出力する。
【0037】
また、カルマンフィルター処理部9Aは、GPS計測結果とは別に、移動体の運動モデルに基づく制約条件を観測量“Z”として適用可能に構成されている。本実施形態では、移動体の停止時における速度制約条件である「停止時速度制約条件」と、移動体の移動時における速度制約条件である「移動時速度制約条件」との2種類の速度制約条件を適用する場合を想定する。
【0038】
停止時速度制約条件(第1の制約条件)は、移動体の停止時に適用可能な制約条件である。移動体が停止しているのであれば、理想的には移動体の速度はゼロである。従って、移動体が停止していると判定した場合は「移動体の各軸の速度成分=0(速度ベクトル=ゼロベクトル)」を観測量“Z”として与えることができる。
【0039】
移動時速度制約条件(第2の制約条件)は、移動体の移動時に適用可能な制約条件である。例えば、移動体として四輪自動車を想定した場合、通常、四輪自動車はジャンプや横滑りすることはないと仮定することができる。従って、移動体が移動していると判定した場合は「移動体の縦横方向の速度成分=0」を観測量“Z”として与えることができる。
【0040】
本実施形態では、GPS計測結果と上記の制約条件(停止時速度制約条件又は移動時速度制約条件)とを切り替えて観測量“Z”に適用する場合を一例として説明する。なお、本実施形態とは異なり、GPS計測結果と上記の制約条件とを観測量“Z”として併用することも当然に可能である。
【0041】
図3は、カルマンフィルター処理の入出力データの説明図である。制御入力“U”と、観測量“Z”と、状態“X”との対応関係を表したテーブルを図示している。カップリングには種々の方式が存在する。その中でも、ルーズカップリング(疎結合)と呼ばれる方式と、タイトカップリング(密結合)と呼ばれる方式とが一般的に用いられる。
【0042】
ルーズカップリング方式は、GPSとINSとの結び付きが比較的弱いカップリング方式である。この方式では、例えば、制御入力“U”をINS演算結果(位置、速度、姿勢角等)とし、観測量“Z”をGPS演算結果(位置、速度等)としてカップリング処理を実行する。そして、移動体情報(位置、速度、姿勢角等)を状態“X”として推定する。
【0043】
タイトカップリング方式は、GPSとINSとの結び付きが比較的強いカップリング方式である。この方式では、例えば、制御入力“U”をINS演算結果(位置、速度、姿勢角等)とし、観測量“Z”をGPSメジャメント情報(コード位相、ドップラー周波数、擬似距離、擬似距離変化率等)としてカップリング処理を実行する。そして、移動体情報(位置、速度、姿勢角等)を状態“X”として推定する。
【0044】
また、タイトカップリング方式として、制御入力“U”をINSメジャメント情報(加速度、角速度等)とし、観測量“Z”をGPSメジャメント情報(コード位相、ドップラー周波数、擬似距離、擬似距離変化率等)とし、状態“X”を移動体情報(位置、速度、姿勢角等)とする手法も存在する。
【0045】
本実施形態の位置算出方法は、上記の何れのカップリング方式に対しても実質的に同一に適用可能である。つまり、GPS計測結果(第1の計測結果)として、GPSメジャメント情報を適用してもよいし、GPS演算結果を適用してもよい。また、INS計測結果(第2の計測結果)として、INSメジャメント情報を適用してもよいし、INS演算結果を適用してもよい。
【0046】
INS計測結果及びGPS計測結果として用いる情報は、適用するシステムに応じて適宜設定可能である。この場合、カルマンフィルター処理の予測演算及び補正演算で用いる演算式やパラメーター値を、適用するシステムに応じて適宜設定すればよい。なお、具体的な演算式やパラメーター値については、公知の手法に基づき定義可能であるため、ここでは説明を省略する。
【0047】
1−2.GPS影響モードの設定
図4は、GPS影響モードの設定に用いる第1のモード設定条件の説明図であり、第1のモード設定条件を定めた第1のモード設定条件テーブルを図示したものである。第1のモード設定条件テーブルには、第1のモード設定条件と、設定モードとが対応付けて定められている。
【0048】
第1のモード設定条件は、(1)GPS初回演算精度、(2)位置算出を開始してからの経過時間又は位置算出回数、に基づいて定められた条件である。GPS初回演算精度は、GPSユニット3の初回の演算結果の精度である。
【0049】
1番目の条件は「GPS初回演算精度=良好」である。そして、この条件を満たす場合の設定モードとして「低影響モード」が定められている。つまり、「GPS初回演算精度=良好」の場合は、GPS影響モードを初期設定である「高影響モード」から「低影響モード」に切り替える。この1番目の条件は、位置算出の開始時のGPS計測結果(第1の計測結果)が所定の精度良好条件を満たすことに相当する。そして、精度良好条件を満たす場合には、GPS計測結果のINS計測結果に対する影響度合を、初期設定である第1の度合(高影響モード)から、第1の度合よりも低い第2の度合(低影響モード)に下げることに相当する。
【0050】
2番目の条件は「GPS初回演算精度=非良好」である。この2番目の条件は、さらに位置算出開始からの経過時間又は位置算出回数に応じて分けられている。経過時間≦精度安定時間“θT”、又は、位置算出回数≦精度安定回数“θC”、の条件を満たす場合の設定モードとして「高影響モード」が定められている。また、経過時間>精度安定時間“θT”、又は、位置算出回数>精度安定回数“θC”、の条件を満たす場合の設定モードとして「低影響モード」が定められている。つまり、「GPS初回演算精度=非良好」の場合は、GPS影響モードを初期設定である「高影響モード」のままとし、ある程度時間が経過した時点で「低影響モード」に切り替える。
【0051】
位置算出開始からの経過時間が所定の精度安定時間“θT”を超えること、又は、位置算出回数が所定の精度安定回数“θC”を超えることは、位置算出を開始してからの経過時間又は位置算出回数が、位置算出の結果の精度が安定するための時間的条件として定められた精度安定条件を満たすことに相当する。そして、この精度良好条件を満たす場合には、GPS計測結果のINS計測結果に対する影響度合を、当初設定した第1の度合(高影響モード)から、第1の度合よりも低い第2の度合(低影響モード)に下げることに相当する。
【0052】
なお、精度安定時間“θT”や精度安定回数“θC”は、本実施形態の位置算出方法を適用するシステムに応じて適宜設定可能である。例えば、位置算出を「1秒間隔」で行うシステムに適用するのであれば、“θT”として「30秒〜60秒」の範囲の秒数、“θC”として「30回〜60回」の範囲の回数を設定すると効果的である。
【0053】
GPS演算では、各GPS衛星それぞれについて観測した擬似距離を利用して、例えば最小二乗法を適用した測位計算を行って移動体の位置を算出する。また、受信したGPS衛星信号の受信周波数の誤差(搬送波周波数からの周波数ズレ)に基づいてGPS衛星と移動体との相対速度(相対速度ベクトル)を算出し、これを用いて移動体の速度(速度ベクトル)を算出する。この際、公知の誤差推定演算を行うことで、算出した位置や速度(速度ベクトル)に含まれ得る最大の誤差を推定することができる。
【0054】
そこで、上記の誤差推定演算で推定される誤差に対する閾値を予め定めておく。例えば、位置誤差の閾値として“10[m]”、速度誤差の閾値として“1[m/s]”を定めておく。そして、位置誤差及び速度誤差に対する閾値判定をそれぞれ行って、GPS初回演算精度を判定する。この場合には、それぞれの閾値判定結果に対してAND条件を適用してGPS初回演算精度を判定してもよいし、OR条件を適用してGPS初回演算精度を判定してもよい。
【0055】
GPSの初回演算精度が良好であるにも関わらずGPS影響モードを「低影響モード」に設定することや、GPSの初回演算精度が非良好であるにも関わらずGPS影響モードを「高影響モード」に設定することは、一見矛盾しているようにも思われる。しかし、敢えてこのような設定方法を採用したのには理由がある。
【0056】
慣性航法演算を利用した位置算出を行う際には、基準位置や基準方位が必要となる。そのため、例えば、位置算出の開始時にGPS演算処理を行い、得られた位置及び方位を以降の位置算出の基準位置及び基準方位とすることが、1つの手法として考えられる。
【0057】
GPS初回演算精度が良好であれば、精度の高い位置や方位を基準として以降の位置算出を行うことができる。そのため、GPSの影響度合を下げてINSに依存して位置算出を行っても、暫くの間は、比較的精度の高い安定した演算結果を得ることができる。
【0058】
ところが、GPS初回演算精度が良好でない場合は、精度の低い位置や方位を基準として以降の位置算出を行わなければならなくなる。つまり、当初から誤差が重畳された状態で位置算出を開始することになる。この場合、GPSの影響度合を下げてINSに依存してしまうと、当初の誤差分がいつまで経っても補正されなくなる。
【0059】
そこで、GPSの初回演算精度が良好である場合は、GPS影響モードを「低影響モード」に設定し、GPSの初回演算精度が非良好である場合は、GPS影響モードを「高影響モード」に設定する。GPS影響モードを「高影響モード」に設定した状態で位置算出を行っていくと、GPSの演算精度が改善されるのに伴い、当初の誤差分が次第に補正されていく。そこで、ある程度の時間が経過した時点で、GPS影響モードを「高影響モード」から「低影響モード」に切り替えることで、GPSへの依存度を低くする。
【0060】
図5は、GPS影響モードの設定に用いる第2のモード設定条件の説明図であり、第2のモード設定条件を定めた第2のモード設定条件テーブルを図示したものである。第2のモード設定条件テーブルには、第2のモード設定条件と、設定モードとが対応付けて定められている。
【0061】
第2のモード設定条件は、カップリング処理の結果に基づき定義される条件である。具体的には、1番目の条件として「カップリング結果精度=良好」が定められている。そして、この1番目の条件を満たす場合の設定モードとして「低影響モード」が定められている。また、2番目の条件として「カップリング結果精度=非良好」が定められている。そして、この2番目の条件を満たす場合の設定モードとして「高影響モード」が定められている。
【0062】
カップリング結果精度は、カップリング処理を行うことで得られた移動体の位置等の精度のことを意味する。例えば、カルマンフィルター処理では、推定対象とする状態“X”の誤差“P”を設定して演算を行う。予測演算及び補正演算において、状態“X”と併せて誤差“P”の予測及び補正を行うことで、状態“X”の精度が定まる。従って、カルマンフィルター処理で用いる誤差“P”を、カップリング結果精度として利用することができる。
【0063】
状態“X”が複数の要素で構成される場合は、誤差“P”は行列形式の誤差共分散行列“P”で表すことができる。この場合、誤差共分散行列“P”の対角成分から、各要素の精度を見積もることができる。例えば、状態“X”の要素が移動体の位置、速度及び姿勢角であれば、誤差共分散行列“P”の対角成分が、それぞれカップリング位置誤差、カップリング速度誤差及びカップリング姿勢角誤差となる。
【0064】
そこで、カップリング位置誤差、カップリング速度誤差及びカップリング姿勢角誤差に対する閾値を予め定めておく。例えば、カップリング位置誤差の閾値として“10[m]”、カップリング速度誤差の閾値として“1[m/s]”、カップリング姿勢角誤差の閾値として“1[°]”を定めておく。そして、それぞれに対する閾値判定を行って、カップリング結果精度を判定する。この場合には、それぞれの閾値判定結果に対してAND条件を適用してカップリング結果精度を判定してもよいし、OR条件を適用してカップリング結果精度を判定してもよい。
【0065】
図6は、本実施形態における影響モード設定処理の流れを示すフローチャートである。最初に、影響モード設定部7Aは、図4の第1のモード設定条件テーブルを参照して、第1のモード設定条件の成否を判定する(ステップS1)。その判定結果(第1の判定結果)が高影響モードであれば(ステップS3;高影響モード)、影響モード設定部7Aは、図5の第2のモード設定条件テーブルを参照して、第2のモード設定条件の成否を判定する(ステップS5)。
【0066】
ステップS5における判定結果(第2の判定結果)が高影響モードであれば(ステップS7;高影響モード)、影響モード設定部7Aは、GPS影響モードを高影響モードに設定する(ステップS9)。一方、ステップS1における第1の判定結果が低影響モードである場合(ステップS3;低影響モード)、又は、ステップS5における第2の判定結果が低影響モードである場合は(ステップS7;低影響モード)、影響モード設定部7Aは、GPS影響モードを低影響モードに設定する(ステップS11)。そして、影響モード設定処理を終了する。
【0067】
この影響モード設定処理では、第1の判定結果及び第2の判定結果が両方とも高影響モードである場合に限り、GPS影響モードを高影響モードに設定することにしている。これには、なるべく早い段階でGPSの影響度合を下げて位置算出を行おうとする狙いがある。
【0068】
1−3.演算設定
カップリング処理部9は、GPS影響モードに応じて演算設定を変更してカップリング処理を行う。本実施形態において、演算設定には、GPS計測結果採用頻度と、カルマンフィルター処理で用いる誤差パラメーター値とが含まれる。
【0069】
GPS計測結果採用頻度は、GPS計測結果を採用してカップリング処理を実行する実行頻度である。図2では、カルマンフィルター処理部9Aが、観測量“Z”であるGPS計測結果を採用して補正演算を実行する頻度が、GPS計測結果採用頻度に相当する。
【0070】
GPS影響モードのモード設定が「高影響モード」である場合は、GPS計測結果をINS計測結果に対して強く作用させることが必要となる。そこで、GPS計測結果採用頻度を、比較的高い第1の頻度に設定する。それに対し、GPS影響モードのモード設定が「低影響モード」である場合は、GPS計測結果のINS計測結果に対する作用を弱める必要がある。そこで、GPS計測結果採用頻度を、第1の頻度よりも低い第2の頻度に設定する。第2の頻度は第1の頻度よりも低い頻度であればよく、その具体的な値は適宜設定可能である。
【0071】
誤差パラメーター値は、カルマンフィルターの演算において設定するパラメーター値の一種である。本実施形態では、観測量“Z”の誤差に相当する観測誤差(観測雑音)“R”を、誤差パラメーターの一例として説明する。
【0072】
カルマンフィルター処理の補正演算において、観測誤差“R”を小さく設定すると、観測量“Z”に追従するように状態“X”が補正される。つまり、観測量“Z”を信用・重視して状態“X”を推定するようにフィルターが作用する。それに対して、観測誤差“R”を大きく設定すると、状態予測値“X−”に追従するように状態“X”が補正される。つまり、予測演算で予測された状態予測値“X−”を信用・重視して状態“X”を推定するようにフィルターが作用する。
【0073】
このことから、GPS影響モードのモード設定が「高影響モード」である場合は、GPS計測結果をINS計測結果に強く作用させるために、観測誤差“R”を比較的小さい第1のパラメーター値に設定する。それに対し、GPS影響モードのモード設定が「低影響モード」である場合は、GPS計測結果のINS計測結果に対する作用を弱めるために、観測誤差“R”を第1のパラメーター値よりも大きい第2のパラメーター値に設定する。第2のパラメーター値は第1のパラメーター値よりも大きな値であればよく、その具体的な値は適宜設定可能である。
【0074】
1−4.実験結果
次に、本実施形態の位置算出方法を用いて実際に位置算出を行った実験結果について説明する。移動体を予め定められた経路に沿って移動させ、その場合に算出される位置を東西南北の2次元平面にプロットする実験を行った。
【0075】
図7は、従来の位置算出方法を適用した実験結果の一例である。それに対し、図8は、本実施形態の位置算出方法を適用した実験結果の一例である。図7及び図8の実験では、真の方位から「10°」ずれた誤った方位を第1の位置算出装置1Aに初期方位として与えて位置算出を行った。
【0076】
各図では、横軸は東西方向、縦軸は南北方向(単位はメートル)をそれぞれ示す。東西方向「0m」、南北方向「0m」の位置をスタート地点とし、スタート地点から西方向に向かい、時計回り方向に周回する経路を辿った。ゴール地点は、スタート地点の近傍所定位置である。移動体の真の軌跡を「点線」で、GPSで算出した軌跡を「一点鎖線」で、カップリング処理で算出した軌跡を「実線」でそれぞれ示している。
【0077】
従来の手法を適用した図7の結果を見ると、初期方位として与えた方位が「10°」誤っていたため、この方位誤差に起因して、位置算出の精度が劣化していることがわかる。つまり、初期方位が誤っている状態で移動時速度制約条件を適用しているために、カルマンフィルター処理において、誤った方位に対して移動方向の制限がかかり、時間経過に伴って位置誤差が累積的に増加している。
【0078】
それに対し、本実施形態の手法を適用した図8の結果を見ると、初期方位が「10°」誤っていたにも関わらず、真の軌跡に沿った滑らかな軌跡が得られていることがわかる。これは、GPSの影響度合をその都度設定しながらカップリング処理を行ったことで、INS演算結果がGPS演算結果によって適切に補正されたことによるものである。
【0079】
図9及び図10は、第1のモード設定条件の有効性を示す実験結果である。図4で説明した第1のモード設定条件では、GPSの初回演算精度が良好であるにも関わらずGPS影響モードを「低影響モード」に設定し、GPSの初回演算精度が良好でないにも関わらずGPS影響モードを「高影響モード」に設定する。一見矛盾しているようにも思われるこのパラドックスの正当性を示す実験結果の一例である。
【0080】
GPS初回演算精度が良好である場合と、良好でない場合とのそれぞれについて、GPS影響モードを変えて位置算出を行ってみた。つまり、GPS影響モードを「低影響モード」に設定した場合と、GPS影響モードを「高影響モード」に設定した場合とで、位置算出結果にどのような違いが現れるかを検証した。
【0081】
各図において、横軸は東西方向、縦軸は南北方向(単位はメートル)をそれぞれ示している。東西方向「0m」、南北方向「0m」の位置をスタート地点とし、スタート地点から西方向に向かった後、北方向に向かう経路を辿った。ゴール地点は、西方向「70m」、北方向「140m」の位置である。各図では、移動体の真の軌跡を「点線」で、GPSを利用して算出した軌跡を「一点鎖線」で、GPS影響モードを高影響モードに設定して算出した軌跡を「細実線」で、GPS影響モードを低影響モードに設定して算出した軌跡を「太実線」でそれぞれ示している。
【0082】
図9は、GPS初回演算精度が良好である場合の実験結果である。これを見ると、GPS影響モードを高影響モード及び低影響モードの何れに設定した場合も、GPSの初回演算精度が良好であるために正確な基準位置及び基準方位が与えられたことで、暫くの期間は正確な軌跡が得られていることがわかる。しかし、ある時点においてGPSの演算精度が低下したことで、両者に明確な違いが生じている。
【0083】
GPS影響モードを高影響モードに設定した場合は、GPSの影響度合が高いために、誤ったGPS演算位置に引きずられて、途中から位置算出の精度が劣化していることがわかる。具体的には、算出位置の軌跡(細実線)が、誤ったGPS演算位置の軌跡(一点鎖線)に引き寄せられ、真の軌跡(点線)から離れていっていることがわかる。
【0084】
それに対し、GPS影響モードを低影響モードに設定した場合は、GPSの影響度合が低いために上記の問題は生じず、真の軌跡(点線)に沿った軌跡(太実線)が得られていることがわかる。
【0085】
図10は、GPS初回演算精度が良好でない場合の実験結果である。この結果を見ると、GPS影響モードを低影響モードに設定した場合は、算出位置の軌跡(太実線)が、真の軌跡(点線)から大きくかけ離れていることがわかる。つまり、誤った基準位置及び基準方位が与えられたために、当初から誤った方位に移動方向の制限がかかってしまっている。そして、ある時点で移動方向を完全に誤ってしまったが、GPSの影響度合が低いために補正がかからず、そのまま誤った方向に位置が算出されてしまっている。
【0086】
それに対し、GPS影響モードを高影響モードに設定した場合は、低影響モードに設定した場合と比べて真の軌跡(点線)に近い軌跡(細実線)が得られていることがわかる。これは、GPSの影響度合を高くし、時間をかけてゆっくりGPS計測結果をINS計測結果に作用させたことで、誤った方位に移動方向の制限がかかることが防止されたためである。
【0087】
図9及び図10の結果から、GPSの初回演算精度が良好である場合はGPS影響モードを「低影響モード」に設定し、GPSの初回演算精度が非良好である場合はGPS影響モードを「高影響モード」に設定することが妥当であることが実証された。
【0088】
2.実施例
次に、上記の位置算出装置を備えた電子機器の実施例について説明する。ここでは、位置算出装置を具備するカーナビゲーション装置の実施例を説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【0089】
2−1.システム構成
図11は、本実施例におけるナビゲーションシステム1000のシステム構成の説明図である。ナビゲーションシステム1000は、移動体の一種である四輪自動車(以下、単に「自動車」と称す。)に、位置算出装置を具備した電子機器の一種であるカーナビゲーション装置100が設置されたシステムである。
【0090】
カーナビゲーション装置100は、自動車に設置され、自動車の運転者に対するナビゲーションを行う電子機器である。カーナビゲーション装置100は、GPSユニット3と、INSユニット5とを備える。
【0091】
本実施例では、GPSユニット3は、GPSメジャメント情報55を計測して出力する。また、INSユニット5は、INSメジャメント情報56を、機体座標系(Body Frame)として知られるBフレームで計測して出力する。Bフレームは、例えば、移動体の前方を正とする前後方向をR軸(ロール軸)、右方を正とする左右方向をP軸(ピッチ軸)、鉛直下方を正とする上下方向をY軸(ヨー軸)とする三次元直交座標系である。
【0092】
カーナビゲーション装置100は、GPSユニット3から取得したGPSメジャメント情報を用いてGPS演算処理を行い、且つ、INSユニット5から取得したINSメジャメント情報を用いてINS演算処理を行う。そして、これらの演算結果に対するカップリング処理を実行して自動車の位置を算出し、算出位置をプロットしたナビゲーション画面を表示部30であるディスプレイに表示させる。
【0093】
自動車の位置は、自動車の移動空間を定める絶対的な座標系であるNフレームにおいて演算する。Nフレームは、例えば、北東下座標系として知られるNED(North East Down)座標系や、地球中心地球固定座標系として知られるECEF(Earth Centered Earth Fixed)座標系として定義される。
【0094】
2−2.機能構成
図12は、カーナビゲーション装置100の機能構成の一例を示すブロック図である。カーナビゲーション装置100は、GPSユニット3と、INSユニット5と、処理部10と、操作部20と、表示部30と、通信部40と、記憶部50とを備えて構成される。
【0095】
処理部10は、記憶部50に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置100の各部を統括的に制御する制御装置であり、CPU(Central Processing Unit)等のプロセッサーを有して構成される。処理部10は、記憶部50に記憶されたナビゲーションプログラム51に従ってナビゲーション処理を行い、自動車の現在位置を指し示した地図を表示部30に表示させる処理を行う。
【0096】
操作部20は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部10に出力する。この操作部20の操作により、目的地の入力等の各種指示入力がなされる。
【0097】
表示部30は、LCD(Liquid Crystal Display)等により構成され、処理部10から入力される表示信号に基づいた各種表示を行う表示装置である。表示部30には、ナビゲーション画面等が表示される。
【0098】
通信部40は、処理部10の制御に従って、装置内部で利用される情報をインターネット等の通信ネットワークを介して外部とやりとりするための通信装置である。この通信には、公知の無線通信技術を適用可能である。
【0099】
記憶部50は、ROM(Read Only Memory)やフラッシュROM、RAM(Random Access Memory)等の記憶装置によって構成され、カーナビゲーション装置100のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【0100】
記憶部50には、プログラムとして、処理部10により読み出され、各種のナビゲーション処理(図14や図16参照)として実行されるナビゲーションプログラム51が記憶されている。ナビゲーションプログラム51は、影響モード設定処理(図6参照)として実行される影響モード設定プログラム511や、カップリング処理(図15参照)として実行されるカップリングプログラム513をサブルーチンとして含む。
【0101】
また、記憶部50には、データとして、演算設定テーブル52と、モード設定条件テーブル53と、設定モード54と、GPSメジャメント情報55と、INSメジャメント情報56と、GPS演算結果57と、INS演算結果58と、カップリング結果59とが記憶される。
【0102】
演算設定テーブル52は、演算設定が定義されたテーブルであり、そのテーブル構成の一例を図13に示す。演算設定テーブル52には、GPS影響モード521と、演算設定523とが対応付けて記憶されている。高影響モードに対しては、GPS計測結果採用頻度として「毎回」(第1の頻度)が定められている。一方、低影響モードに対しては、GPS計測結果採用頻度として「10回に1回」(第2の頻度)が定められている。
【0103】
つまり、GPS影響モードを高影響モード(第1の度合)に設定した場合は、GPS演算結果を毎回採用して、カップリング処理を実行する。それに対し、GPS影響モードを低影響モード(第2の度合)に設定した場合は、GPS演算結果を10回に1回採用して、カップリング処理を実行する。
【0104】
観測誤差(R値)には、位置の観測誤差である位置観測誤差“RP”と、速度の観測誤差である速度観測誤差“RV”とが含まれる。高影響モードには「RP=(σP)2,RV=(σV)2」(第1のパラメーター値)が定められている。一方、低影響モードには「RP=500,RV=50」(第2のパラメーター値)が定められている。
【0105】
“σP”及び“σV”は、GPS演算結果に含まれる位置誤差及び速度誤差である。本願発明者が行った実験によれば、通常の測位環境では、位置誤差及び速度誤差は“σP=3〜4[m]”、“σV=0.6〜0.8[m/s]”程度の値として算出される。このため、高影響モードでは、位置観測誤差“RP”として“10”程度の値が設定され、速度観測誤差“RV”として“0.5”程度の値が設定される。従って、低影響モードでは、高影響モードと比べて大きい値が観測誤差“R”に設定される。
【0106】
モード設定条件テーブル53は、GPS影響モードを設定するための条件が定められたテーブルであり、例えば第1のモード設定条件テーブルや第2のモード設定条件テーブル(図4及び図5参照)がこれに含まれる。
【0107】
設定モード54は、設定されているGPS影響モードであり、影響モード設定処理により随時更新される。
【0108】
2−3.処理の流れ
図14は、処理部10が、記憶部50に記憶されたナビゲーションプログラム51に従って実行するナビゲーション処理の一例である第1のナビゲーション処理の流れを示すフローチャートである。
【0109】
先ず、処理部10は、GPSユニット3及びINSユニット5から、GPSメジャメント情報55及びINSメジャメント情報56の取得を開始する(ステップA1)。そして、処理部10は、移動状況判定処理を行う(ステップA3)。具体的には、例えば、INSユニット5からINSメジャメント情報56として取得した自動車の加速度(加速度ベクトル)や角速度に基づいて、自動車が停止している状態であるか、移動している状態であるかを判定する。
【0110】
その後、処理部10は、GPS演算処理を行う(ステップA5)。具体的には、GPSユニット3から取得したGPSメジャメント情報55を用いて、公知の測位計算を行って、自動車の位置や速度(速度ベクトル)を演算する。また、公知の誤差推定演算を行って、位置及び速度(速度ベクトル)の誤差を推定する。そして、これらの演算結果をGPS演算結果57として記憶部50に記憶させる。
【0111】
また、処理部10は、INS演算処理を行う(ステップA7)。具体的には、INSユニット5から取得したINSメジャメント情報56を用いて、公知の慣性航法演算を行って、自動車の位置や速度(速度ベクトル)、姿勢角を演算する。そして、これらの演算結果をINS演算結果58として記憶部50に記憶させる。
【0112】
次いで、処理部10は、記憶部50に記憶された影響モード設定プログラム511に従って、図6で説明した影響モード設定処理を行う(ステップA9)。そして、処理部10は、記憶部50に記憶されたカップリングプログラム513に従ってカップリング処理を実行する(ステップA11)。
【0113】
図15は、カップリング処理の流れを示すフローチャートである。
処理部10は、GPS演算結果57をカップリング処理に採用するか否かを判定する(ステップB1)。具体的には、影響モード設定処理で設定されたGPS影響モードに対応付けられたGPS計測結果採用頻度に基づき、今回のカップリング処理においてGPS演算結果57を採用するか否かを判定する。
【0114】
GPS演算結果57をカップリング処理に採用すると判定したならば(ステップB1;Yes)、処理部10は、最新のGPS演算結果57を観測量とする観測量ベクトル“Z”を設定する(ステップB3)。また、演算設定テーブル52を参照し、設定モード54に対応する観測誤差に基づいて観測誤差共分散行列“R”を設定する(ステップB5)。そして、処理部10は、ステップB3及びB5で設定した観測量ベクトル“Z”及び観測誤差共分散行列“R”を用いて、カルマンフィルター処理を実行する(ステップB7)。
【0115】
一方、ステップB1においてGPS演算結果57をカップリング処理に採用しないと判定したならば(ステップB1;No)、処理部10は、ステップA3で判定した移動状況を判定する(ステップB9)。
【0116】
移動状況が「移動」である場合は(ステップB9;移動)、処理部10は、移動時制約速度ベクトルを観測量ベクトル“Z”として設定する(ステップB11)。また、移動時用速度観測誤差(例えば所定値)に基づき観測誤差共分散行列“R”を設定する(ステップB13)。そして、処理部10は、ステップB11及びB13で設定した観測量ベクトル“Z”及び観測誤差共分散行列“R”を用いて、カルマンフィルター処理を実行する(ステップB7)。
【0117】
一方、移動状況が「停止」である場合は(ステップB9;停止)、処理部10は、停止時制約速度ベクトルを観測量ベクトル“Z”として設定する(ステップB15)。また、停止時用速度観測誤差(例えば所定値)に基づき観測誤差共分散行列“R”を設定する(ステップB17)。そして、処理部10は、ステップB15及びB17で設定した観測量ベクトル“Z”及び観測誤差共分散行列“R”を用いて、カルマンフィルター処理を実行する(ステップB7)。
【0118】
ステップB7においてカルマンフィルター処理を行うと、処理部10は、その結果をカップリング結果59として記憶部50に記憶させる。そして、処理部10は、カップリング処理を終了する。
【0119】
図14の第1のナビゲーション処理に戻り、カップリング処理を行った後、処理部10は、カップリング結果59を出力する(ステップA13)。例えば、カップリング結果59として得られた位置(カップリング位置)に対してマップマッチング処理を行い、その結果で表示部30のナビゲーション画面の表示を更新する。
【0120】
次いで、処理部10は、処理を終了するか否かを判定する(ステップA15)。例えば、操作部20を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、ナビゲーション処理を終了すると判定する。まだ処理を終了しないと判定した場合は(ステップA15;No)、処理部10は、ステップA3に戻る。また、処理を終了すると判定した場合は(ステップA15;Yes)、第1のナビゲーション処理を終了する。
【0121】
3.作用効果
GPSユニット3とINSユニット5とを備えた位置算出装置1において、影響度合設定部7は、位置算出を開始してから所与の条件が成立するまでの間は、GPS計測結果のINS計測結果に対する影響度合を第1の度合に設定し、所与の条件が成立した後は、影響度合を第1の度合よりも低い第2の度合に設定する。そして、カップリング処理部9は、影響度合設定部7により設定された影響度合に基づいてGPS計測結果とINS計測結果とのカップリング処理を実行して、移動体の位置を算出する。
【0122】
例えば、第1の位置算出装置1Aでは、影響度合設定部7が影響モード設定部7Aを有する。そして、(1)GPSの初回演算精度、(2)位置算出開始からの経過時間又は位置算出回数、に基づき定められた第1のモード設定条件と、カップリング結果の精度に基づき定められた第2のモード設定条件とを用いて、GPSの影響度合を決定付けるGPS影響モードを設定する。
【0123】
また、第1の位置算出装置1Aでは、カップリング処理部9がカルマンフィルター処理部9Aを有し、影響モード設定部7Aより設定されたGPS影響モードに応じて演算設定を変えてカルマンフィルター処理を行う。演算設定には、GPS計測結果を採用してカップリング処理を実行する実行頻度が含まれ、低影響モードでは、高影響モードと比べて実行頻度が低く設定される。また、演算設定には、観測量とするGPS計測結果の想定誤差である観測誤差(R値)が含まれ、低影響モードでは、高影響モードと比べて大きい値が設定される。
【0124】
このように、所与の条件が成立するまでの間は、衛星測位用ユニットの計測結果の慣性測位用ユニットの計測結果に対する影響度合を高く設定し、所与の条件が成立した後は、その影響度合を低く設定することで、特に位置算出装置の初期動作時における影響度合を適正化することができる。そして、影響度合に応じてカップリング処理の演算設定を変更することで、カップリング処理の実効性を高め、ひいては位置算出をより正確に行うことが可能となる。
【0125】
4.変形例
本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明するが、上記の実施例と同一の構成要素については同一の符号を付して説明を省略し、上記の実施例とは異なる部分を中心に説明する。
【0126】
4−1.ユニット
上記の実施形態では、衛星測位用ユニットとして、GPSを適用したGPSユニットを例に挙げて説明したが、WAAS(Wide Area Augmentation System)、QZSS(Quasi Zenith Satellite System)、GLONASS(GLObal NAvigation Satellite System)、GALILEO等の他の衛星測位システムを適用したユニットとしてもよいことは勿論である。
【0127】
また、上記の実施形態では、慣性測位用ユニットとして、INSユニットを例に挙げて説明したが、INSメジャメント情報(加速度や角速度)を計測する慣性センサーや慣性計測ユニット(IMU)を慣性測位用ユニットとしてもよい。この場合は、慣性測位用ユニットにより計測されたINSメジャメント情報を用いて、位置算出装置の処理部がINS演算処理を行うように構成すればよい。
【0128】
4−2.カップリング処理
上記の実施形態では、カップリング処理の一例としてカルマンフィルター処理を例に挙げて説明したが、カップリング処理はこれに限られない。例えば、GPS計測結果とINS計測結果との平均演算を行う平均処理をカップリング処理に含めてもよい。
【0129】
平均演算としては、単純な算術平均や幾何平均を適用してもよいし、加重平均を適用してもよい。算術平均や幾何平均を適用するのであれば、カップリング処理の演算設定として、例えば、GPS計測結果を採用して平均処理を実行する実行頻度を定めておくことができる。高影響モードでは、GPS計測結果をINS計測結果に作用し易くするために実行頻度を高くし、低影響モードでは、GPS計測結果をINS計測結果に作用しにくくするために実行頻度を低くすればよい。
【0130】
また、加重平均を適用するのであれば、カップリング処理の演算設定として、例えば加重平均の重みを定めておくことができる。高影響モードでは、GPS計測結果をINS計測結果よりも重視して加重平均するようにGPS計測結果の重みを大きく設定し、低影響モードでは、INS計測結果をGPS計測結果よりも重視して加重平均するようにGPS計測結果の重みを小さく設定すればよい。
【0131】
4−3.影響モードの設定方法
上記の実施形態では、第1のモード設定条件と第2のモード設定条件とを併用してGPS影響モードを設定することとして説明したが、第1のモード設定条件を単体で用いてGPS影響モードを設定することとしてもよいし、第2のモード設定条件を単体で用いてGPS影響モードを設定することとしてもよい。
【0132】
また、上記の実施形態では、第1のモード設定条件を、(1)GPS初回演算精度、(2)位置算出開始からの経過時間又は位置算出回数、を組み合わせた条件として定義したが、(1)及び(2)を個別の条件として定義してもよい。すなわち、(1)GPS初回演算精度に基づく条件を単体で用いてGPS影響モードを設定してもよいし、(2)位置算出開始からの経過時間又は位置算出回数に基づく条件を単体で用いてGPS影響モードを設定してもよい。
【0133】
4−4.処理主体
上記の実施例では、電子機器の処理部10が、GPSユニット3から取得したGPSメジャメント情報55を用いてGPS演算処理を行うこととして説明した。また、処理部10が、INSユニット5から取得したINSメジャメント情報56を用いてINS演算処理を行うこととして説明した。つまり、GPS演算処理、INS演算処理及びカップリング処理の実行主体が、全て電子機器の処理部10であるものとして説明した。この構成を次のようにしてもよい。
【0134】
GPSユニット3は、GPSメジャメント情報55を用いたGPS演算処理を行ってGPS演算結果57を求め、処理部10に出力する。また、INSユニット5は、INSメジャメント情報56を用いたINS演算処理を行ってINS演算結果58を求め、処理部10に出力する。
【0135】
そして、処理部10は、各ユニットから取得したGPS演算結果57とINS演算結果58とのカップリング処理を実行する。つまり、この場合は、GPS演算処理及びINS演算処理の実行主体が、それぞれGPSユニット3及びINSユニット5となり、カップリング処理の実行主体が、電子機器の処理部10となる。
【0136】
4−5.位置算出方法
設定されている影響モード(影響度合)に見合った位置算出の方式を適用して移動体の位置を算出することとしてもよい。例えば、GPS影響モードが高影響モードの場合は、GPS計測結果を用いた所定の位置算出処理を実行して位置を算出し、GPS影響モードが低影響モードの場合は、GPS計測結果及びINS計測結果を用いたカップリング処理を実行して位置を算出することとしてもよい。
【0137】
図16は、この場合に処理部10が図14の第1のナビゲーション処理に代えて実行する第2のナビゲーション処理の流れを示すフローチャートである。処理部10は、ステップA9において影響モード設定処理を行った後、影響モード設定処理で設定したモード(設定モード54)を判定する(ステップC10)。
【0138】
設定モード54が高影響モードであると判定した場合は(ステップC10;高影響モード)、処理部10は、GPS演算結果フィルターリング処理を行う(ステップC17)。そして、処理部10は、フィルターリング結果を出力する(ステップC19)。
【0139】
一方、設定モード54が低影響モードであると判定した場合は(ステップC10;低影響モード)、処理部10は、図15で説明したカップリング処理を行う(ステップA11)。そして、処理部10は、カップリング結果59を出力する(ステップA13)。
【0140】
ステップC17のGPS演算結果フィルターリング処理は、ステップA5のGPS演算処理で求めたGPS演算結果をフィルターリングすることで、より確からしい位置を算出する位置算出処理である。この場合のフィルターリング方式としては、種々の方式を適用可能である。例えば、カルマンフィルター処理を適用してもよいし、GPS演算結果の過去の履歴に基づくフィルターリング処理を適用してもよい。
【0141】
カルマンフィルター処理を適用するのであれば、例えば、GPS演算結果を制御入力“U”とし、上記の実施形態で説明した移動時速度制約条件や停止時速度制約条件を観測量“Z”として、移動体の位置や速度等の状態“X”を推定する手法が適用可能である。
【0142】
また、GPS演算結果の過去の履歴に基づくフィルターリング処理を適用するのであれば、例えば、過去所定期間分(例えば過去10秒間分)のGPS演算結果を平均化するなどして、移動体の位置や速度等を推定する手法が適用可能である。
【0143】
4−6.電子機器
上記の実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【0144】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、PDA(Personal Digital Assistant)といった他の電子機器についても本発明を同様に適用して、当該電子機器の位置算出を実現することが可能である。
【符号の説明】
【0145】
1、1A 位置算出装置、 3 GPSユニット、 5 INSユニット、 7 影響度合設定部、 7A 影響モード設定部、 9 カップリング処理部、 9A カルマンフィルター処理部、 10 処理部、 20 操作部、 30 表示部、 40 通信部、 50 記憶部、 100 カーナビゲーション装置、 1000 ナビゲーションシステム
【技術分野】
【0001】
本発明は、移動体に備えられた衛星測位用ユニット及び慣性測位用ユニットの計測結果を用いた位置算出方法等に関する。
【背景技術】
【0002】
いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、慣性センサーの活用が注目されている。慣性センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。慣性センサーの検出結果を利用して慣性航法演算を行う慣性航法システム(以下、「INS(Inertial Navigation System)」と称する。)も考案されている。
【0003】
INSでは、慣性センサーの検出結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献1には、INS演算結果を、GPS(Global Positioning System)を利用して補正する技術が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】米国特許出願公開第2010/0019963号明細書
【発明の概要】
【発明が解決しようとする課題】
【0005】
GPSを利用してINS演算結果を補正する技術は、GPS演算結果が正しいことを前提としている。特許文献1に開示された技術も同様である。しかし、GPS演算結果は、GPS衛星から受信したGPS衛星信号の信号強度、受信環境、GPS衛星の天空配置、マルチパスといった種々の要因により、演算結果の精度が低下する場合がある。このため、常にGPS演算結果を利用してINS演算結果を補正することが、必ずしも位置算出の精度改善に繋がるとは限らなかった。
【0006】
本発明は上述した課題に鑑みて為されたものであり、移動体に備えられた衛星測位用ユニット及び慣性測位用ユニットの計測結果を併用して、移動体の位置をより正確に算出するための手法を提案することを目的とする。
【課題を解決するための手段】
【0007】
以上の課題を解決するための第1の形態は、移動体に備えられた衛星測位用ユニットの第1の計測結果と、前記移動体に備えられた慣性測位用ユニットの第2の計測結果とを用いて、前記移動体の位置を算出する位置算出方法であって、位置算出を開始してから所与の条件が成立するまでの間は、前記第1の計測結果の前記第2の計測結果に対する影響度合を第1の度合に設定し、前記条件が成立した後は、前記影響度合を前記第1の度合よりも低い第2の度合に設定することと、前記影響度合に基づき、前記第1の計測結果と前記第2の計測結果とのカップリング処理を実行して前記移動体の位置を算出することと、を含む位置算出方法である。
【0008】
また、他の形態として、移動体に備えられた衛星測位用ユニットの第1の計測結果と、前記移動体に備えられた慣性測位用ユニットの第2の計測結果とを用いて、前記移動体の位置を算出する位置算出装置であって、位置算出を開始してから所与の条件が成立するまでの間は、前記第1の計測結果の前記第2の計測結果に対する影響度合を第1の度合に設定し、前記条件が成立した後は、前記影響度合を前記第1の度合よりも低い第2の度合に設定する影響度合設定部と、前記影響度合に基づき、前記第1の計測結果と前記第2の計測結果とのカップリング処理を実行して前記移動体の位置を算出するカップリング処理部と、を備えた位置算出装置を構成してもよい。
【0009】
この第1の形態等によれば、位置算出を開始してから所与の条件が成立するまでの間は、第1の計測結果の第2の計測結果に対する影響度合を第1の度合に設定する。所与の条件が成立した後は、影響度合を第1の度合よりも低い第2の度合に設定する。つまり、所与の条件の成立前後で、慣性測位用ユニットの計測結果に対する衛星測位用ユニットの計測結果の影響度合を変更する。これにより、特に初期動作時における影響度合の適正化を実現することができる。適正化した影響度合に基づき第1の計測結果と第2の計測結果とのカップリング処理を実行することで、位置算出の正確性を向上させることができる。
【0010】
また、第2の形態として、第1の形態の位置算出方法において、前記影響度合には、前記第1の計測結果を採用して前記カップリング処理を実行する実行頻度が含まれており、前記第1の度合に設定することは、前記実行頻度を第1の頻度に設定することを含み、前記第2の度合に設定することは、前記実行頻度を前記第1の頻度よりも低い第2の頻度に設定することを含み、前記位置を算出することは、前記実行頻度に従って前記第1の計測結果を採用した前記カップリング処理を実行することを含む、位置算出方法を構成することとしてもよい。
【0011】
この第2の形態によれば、所与の条件が成立するまでの間は、第1の計測結果を採用してカップリング処理を実行する実行頻度を第1の頻度に設定する。所与の条件が成立した後は、実行頻度を第1の頻度よりも低い第2の頻度に設定する。つまり、所与の条件が成立した後は、第1の計測結果をカップリング処理に採用する頻度を下げて、第1の計測結果の第2の計測結果に対する影響度合を低くする。これにより、衛星測位用ユニットの計測結果をカップリング処理に採用する頻度を適正化し、カップリング処理の実効性を高めることができる。
【0012】
また、第3の形態として、第1の形態の位置算出方法において、前記カップリング処理は、前記第1の計測結果を観測量とするカルマンフィルター処理を含み、前記影響度合には、前記カルマンフィルター処理に用いる誤差パラメーター値が含まれており、前記第1の度合に設定することは、前記誤差パラメーター値を第1のパラメーター値に設定することを含み、前記第2の度合に設定することは、前記誤差パラメーター値を前記第1のパラメーター値よりも大きい第2のパラメーター値に設定することを含み、前記位置を算出することは、前記第1の計測結果及び前記誤差パラメーター値を用いて前記カルマンフィルター処理を実行することを含む、位置算出方法を構成することとしてもよい。
【0013】
この第3の形態によれば、所与の条件が成立するまでの間は、カルマンフィルター処理に用いる誤差パラメーター値を第1のパラメーター値に設定する。所与の条件が成立した後は、誤差パラメーター値を第1のパラメーター値よりも大きい第2のパラメーター値に設定する。誤差パラメーター値は、例えば、カルマンフィルター処理において第1の計測結果を重視する程度を決定付ける値である。誤差パラメーター値を大きく設定することは、第1の計測結果の第2の計測結果に対する影響度合を下げることに相当する。誤差パラメーター値を適正化することで、算出される移動体の位置の正確性が向上する。
【0014】
また、第4の形態として、第1の形態の位置算出方法において、前記位置を算出することは、前記影響度合が前記第1の度合の場合に前記第1の計測結果を用いた所定の位置算出処理を実行して位置を算出し、前記影響度合が前記第2の度合の場合に前記カップリング処理を実行して位置を算出することである、位置算出方法を構成することとしてもよい。
【0015】
この第4の形態によれば、影響度合が第1の度合の場合は、第1の計測結果を用いた所定の位置算出処理を実行して位置を算出する。他方、影響度合が第2の度合の場合は、カップリング処理を実行して位置を算出する。これにより、影響度合に見合った位置算出の方式を適用して移動体の位置を算出することが可能となる。
【0016】
また、第5の形態として、第1〜第3の何れかの形態の位置算出方法において、前記影響度合を設定することは、位置算出を開始してからの経過時間又は位置算出回数が、前記位置算出の結果の精度が安定するための時間的条件として定められた精度安定条件を満たす場合に、前記所与の条件が成立したと判定することを含む、位置算出方法を構成することとしてもよい。
【0017】
この第5の形態によれば、位置算出を開始してからの経過時間又は位置算出回数が、位置算出の結果の精度が安定するための時間的条件として定められた精度安定条件を満たす場合に、第1の計測結果の第2の計測結果に対する影響度合を、第1の度合から第2の度合に低下させる。これにより、位置算出の結果の精度が安定したと判断した後は、衛星測位用ユニットの計測結果への依存度を下げて位置算出を行うことが可能となる。
【0018】
また、第6の形態として、第1〜第3の何れかの形態の位置算出方法において、前記位置を算出することは、位置算出の開始時に、前記第1の計測結果から求められた位置を以降の位置算出の基準位置とすることを含み、前記影響度合を設定することは、位置算出の開始時の前記第1の計測結果が所定の精度良好条件を満たす場合に、前記所与の条件が成立したと判定することを含む、位置算出方法を構成することとしてもよい。
【0019】
この第6の形態によれば、位置算出の開始時に、第1の計測結果から求められた位置を基準位置として以降の位置算出を行う。つまり、第1の計測結果の精度が良好であれば、真の位置に近い位置を基準位置として以降の位置算出を行うことができる。そこで、位置算出の開始時の第1の計測結果が所定の精度良好条件を満たす場合には、影響度合を第1の度合から第2の度合に低下させる。これにより、信頼性の高い基準位置が得られたと判断できる場合には、衛星測位用ユニットの計測結果への依存度を下げて位置算出を行うことが可能となる。
【0020】
また、第7の形態として、第1〜第3の何れかの形態の位置算出方法において、前記影響度合を設定することは、前記カップリング処理の結果が所定の精度良好条件を満たす場合に、前記所与の条件が成立したと判定することを含む、位置算出方法を構成することとしてもよい。
【0021】
この第7の形態によれば、カップリング処理の結果が所定の精度良好条件を満たす場合は、影響度合を第1の度合から第2の度合に低下させる。これにより、カップリング処理の結果の精度が良好であると判断できる場合には、衛星測位用ユニットの計測結果への依存度を下げて位置算出を行うことが可能となる。
【図面の簡単な説明】
【0022】
【図1】位置算出装置の主要構成図。
【図2】第1の位置算出装置の構成図。
【図3】カップリング処理の入出力データの説明図。
【図4】第1のモード設定条件の説明図。
【図5】第2のモード設定条件の説明図。
【図6】影響モード設定処理の流れを示すフローチャート。
【図7】位置算出を行った実験結果の一例を示す図。
【図8】位置算出を行った実験結果の一例を示す図。
【図9】位置算出を行った実験結果の一例を示す図。
【図10】位置算出を行った実験結果の一例を示す図。
【図11】ナビゲーションシステムのシステム構成図。
【図12】カーナビゲーション装置の機能構成を示すブロック図。
【図13】演算設定テーブルのテーブル構成図。
【図14】第1のナビゲーション処理の流れを示すフローチャート。
【図15】カップリング処理の流れを示すフローチャート。
【図16】第2のナビゲーション処理の流れを示すフローチャート。
【発明を実施するための形態】
【0023】
以下、図面を参照して、本発明の好適な実施形態の一例について説明する。本実施形態は、衛星測位システムの一種であるGPS(Global Positioning System)と、慣性航法演算を行うシステムであるINS(Inertial Navigation System)とを併用して位置算出を行う実施形態である。
【0024】
1.原理
1−1.構成
図1は、本実施形態における位置算出装置1の主要構成図である。位置算出装置1は、移動体に備えられて、当該移動体の位置を算出する装置(位置算出システム)である。移動体は、自動車やオートバイ、自転車、船、電車といった物の他、人間自体であってもよい。人間が位置算出装置1を携帯し、人間自身が位置算出装置1を備えることとしてもよい。図1では、ユニット(モジュール)を二重線で図示し、ユニットの計測結果を利用して演算処理を行う処理ブロックを一重線で図示することで、両者を区別する。図2についても同様である。
【0025】
位置算出装置1は、ユニット(モジュール)として、GPSユニット3と、INSユニット5とを備える。また、位置算出装置1は、主要な処理ブロックとして、影響度合設定部7と、カップリング処理部9とを有する。
【0026】
GPSユニット3は、衛星測位システムを利用して測位を行うためのユニット(衛星測位用ユニット)である。GPSユニット3は、GPS衛星から発信されているGPS衛星信号を受信するアンテナや、受信したGPS衛星信号を処理するプロセッサー等を有して構成される。
【0027】
GPSユニット3は、GPS衛星信号のコード位相やドップラー周波数、擬似距離、擬似距離変化率等のGPSメジャメント情報を計測可能に構成されている。また、GPSユニット3は、GPSメジャメント情報を用いたGPS演算を行って、移動体の位置や速度(速度ベクトル)等を計測可能に構成されている。GPSメジャメント情報やGPS演算結果は、GPS計測結果(第1の計測結果)としてカップリング処理部9に出力される。
【0028】
INSユニット5は、慣性航法を利用して測位を行うためのユニット(慣性測位用ユニット)である。INSユニット5は、加速度センサーやジャイロセンサー等の慣性センサーや、慣性センサーをパッケージ化した慣性計測ユニット(IMU(Inertial Measurement Unit))、慣性センサーの計測結果を処理するプロセッサー等を有して構成される。
【0029】
INSユニット5は、慣性センサーの計測結果を用いて、移動体の加速度(加速度ベクトル)や角速度等をINSメジャメント情報として計測可能に構成されている。また、INSユニット5は、INSメジャメント情報を用いた慣性航法演算(INS演算)を行って、移動体の位置や速度(速度ベクトル)、姿勢角等を計測可能に構成されている。INSメジャメント情報やINS演算結果は、INS計測結果(第2の計測結果)としてカップリング処理部9に出力される。
【0030】
影響度合設定部7は、GPS計測結果(第1の計測結果)のINS計測結果(第2の計測結果)に対する影響度合を設定する。影響度合設定部7は、位置算出を開始してから所与の条件が成立したか否かを判定する。そして、所与の条件が成立するまでの間は、GPS計測結果のINS計測結果に対する影響度合を第1の度合に設定し、条件が成立した後は、影響度合を第1の度合よりも低い第2の度合に設定する。
【0031】
カップリング処理部9は、影響度合設定部7により設定された影響度合に基づいて、GPS計測結果(第1の計測結果)とINS計測結果(第2の計測結果)とのカップリング処理を実行して移動体の位置等を算出する。
【0032】
図2は、図1の位置算出装置1を適用した第1の位置算出装置1Aの構成図である。第1の位置算出装置1Aでは、図1の影響度合設定部7が影響モード設定部7Aを有し、カップリング処理部9がカルマンフィルター処理部9Aを有する。
【0033】
影響モード設定部7Aは、後述するモード設定条件に従ってGPS影響モードを設定する。GPS影響モードは、GPSの影響度合を決定づけるモードである。本実施形態では、GPS計測結果のINS計測結果に対する影響度合が比較的高いモードである「高影響モード」と、GPS計測結果のINS計測結果に対する影響度合が比較的低いモードである「低影響モード」との2種類のモードを択一的に設定する場合を想定する。なお、初期設定では、高影響モードを設定することとする。
【0034】
カルマンフィルター処理部9Aは、GPS計測結果を観測量“Z”とするカルマンフィルター処理を実行して、GPS計測結果とINS計測結果とをカップリング(結び付け)する。具体的には、カルマンフィルターの理論に基づき、予測演算(時刻更新)と補正演算(観測更新)とを行って、移動体の状態“X”を推定する。
【0035】
本実施形態において、移動体の状態“X”には、少なくとも移動体の位置が含まれる。予測演算では、例えば、INSユニット5から入力したINS計測結果を制御入力“U”として、1つ前の時刻(前回時刻)における状態補正値“X+”から、現在の時刻(今回時刻)における状態“X”を予測する演算を行って、状態予測値“X−”を算出する。
【0036】
補正演算では、例えば、GPSユニット3から入力したGPS計測結果を観測量“Z”として用いて、予測演算で算出した状態予測値“X−”を補正する演算を行って、状態補正値“X+”を算出する。そして、算出した状態補正値“X+”を、カップリング結果として出力する。
【0037】
また、カルマンフィルター処理部9Aは、GPS計測結果とは別に、移動体の運動モデルに基づく制約条件を観測量“Z”として適用可能に構成されている。本実施形態では、移動体の停止時における速度制約条件である「停止時速度制約条件」と、移動体の移動時における速度制約条件である「移動時速度制約条件」との2種類の速度制約条件を適用する場合を想定する。
【0038】
停止時速度制約条件(第1の制約条件)は、移動体の停止時に適用可能な制約条件である。移動体が停止しているのであれば、理想的には移動体の速度はゼロである。従って、移動体が停止していると判定した場合は「移動体の各軸の速度成分=0(速度ベクトル=ゼロベクトル)」を観測量“Z”として与えることができる。
【0039】
移動時速度制約条件(第2の制約条件)は、移動体の移動時に適用可能な制約条件である。例えば、移動体として四輪自動車を想定した場合、通常、四輪自動車はジャンプや横滑りすることはないと仮定することができる。従って、移動体が移動していると判定した場合は「移動体の縦横方向の速度成分=0」を観測量“Z”として与えることができる。
【0040】
本実施形態では、GPS計測結果と上記の制約条件(停止時速度制約条件又は移動時速度制約条件)とを切り替えて観測量“Z”に適用する場合を一例として説明する。なお、本実施形態とは異なり、GPS計測結果と上記の制約条件とを観測量“Z”として併用することも当然に可能である。
【0041】
図3は、カルマンフィルター処理の入出力データの説明図である。制御入力“U”と、観測量“Z”と、状態“X”との対応関係を表したテーブルを図示している。カップリングには種々の方式が存在する。その中でも、ルーズカップリング(疎結合)と呼ばれる方式と、タイトカップリング(密結合)と呼ばれる方式とが一般的に用いられる。
【0042】
ルーズカップリング方式は、GPSとINSとの結び付きが比較的弱いカップリング方式である。この方式では、例えば、制御入力“U”をINS演算結果(位置、速度、姿勢角等)とし、観測量“Z”をGPS演算結果(位置、速度等)としてカップリング処理を実行する。そして、移動体情報(位置、速度、姿勢角等)を状態“X”として推定する。
【0043】
タイトカップリング方式は、GPSとINSとの結び付きが比較的強いカップリング方式である。この方式では、例えば、制御入力“U”をINS演算結果(位置、速度、姿勢角等)とし、観測量“Z”をGPSメジャメント情報(コード位相、ドップラー周波数、擬似距離、擬似距離変化率等)としてカップリング処理を実行する。そして、移動体情報(位置、速度、姿勢角等)を状態“X”として推定する。
【0044】
また、タイトカップリング方式として、制御入力“U”をINSメジャメント情報(加速度、角速度等)とし、観測量“Z”をGPSメジャメント情報(コード位相、ドップラー周波数、擬似距離、擬似距離変化率等)とし、状態“X”を移動体情報(位置、速度、姿勢角等)とする手法も存在する。
【0045】
本実施形態の位置算出方法は、上記の何れのカップリング方式に対しても実質的に同一に適用可能である。つまり、GPS計測結果(第1の計測結果)として、GPSメジャメント情報を適用してもよいし、GPS演算結果を適用してもよい。また、INS計測結果(第2の計測結果)として、INSメジャメント情報を適用してもよいし、INS演算結果を適用してもよい。
【0046】
INS計測結果及びGPS計測結果として用いる情報は、適用するシステムに応じて適宜設定可能である。この場合、カルマンフィルター処理の予測演算及び補正演算で用いる演算式やパラメーター値を、適用するシステムに応じて適宜設定すればよい。なお、具体的な演算式やパラメーター値については、公知の手法に基づき定義可能であるため、ここでは説明を省略する。
【0047】
1−2.GPS影響モードの設定
図4は、GPS影響モードの設定に用いる第1のモード設定条件の説明図であり、第1のモード設定条件を定めた第1のモード設定条件テーブルを図示したものである。第1のモード設定条件テーブルには、第1のモード設定条件と、設定モードとが対応付けて定められている。
【0048】
第1のモード設定条件は、(1)GPS初回演算精度、(2)位置算出を開始してからの経過時間又は位置算出回数、に基づいて定められた条件である。GPS初回演算精度は、GPSユニット3の初回の演算結果の精度である。
【0049】
1番目の条件は「GPS初回演算精度=良好」である。そして、この条件を満たす場合の設定モードとして「低影響モード」が定められている。つまり、「GPS初回演算精度=良好」の場合は、GPS影響モードを初期設定である「高影響モード」から「低影響モード」に切り替える。この1番目の条件は、位置算出の開始時のGPS計測結果(第1の計測結果)が所定の精度良好条件を満たすことに相当する。そして、精度良好条件を満たす場合には、GPS計測結果のINS計測結果に対する影響度合を、初期設定である第1の度合(高影響モード)から、第1の度合よりも低い第2の度合(低影響モード)に下げることに相当する。
【0050】
2番目の条件は「GPS初回演算精度=非良好」である。この2番目の条件は、さらに位置算出開始からの経過時間又は位置算出回数に応じて分けられている。経過時間≦精度安定時間“θT”、又は、位置算出回数≦精度安定回数“θC”、の条件を満たす場合の設定モードとして「高影響モード」が定められている。また、経過時間>精度安定時間“θT”、又は、位置算出回数>精度安定回数“θC”、の条件を満たす場合の設定モードとして「低影響モード」が定められている。つまり、「GPS初回演算精度=非良好」の場合は、GPS影響モードを初期設定である「高影響モード」のままとし、ある程度時間が経過した時点で「低影響モード」に切り替える。
【0051】
位置算出開始からの経過時間が所定の精度安定時間“θT”を超えること、又は、位置算出回数が所定の精度安定回数“θC”を超えることは、位置算出を開始してからの経過時間又は位置算出回数が、位置算出の結果の精度が安定するための時間的条件として定められた精度安定条件を満たすことに相当する。そして、この精度良好条件を満たす場合には、GPS計測結果のINS計測結果に対する影響度合を、当初設定した第1の度合(高影響モード)から、第1の度合よりも低い第2の度合(低影響モード)に下げることに相当する。
【0052】
なお、精度安定時間“θT”や精度安定回数“θC”は、本実施形態の位置算出方法を適用するシステムに応じて適宜設定可能である。例えば、位置算出を「1秒間隔」で行うシステムに適用するのであれば、“θT”として「30秒〜60秒」の範囲の秒数、“θC”として「30回〜60回」の範囲の回数を設定すると効果的である。
【0053】
GPS演算では、各GPS衛星それぞれについて観測した擬似距離を利用して、例えば最小二乗法を適用した測位計算を行って移動体の位置を算出する。また、受信したGPS衛星信号の受信周波数の誤差(搬送波周波数からの周波数ズレ)に基づいてGPS衛星と移動体との相対速度(相対速度ベクトル)を算出し、これを用いて移動体の速度(速度ベクトル)を算出する。この際、公知の誤差推定演算を行うことで、算出した位置や速度(速度ベクトル)に含まれ得る最大の誤差を推定することができる。
【0054】
そこで、上記の誤差推定演算で推定される誤差に対する閾値を予め定めておく。例えば、位置誤差の閾値として“10[m]”、速度誤差の閾値として“1[m/s]”を定めておく。そして、位置誤差及び速度誤差に対する閾値判定をそれぞれ行って、GPS初回演算精度を判定する。この場合には、それぞれの閾値判定結果に対してAND条件を適用してGPS初回演算精度を判定してもよいし、OR条件を適用してGPS初回演算精度を判定してもよい。
【0055】
GPSの初回演算精度が良好であるにも関わらずGPS影響モードを「低影響モード」に設定することや、GPSの初回演算精度が非良好であるにも関わらずGPS影響モードを「高影響モード」に設定することは、一見矛盾しているようにも思われる。しかし、敢えてこのような設定方法を採用したのには理由がある。
【0056】
慣性航法演算を利用した位置算出を行う際には、基準位置や基準方位が必要となる。そのため、例えば、位置算出の開始時にGPS演算処理を行い、得られた位置及び方位を以降の位置算出の基準位置及び基準方位とすることが、1つの手法として考えられる。
【0057】
GPS初回演算精度が良好であれば、精度の高い位置や方位を基準として以降の位置算出を行うことができる。そのため、GPSの影響度合を下げてINSに依存して位置算出を行っても、暫くの間は、比較的精度の高い安定した演算結果を得ることができる。
【0058】
ところが、GPS初回演算精度が良好でない場合は、精度の低い位置や方位を基準として以降の位置算出を行わなければならなくなる。つまり、当初から誤差が重畳された状態で位置算出を開始することになる。この場合、GPSの影響度合を下げてINSに依存してしまうと、当初の誤差分がいつまで経っても補正されなくなる。
【0059】
そこで、GPSの初回演算精度が良好である場合は、GPS影響モードを「低影響モード」に設定し、GPSの初回演算精度が非良好である場合は、GPS影響モードを「高影響モード」に設定する。GPS影響モードを「高影響モード」に設定した状態で位置算出を行っていくと、GPSの演算精度が改善されるのに伴い、当初の誤差分が次第に補正されていく。そこで、ある程度の時間が経過した時点で、GPS影響モードを「高影響モード」から「低影響モード」に切り替えることで、GPSへの依存度を低くする。
【0060】
図5は、GPS影響モードの設定に用いる第2のモード設定条件の説明図であり、第2のモード設定条件を定めた第2のモード設定条件テーブルを図示したものである。第2のモード設定条件テーブルには、第2のモード設定条件と、設定モードとが対応付けて定められている。
【0061】
第2のモード設定条件は、カップリング処理の結果に基づき定義される条件である。具体的には、1番目の条件として「カップリング結果精度=良好」が定められている。そして、この1番目の条件を満たす場合の設定モードとして「低影響モード」が定められている。また、2番目の条件として「カップリング結果精度=非良好」が定められている。そして、この2番目の条件を満たす場合の設定モードとして「高影響モード」が定められている。
【0062】
カップリング結果精度は、カップリング処理を行うことで得られた移動体の位置等の精度のことを意味する。例えば、カルマンフィルター処理では、推定対象とする状態“X”の誤差“P”を設定して演算を行う。予測演算及び補正演算において、状態“X”と併せて誤差“P”の予測及び補正を行うことで、状態“X”の精度が定まる。従って、カルマンフィルター処理で用いる誤差“P”を、カップリング結果精度として利用することができる。
【0063】
状態“X”が複数の要素で構成される場合は、誤差“P”は行列形式の誤差共分散行列“P”で表すことができる。この場合、誤差共分散行列“P”の対角成分から、各要素の精度を見積もることができる。例えば、状態“X”の要素が移動体の位置、速度及び姿勢角であれば、誤差共分散行列“P”の対角成分が、それぞれカップリング位置誤差、カップリング速度誤差及びカップリング姿勢角誤差となる。
【0064】
そこで、カップリング位置誤差、カップリング速度誤差及びカップリング姿勢角誤差に対する閾値を予め定めておく。例えば、カップリング位置誤差の閾値として“10[m]”、カップリング速度誤差の閾値として“1[m/s]”、カップリング姿勢角誤差の閾値として“1[°]”を定めておく。そして、それぞれに対する閾値判定を行って、カップリング結果精度を判定する。この場合には、それぞれの閾値判定結果に対してAND条件を適用してカップリング結果精度を判定してもよいし、OR条件を適用してカップリング結果精度を判定してもよい。
【0065】
図6は、本実施形態における影響モード設定処理の流れを示すフローチャートである。最初に、影響モード設定部7Aは、図4の第1のモード設定条件テーブルを参照して、第1のモード設定条件の成否を判定する(ステップS1)。その判定結果(第1の判定結果)が高影響モードであれば(ステップS3;高影響モード)、影響モード設定部7Aは、図5の第2のモード設定条件テーブルを参照して、第2のモード設定条件の成否を判定する(ステップS5)。
【0066】
ステップS5における判定結果(第2の判定結果)が高影響モードであれば(ステップS7;高影響モード)、影響モード設定部7Aは、GPS影響モードを高影響モードに設定する(ステップS9)。一方、ステップS1における第1の判定結果が低影響モードである場合(ステップS3;低影響モード)、又は、ステップS5における第2の判定結果が低影響モードである場合は(ステップS7;低影響モード)、影響モード設定部7Aは、GPS影響モードを低影響モードに設定する(ステップS11)。そして、影響モード設定処理を終了する。
【0067】
この影響モード設定処理では、第1の判定結果及び第2の判定結果が両方とも高影響モードである場合に限り、GPS影響モードを高影響モードに設定することにしている。これには、なるべく早い段階でGPSの影響度合を下げて位置算出を行おうとする狙いがある。
【0068】
1−3.演算設定
カップリング処理部9は、GPS影響モードに応じて演算設定を変更してカップリング処理を行う。本実施形態において、演算設定には、GPS計測結果採用頻度と、カルマンフィルター処理で用いる誤差パラメーター値とが含まれる。
【0069】
GPS計測結果採用頻度は、GPS計測結果を採用してカップリング処理を実行する実行頻度である。図2では、カルマンフィルター処理部9Aが、観測量“Z”であるGPS計測結果を採用して補正演算を実行する頻度が、GPS計測結果採用頻度に相当する。
【0070】
GPS影響モードのモード設定が「高影響モード」である場合は、GPS計測結果をINS計測結果に対して強く作用させることが必要となる。そこで、GPS計測結果採用頻度を、比較的高い第1の頻度に設定する。それに対し、GPS影響モードのモード設定が「低影響モード」である場合は、GPS計測結果のINS計測結果に対する作用を弱める必要がある。そこで、GPS計測結果採用頻度を、第1の頻度よりも低い第2の頻度に設定する。第2の頻度は第1の頻度よりも低い頻度であればよく、その具体的な値は適宜設定可能である。
【0071】
誤差パラメーター値は、カルマンフィルターの演算において設定するパラメーター値の一種である。本実施形態では、観測量“Z”の誤差に相当する観測誤差(観測雑音)“R”を、誤差パラメーターの一例として説明する。
【0072】
カルマンフィルター処理の補正演算において、観測誤差“R”を小さく設定すると、観測量“Z”に追従するように状態“X”が補正される。つまり、観測量“Z”を信用・重視して状態“X”を推定するようにフィルターが作用する。それに対して、観測誤差“R”を大きく設定すると、状態予測値“X−”に追従するように状態“X”が補正される。つまり、予測演算で予測された状態予測値“X−”を信用・重視して状態“X”を推定するようにフィルターが作用する。
【0073】
このことから、GPS影響モードのモード設定が「高影響モード」である場合は、GPS計測結果をINS計測結果に強く作用させるために、観測誤差“R”を比較的小さい第1のパラメーター値に設定する。それに対し、GPS影響モードのモード設定が「低影響モード」である場合は、GPS計測結果のINS計測結果に対する作用を弱めるために、観測誤差“R”を第1のパラメーター値よりも大きい第2のパラメーター値に設定する。第2のパラメーター値は第1のパラメーター値よりも大きな値であればよく、その具体的な値は適宜設定可能である。
【0074】
1−4.実験結果
次に、本実施形態の位置算出方法を用いて実際に位置算出を行った実験結果について説明する。移動体を予め定められた経路に沿って移動させ、その場合に算出される位置を東西南北の2次元平面にプロットする実験を行った。
【0075】
図7は、従来の位置算出方法を適用した実験結果の一例である。それに対し、図8は、本実施形態の位置算出方法を適用した実験結果の一例である。図7及び図8の実験では、真の方位から「10°」ずれた誤った方位を第1の位置算出装置1Aに初期方位として与えて位置算出を行った。
【0076】
各図では、横軸は東西方向、縦軸は南北方向(単位はメートル)をそれぞれ示す。東西方向「0m」、南北方向「0m」の位置をスタート地点とし、スタート地点から西方向に向かい、時計回り方向に周回する経路を辿った。ゴール地点は、スタート地点の近傍所定位置である。移動体の真の軌跡を「点線」で、GPSで算出した軌跡を「一点鎖線」で、カップリング処理で算出した軌跡を「実線」でそれぞれ示している。
【0077】
従来の手法を適用した図7の結果を見ると、初期方位として与えた方位が「10°」誤っていたため、この方位誤差に起因して、位置算出の精度が劣化していることがわかる。つまり、初期方位が誤っている状態で移動時速度制約条件を適用しているために、カルマンフィルター処理において、誤った方位に対して移動方向の制限がかかり、時間経過に伴って位置誤差が累積的に増加している。
【0078】
それに対し、本実施形態の手法を適用した図8の結果を見ると、初期方位が「10°」誤っていたにも関わらず、真の軌跡に沿った滑らかな軌跡が得られていることがわかる。これは、GPSの影響度合をその都度設定しながらカップリング処理を行ったことで、INS演算結果がGPS演算結果によって適切に補正されたことによるものである。
【0079】
図9及び図10は、第1のモード設定条件の有効性を示す実験結果である。図4で説明した第1のモード設定条件では、GPSの初回演算精度が良好であるにも関わらずGPS影響モードを「低影響モード」に設定し、GPSの初回演算精度が良好でないにも関わらずGPS影響モードを「高影響モード」に設定する。一見矛盾しているようにも思われるこのパラドックスの正当性を示す実験結果の一例である。
【0080】
GPS初回演算精度が良好である場合と、良好でない場合とのそれぞれについて、GPS影響モードを変えて位置算出を行ってみた。つまり、GPS影響モードを「低影響モード」に設定した場合と、GPS影響モードを「高影響モード」に設定した場合とで、位置算出結果にどのような違いが現れるかを検証した。
【0081】
各図において、横軸は東西方向、縦軸は南北方向(単位はメートル)をそれぞれ示している。東西方向「0m」、南北方向「0m」の位置をスタート地点とし、スタート地点から西方向に向かった後、北方向に向かう経路を辿った。ゴール地点は、西方向「70m」、北方向「140m」の位置である。各図では、移動体の真の軌跡を「点線」で、GPSを利用して算出した軌跡を「一点鎖線」で、GPS影響モードを高影響モードに設定して算出した軌跡を「細実線」で、GPS影響モードを低影響モードに設定して算出した軌跡を「太実線」でそれぞれ示している。
【0082】
図9は、GPS初回演算精度が良好である場合の実験結果である。これを見ると、GPS影響モードを高影響モード及び低影響モードの何れに設定した場合も、GPSの初回演算精度が良好であるために正確な基準位置及び基準方位が与えられたことで、暫くの期間は正確な軌跡が得られていることがわかる。しかし、ある時点においてGPSの演算精度が低下したことで、両者に明確な違いが生じている。
【0083】
GPS影響モードを高影響モードに設定した場合は、GPSの影響度合が高いために、誤ったGPS演算位置に引きずられて、途中から位置算出の精度が劣化していることがわかる。具体的には、算出位置の軌跡(細実線)が、誤ったGPS演算位置の軌跡(一点鎖線)に引き寄せられ、真の軌跡(点線)から離れていっていることがわかる。
【0084】
それに対し、GPS影響モードを低影響モードに設定した場合は、GPSの影響度合が低いために上記の問題は生じず、真の軌跡(点線)に沿った軌跡(太実線)が得られていることがわかる。
【0085】
図10は、GPS初回演算精度が良好でない場合の実験結果である。この結果を見ると、GPS影響モードを低影響モードに設定した場合は、算出位置の軌跡(太実線)が、真の軌跡(点線)から大きくかけ離れていることがわかる。つまり、誤った基準位置及び基準方位が与えられたために、当初から誤った方位に移動方向の制限がかかってしまっている。そして、ある時点で移動方向を完全に誤ってしまったが、GPSの影響度合が低いために補正がかからず、そのまま誤った方向に位置が算出されてしまっている。
【0086】
それに対し、GPS影響モードを高影響モードに設定した場合は、低影響モードに設定した場合と比べて真の軌跡(点線)に近い軌跡(細実線)が得られていることがわかる。これは、GPSの影響度合を高くし、時間をかけてゆっくりGPS計測結果をINS計測結果に作用させたことで、誤った方位に移動方向の制限がかかることが防止されたためである。
【0087】
図9及び図10の結果から、GPSの初回演算精度が良好である場合はGPS影響モードを「低影響モード」に設定し、GPSの初回演算精度が非良好である場合はGPS影響モードを「高影響モード」に設定することが妥当であることが実証された。
【0088】
2.実施例
次に、上記の位置算出装置を備えた電子機器の実施例について説明する。ここでは、位置算出装置を具備するカーナビゲーション装置の実施例を説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【0089】
2−1.システム構成
図11は、本実施例におけるナビゲーションシステム1000のシステム構成の説明図である。ナビゲーションシステム1000は、移動体の一種である四輪自動車(以下、単に「自動車」と称す。)に、位置算出装置を具備した電子機器の一種であるカーナビゲーション装置100が設置されたシステムである。
【0090】
カーナビゲーション装置100は、自動車に設置され、自動車の運転者に対するナビゲーションを行う電子機器である。カーナビゲーション装置100は、GPSユニット3と、INSユニット5とを備える。
【0091】
本実施例では、GPSユニット3は、GPSメジャメント情報55を計測して出力する。また、INSユニット5は、INSメジャメント情報56を、機体座標系(Body Frame)として知られるBフレームで計測して出力する。Bフレームは、例えば、移動体の前方を正とする前後方向をR軸(ロール軸)、右方を正とする左右方向をP軸(ピッチ軸)、鉛直下方を正とする上下方向をY軸(ヨー軸)とする三次元直交座標系である。
【0092】
カーナビゲーション装置100は、GPSユニット3から取得したGPSメジャメント情報を用いてGPS演算処理を行い、且つ、INSユニット5から取得したINSメジャメント情報を用いてINS演算処理を行う。そして、これらの演算結果に対するカップリング処理を実行して自動車の位置を算出し、算出位置をプロットしたナビゲーション画面を表示部30であるディスプレイに表示させる。
【0093】
自動車の位置は、自動車の移動空間を定める絶対的な座標系であるNフレームにおいて演算する。Nフレームは、例えば、北東下座標系として知られるNED(North East Down)座標系や、地球中心地球固定座標系として知られるECEF(Earth Centered Earth Fixed)座標系として定義される。
【0094】
2−2.機能構成
図12は、カーナビゲーション装置100の機能構成の一例を示すブロック図である。カーナビゲーション装置100は、GPSユニット3と、INSユニット5と、処理部10と、操作部20と、表示部30と、通信部40と、記憶部50とを備えて構成される。
【0095】
処理部10は、記憶部50に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置100の各部を統括的に制御する制御装置であり、CPU(Central Processing Unit)等のプロセッサーを有して構成される。処理部10は、記憶部50に記憶されたナビゲーションプログラム51に従ってナビゲーション処理を行い、自動車の現在位置を指し示した地図を表示部30に表示させる処理を行う。
【0096】
操作部20は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部10に出力する。この操作部20の操作により、目的地の入力等の各種指示入力がなされる。
【0097】
表示部30は、LCD(Liquid Crystal Display)等により構成され、処理部10から入力される表示信号に基づいた各種表示を行う表示装置である。表示部30には、ナビゲーション画面等が表示される。
【0098】
通信部40は、処理部10の制御に従って、装置内部で利用される情報をインターネット等の通信ネットワークを介して外部とやりとりするための通信装置である。この通信には、公知の無線通信技術を適用可能である。
【0099】
記憶部50は、ROM(Read Only Memory)やフラッシュROM、RAM(Random Access Memory)等の記憶装置によって構成され、カーナビゲーション装置100のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【0100】
記憶部50には、プログラムとして、処理部10により読み出され、各種のナビゲーション処理(図14や図16参照)として実行されるナビゲーションプログラム51が記憶されている。ナビゲーションプログラム51は、影響モード設定処理(図6参照)として実行される影響モード設定プログラム511や、カップリング処理(図15参照)として実行されるカップリングプログラム513をサブルーチンとして含む。
【0101】
また、記憶部50には、データとして、演算設定テーブル52と、モード設定条件テーブル53と、設定モード54と、GPSメジャメント情報55と、INSメジャメント情報56と、GPS演算結果57と、INS演算結果58と、カップリング結果59とが記憶される。
【0102】
演算設定テーブル52は、演算設定が定義されたテーブルであり、そのテーブル構成の一例を図13に示す。演算設定テーブル52には、GPS影響モード521と、演算設定523とが対応付けて記憶されている。高影響モードに対しては、GPS計測結果採用頻度として「毎回」(第1の頻度)が定められている。一方、低影響モードに対しては、GPS計測結果採用頻度として「10回に1回」(第2の頻度)が定められている。
【0103】
つまり、GPS影響モードを高影響モード(第1の度合)に設定した場合は、GPS演算結果を毎回採用して、カップリング処理を実行する。それに対し、GPS影響モードを低影響モード(第2の度合)に設定した場合は、GPS演算結果を10回に1回採用して、カップリング処理を実行する。
【0104】
観測誤差(R値)には、位置の観測誤差である位置観測誤差“RP”と、速度の観測誤差である速度観測誤差“RV”とが含まれる。高影響モードには「RP=(σP)2,RV=(σV)2」(第1のパラメーター値)が定められている。一方、低影響モードには「RP=500,RV=50」(第2のパラメーター値)が定められている。
【0105】
“σP”及び“σV”は、GPS演算結果に含まれる位置誤差及び速度誤差である。本願発明者が行った実験によれば、通常の測位環境では、位置誤差及び速度誤差は“σP=3〜4[m]”、“σV=0.6〜0.8[m/s]”程度の値として算出される。このため、高影響モードでは、位置観測誤差“RP”として“10”程度の値が設定され、速度観測誤差“RV”として“0.5”程度の値が設定される。従って、低影響モードでは、高影響モードと比べて大きい値が観測誤差“R”に設定される。
【0106】
モード設定条件テーブル53は、GPS影響モードを設定するための条件が定められたテーブルであり、例えば第1のモード設定条件テーブルや第2のモード設定条件テーブル(図4及び図5参照)がこれに含まれる。
【0107】
設定モード54は、設定されているGPS影響モードであり、影響モード設定処理により随時更新される。
【0108】
2−3.処理の流れ
図14は、処理部10が、記憶部50に記憶されたナビゲーションプログラム51に従って実行するナビゲーション処理の一例である第1のナビゲーション処理の流れを示すフローチャートである。
【0109】
先ず、処理部10は、GPSユニット3及びINSユニット5から、GPSメジャメント情報55及びINSメジャメント情報56の取得を開始する(ステップA1)。そして、処理部10は、移動状況判定処理を行う(ステップA3)。具体的には、例えば、INSユニット5からINSメジャメント情報56として取得した自動車の加速度(加速度ベクトル)や角速度に基づいて、自動車が停止している状態であるか、移動している状態であるかを判定する。
【0110】
その後、処理部10は、GPS演算処理を行う(ステップA5)。具体的には、GPSユニット3から取得したGPSメジャメント情報55を用いて、公知の測位計算を行って、自動車の位置や速度(速度ベクトル)を演算する。また、公知の誤差推定演算を行って、位置及び速度(速度ベクトル)の誤差を推定する。そして、これらの演算結果をGPS演算結果57として記憶部50に記憶させる。
【0111】
また、処理部10は、INS演算処理を行う(ステップA7)。具体的には、INSユニット5から取得したINSメジャメント情報56を用いて、公知の慣性航法演算を行って、自動車の位置や速度(速度ベクトル)、姿勢角を演算する。そして、これらの演算結果をINS演算結果58として記憶部50に記憶させる。
【0112】
次いで、処理部10は、記憶部50に記憶された影響モード設定プログラム511に従って、図6で説明した影響モード設定処理を行う(ステップA9)。そして、処理部10は、記憶部50に記憶されたカップリングプログラム513に従ってカップリング処理を実行する(ステップA11)。
【0113】
図15は、カップリング処理の流れを示すフローチャートである。
処理部10は、GPS演算結果57をカップリング処理に採用するか否かを判定する(ステップB1)。具体的には、影響モード設定処理で設定されたGPS影響モードに対応付けられたGPS計測結果採用頻度に基づき、今回のカップリング処理においてGPS演算結果57を採用するか否かを判定する。
【0114】
GPS演算結果57をカップリング処理に採用すると判定したならば(ステップB1;Yes)、処理部10は、最新のGPS演算結果57を観測量とする観測量ベクトル“Z”を設定する(ステップB3)。また、演算設定テーブル52を参照し、設定モード54に対応する観測誤差に基づいて観測誤差共分散行列“R”を設定する(ステップB5)。そして、処理部10は、ステップB3及びB5で設定した観測量ベクトル“Z”及び観測誤差共分散行列“R”を用いて、カルマンフィルター処理を実行する(ステップB7)。
【0115】
一方、ステップB1においてGPS演算結果57をカップリング処理に採用しないと判定したならば(ステップB1;No)、処理部10は、ステップA3で判定した移動状況を判定する(ステップB9)。
【0116】
移動状況が「移動」である場合は(ステップB9;移動)、処理部10は、移動時制約速度ベクトルを観測量ベクトル“Z”として設定する(ステップB11)。また、移動時用速度観測誤差(例えば所定値)に基づき観測誤差共分散行列“R”を設定する(ステップB13)。そして、処理部10は、ステップB11及びB13で設定した観測量ベクトル“Z”及び観測誤差共分散行列“R”を用いて、カルマンフィルター処理を実行する(ステップB7)。
【0117】
一方、移動状況が「停止」である場合は(ステップB9;停止)、処理部10は、停止時制約速度ベクトルを観測量ベクトル“Z”として設定する(ステップB15)。また、停止時用速度観測誤差(例えば所定値)に基づき観測誤差共分散行列“R”を設定する(ステップB17)。そして、処理部10は、ステップB15及びB17で設定した観測量ベクトル“Z”及び観測誤差共分散行列“R”を用いて、カルマンフィルター処理を実行する(ステップB7)。
【0118】
ステップB7においてカルマンフィルター処理を行うと、処理部10は、その結果をカップリング結果59として記憶部50に記憶させる。そして、処理部10は、カップリング処理を終了する。
【0119】
図14の第1のナビゲーション処理に戻り、カップリング処理を行った後、処理部10は、カップリング結果59を出力する(ステップA13)。例えば、カップリング結果59として得られた位置(カップリング位置)に対してマップマッチング処理を行い、その結果で表示部30のナビゲーション画面の表示を更新する。
【0120】
次いで、処理部10は、処理を終了するか否かを判定する(ステップA15)。例えば、操作部20を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、ナビゲーション処理を終了すると判定する。まだ処理を終了しないと判定した場合は(ステップA15;No)、処理部10は、ステップA3に戻る。また、処理を終了すると判定した場合は(ステップA15;Yes)、第1のナビゲーション処理を終了する。
【0121】
3.作用効果
GPSユニット3とINSユニット5とを備えた位置算出装置1において、影響度合設定部7は、位置算出を開始してから所与の条件が成立するまでの間は、GPS計測結果のINS計測結果に対する影響度合を第1の度合に設定し、所与の条件が成立した後は、影響度合を第1の度合よりも低い第2の度合に設定する。そして、カップリング処理部9は、影響度合設定部7により設定された影響度合に基づいてGPS計測結果とINS計測結果とのカップリング処理を実行して、移動体の位置を算出する。
【0122】
例えば、第1の位置算出装置1Aでは、影響度合設定部7が影響モード設定部7Aを有する。そして、(1)GPSの初回演算精度、(2)位置算出開始からの経過時間又は位置算出回数、に基づき定められた第1のモード設定条件と、カップリング結果の精度に基づき定められた第2のモード設定条件とを用いて、GPSの影響度合を決定付けるGPS影響モードを設定する。
【0123】
また、第1の位置算出装置1Aでは、カップリング処理部9がカルマンフィルター処理部9Aを有し、影響モード設定部7Aより設定されたGPS影響モードに応じて演算設定を変えてカルマンフィルター処理を行う。演算設定には、GPS計測結果を採用してカップリング処理を実行する実行頻度が含まれ、低影響モードでは、高影響モードと比べて実行頻度が低く設定される。また、演算設定には、観測量とするGPS計測結果の想定誤差である観測誤差(R値)が含まれ、低影響モードでは、高影響モードと比べて大きい値が設定される。
【0124】
このように、所与の条件が成立するまでの間は、衛星測位用ユニットの計測結果の慣性測位用ユニットの計測結果に対する影響度合を高く設定し、所与の条件が成立した後は、その影響度合を低く設定することで、特に位置算出装置の初期動作時における影響度合を適正化することができる。そして、影響度合に応じてカップリング処理の演算設定を変更することで、カップリング処理の実効性を高め、ひいては位置算出をより正確に行うことが可能となる。
【0125】
4.変形例
本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明するが、上記の実施例と同一の構成要素については同一の符号を付して説明を省略し、上記の実施例とは異なる部分を中心に説明する。
【0126】
4−1.ユニット
上記の実施形態では、衛星測位用ユニットとして、GPSを適用したGPSユニットを例に挙げて説明したが、WAAS(Wide Area Augmentation System)、QZSS(Quasi Zenith Satellite System)、GLONASS(GLObal NAvigation Satellite System)、GALILEO等の他の衛星測位システムを適用したユニットとしてもよいことは勿論である。
【0127】
また、上記の実施形態では、慣性測位用ユニットとして、INSユニットを例に挙げて説明したが、INSメジャメント情報(加速度や角速度)を計測する慣性センサーや慣性計測ユニット(IMU)を慣性測位用ユニットとしてもよい。この場合は、慣性測位用ユニットにより計測されたINSメジャメント情報を用いて、位置算出装置の処理部がINS演算処理を行うように構成すればよい。
【0128】
4−2.カップリング処理
上記の実施形態では、カップリング処理の一例としてカルマンフィルター処理を例に挙げて説明したが、カップリング処理はこれに限られない。例えば、GPS計測結果とINS計測結果との平均演算を行う平均処理をカップリング処理に含めてもよい。
【0129】
平均演算としては、単純な算術平均や幾何平均を適用してもよいし、加重平均を適用してもよい。算術平均や幾何平均を適用するのであれば、カップリング処理の演算設定として、例えば、GPS計測結果を採用して平均処理を実行する実行頻度を定めておくことができる。高影響モードでは、GPS計測結果をINS計測結果に作用し易くするために実行頻度を高くし、低影響モードでは、GPS計測結果をINS計測結果に作用しにくくするために実行頻度を低くすればよい。
【0130】
また、加重平均を適用するのであれば、カップリング処理の演算設定として、例えば加重平均の重みを定めておくことができる。高影響モードでは、GPS計測結果をINS計測結果よりも重視して加重平均するようにGPS計測結果の重みを大きく設定し、低影響モードでは、INS計測結果をGPS計測結果よりも重視して加重平均するようにGPS計測結果の重みを小さく設定すればよい。
【0131】
4−3.影響モードの設定方法
上記の実施形態では、第1のモード設定条件と第2のモード設定条件とを併用してGPS影響モードを設定することとして説明したが、第1のモード設定条件を単体で用いてGPS影響モードを設定することとしてもよいし、第2のモード設定条件を単体で用いてGPS影響モードを設定することとしてもよい。
【0132】
また、上記の実施形態では、第1のモード設定条件を、(1)GPS初回演算精度、(2)位置算出開始からの経過時間又は位置算出回数、を組み合わせた条件として定義したが、(1)及び(2)を個別の条件として定義してもよい。すなわち、(1)GPS初回演算精度に基づく条件を単体で用いてGPS影響モードを設定してもよいし、(2)位置算出開始からの経過時間又は位置算出回数に基づく条件を単体で用いてGPS影響モードを設定してもよい。
【0133】
4−4.処理主体
上記の実施例では、電子機器の処理部10が、GPSユニット3から取得したGPSメジャメント情報55を用いてGPS演算処理を行うこととして説明した。また、処理部10が、INSユニット5から取得したINSメジャメント情報56を用いてINS演算処理を行うこととして説明した。つまり、GPS演算処理、INS演算処理及びカップリング処理の実行主体が、全て電子機器の処理部10であるものとして説明した。この構成を次のようにしてもよい。
【0134】
GPSユニット3は、GPSメジャメント情報55を用いたGPS演算処理を行ってGPS演算結果57を求め、処理部10に出力する。また、INSユニット5は、INSメジャメント情報56を用いたINS演算処理を行ってINS演算結果58を求め、処理部10に出力する。
【0135】
そして、処理部10は、各ユニットから取得したGPS演算結果57とINS演算結果58とのカップリング処理を実行する。つまり、この場合は、GPS演算処理及びINS演算処理の実行主体が、それぞれGPSユニット3及びINSユニット5となり、カップリング処理の実行主体が、電子機器の処理部10となる。
【0136】
4−5.位置算出方法
設定されている影響モード(影響度合)に見合った位置算出の方式を適用して移動体の位置を算出することとしてもよい。例えば、GPS影響モードが高影響モードの場合は、GPS計測結果を用いた所定の位置算出処理を実行して位置を算出し、GPS影響モードが低影響モードの場合は、GPS計測結果及びINS計測結果を用いたカップリング処理を実行して位置を算出することとしてもよい。
【0137】
図16は、この場合に処理部10が図14の第1のナビゲーション処理に代えて実行する第2のナビゲーション処理の流れを示すフローチャートである。処理部10は、ステップA9において影響モード設定処理を行った後、影響モード設定処理で設定したモード(設定モード54)を判定する(ステップC10)。
【0138】
設定モード54が高影響モードであると判定した場合は(ステップC10;高影響モード)、処理部10は、GPS演算結果フィルターリング処理を行う(ステップC17)。そして、処理部10は、フィルターリング結果を出力する(ステップC19)。
【0139】
一方、設定モード54が低影響モードであると判定した場合は(ステップC10;低影響モード)、処理部10は、図15で説明したカップリング処理を行う(ステップA11)。そして、処理部10は、カップリング結果59を出力する(ステップA13)。
【0140】
ステップC17のGPS演算結果フィルターリング処理は、ステップA5のGPS演算処理で求めたGPS演算結果をフィルターリングすることで、より確からしい位置を算出する位置算出処理である。この場合のフィルターリング方式としては、種々の方式を適用可能である。例えば、カルマンフィルター処理を適用してもよいし、GPS演算結果の過去の履歴に基づくフィルターリング処理を適用してもよい。
【0141】
カルマンフィルター処理を適用するのであれば、例えば、GPS演算結果を制御入力“U”とし、上記の実施形態で説明した移動時速度制約条件や停止時速度制約条件を観測量“Z”として、移動体の位置や速度等の状態“X”を推定する手法が適用可能である。
【0142】
また、GPS演算結果の過去の履歴に基づくフィルターリング処理を適用するのであれば、例えば、過去所定期間分(例えば過去10秒間分)のGPS演算結果を平均化するなどして、移動体の位置や速度等を推定する手法が適用可能である。
【0143】
4−6.電子機器
上記の実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【0144】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、PDA(Personal Digital Assistant)といった他の電子機器についても本発明を同様に適用して、当該電子機器の位置算出を実現することが可能である。
【符号の説明】
【0145】
1、1A 位置算出装置、 3 GPSユニット、 5 INSユニット、 7 影響度合設定部、 7A 影響モード設定部、 9 カップリング処理部、 9A カルマンフィルター処理部、 10 処理部、 20 操作部、 30 表示部、 40 通信部、 50 記憶部、 100 カーナビゲーション装置、 1000 ナビゲーションシステム
【特許請求の範囲】
【請求項1】
移動体に備えられた衛星測位用ユニットの第1の計測結果と、前記移動体に備えられた慣性測位用ユニットの第2の計測結果とを用いて、前記移動体の位置を算出する位置算出方法であって、
位置算出を開始してから所与の条件が成立するまでの間は、前記第1の計測結果の前記第2の計測結果に対する影響度合を第1の度合に設定し、前記条件が成立した後は、前記影響度合を前記第1の度合よりも低い第2の度合に設定することと、
前記影響度合に基づき、前記第1の計測結果と前記第2の計測結果とのカップリング処理を実行して前記移動体の位置を算出することと、
を含む位置算出方法。
【請求項2】
前記影響度合には、前記第1の計測結果を採用して前記カップリング処理を実行する実行頻度が含まれており、
前記第1の度合に設定することは、前記実行頻度を第1の頻度に設定することを含み、
前記第2の度合に設定することは、前記実行頻度を前記第1の頻度よりも低い第2の頻度に設定することを含み、
前記位置を算出することは、前記実行頻度に従って前記第1の計測結果を採用した前記カップリング処理を実行することを含む、
請求項1に記載の位置算出方法。
【請求項3】
前記カップリング処理は、前記第1の計測結果を観測量とするカルマンフィルター処理を含み、
前記影響度合には、前記カルマンフィルター処理に用いる誤差パラメーター値が含まれており、
前記第1の度合に設定することは、前記誤差パラメーター値を第1のパラメーター値に設定することを含み、
前記第2の度合に設定することは、前記誤差パラメーター値を前記第1のパラメーター値よりも大きい第2のパラメーター値に設定することを含み、
前記位置を算出することは、前記第1の計測結果及び前記誤差パラメーター値を用いて前記カルマンフィルター処理を実行することを含む、
請求項1に記載の位置算出方法。
【請求項4】
前記位置を算出することは、前記影響度合が前記第1の度合の場合に前記第1の計測結果を用いた所定の位置算出処理を実行して位置を算出し、前記影響度合が前記第2の度合の場合に前記カップリング処理を実行して位置を算出することである、
請求項1に記載の位置算出方法。
【請求項5】
前記影響度合を設定することは、位置算出を開始してからの経過時間又は位置算出回数が、前記位置算出の結果の精度が安定するための時間的条件として定められた精度安定条件を満たす場合に、前記所与の条件が成立したと判定することを含む、
請求項1〜3の何れか一項に記載の位置算出方法。
【請求項6】
前記位置を算出することは、位置算出の開始時に、前記第1の計測結果から求められた位置を以降の位置算出の基準位置とすることを含み、
前記影響度合を設定することは、位置算出の開始時の前記第1の計測結果が所定の精度良好条件を満たす場合に、前記所与の条件が成立したと判定することを含む、
請求項1〜3の何れか一項に記載の位置算出方法。
【請求項7】
前記影響度合を設定することは、前記カップリング処理の結果が所定の精度良好条件を満たす場合に、前記所与の条件が成立したと判定することを含む、
請求項1〜3の何れか一項に記載の位置算出方法。
【請求項8】
移動体に備えられた衛星測位用ユニットの第1の計測結果と、前記移動体に備えられた慣性測位用ユニットの第2の計測結果とを用いて、前記移動体の位置を算出する位置算出装置であって、
位置算出を開始してから所与の条件が成立するまでの間は、前記第1の計測結果の前記第2の計測結果に対する影響度合を第1の度合に設定し、前記条件が成立した後は、前記影響度合を前記第1の度合よりも低い第2の度合に設定する影響度合設定部と、
前記影響度合に基づき、前記第1の計測結果と前記第2の計測結果とのカップリング処理を実行して前記移動体の位置を算出するカップリング処理部と、
を備えた位置算出装置。
【請求項1】
移動体に備えられた衛星測位用ユニットの第1の計測結果と、前記移動体に備えられた慣性測位用ユニットの第2の計測結果とを用いて、前記移動体の位置を算出する位置算出方法であって、
位置算出を開始してから所与の条件が成立するまでの間は、前記第1の計測結果の前記第2の計測結果に対する影響度合を第1の度合に設定し、前記条件が成立した後は、前記影響度合を前記第1の度合よりも低い第2の度合に設定することと、
前記影響度合に基づき、前記第1の計測結果と前記第2の計測結果とのカップリング処理を実行して前記移動体の位置を算出することと、
を含む位置算出方法。
【請求項2】
前記影響度合には、前記第1の計測結果を採用して前記カップリング処理を実行する実行頻度が含まれており、
前記第1の度合に設定することは、前記実行頻度を第1の頻度に設定することを含み、
前記第2の度合に設定することは、前記実行頻度を前記第1の頻度よりも低い第2の頻度に設定することを含み、
前記位置を算出することは、前記実行頻度に従って前記第1の計測結果を採用した前記カップリング処理を実行することを含む、
請求項1に記載の位置算出方法。
【請求項3】
前記カップリング処理は、前記第1の計測結果を観測量とするカルマンフィルター処理を含み、
前記影響度合には、前記カルマンフィルター処理に用いる誤差パラメーター値が含まれており、
前記第1の度合に設定することは、前記誤差パラメーター値を第1のパラメーター値に設定することを含み、
前記第2の度合に設定することは、前記誤差パラメーター値を前記第1のパラメーター値よりも大きい第2のパラメーター値に設定することを含み、
前記位置を算出することは、前記第1の計測結果及び前記誤差パラメーター値を用いて前記カルマンフィルター処理を実行することを含む、
請求項1に記載の位置算出方法。
【請求項4】
前記位置を算出することは、前記影響度合が前記第1の度合の場合に前記第1の計測結果を用いた所定の位置算出処理を実行して位置を算出し、前記影響度合が前記第2の度合の場合に前記カップリング処理を実行して位置を算出することである、
請求項1に記載の位置算出方法。
【請求項5】
前記影響度合を設定することは、位置算出を開始してからの経過時間又は位置算出回数が、前記位置算出の結果の精度が安定するための時間的条件として定められた精度安定条件を満たす場合に、前記所与の条件が成立したと判定することを含む、
請求項1〜3の何れか一項に記載の位置算出方法。
【請求項6】
前記位置を算出することは、位置算出の開始時に、前記第1の計測結果から求められた位置を以降の位置算出の基準位置とすることを含み、
前記影響度合を設定することは、位置算出の開始時の前記第1の計測結果が所定の精度良好条件を満たす場合に、前記所与の条件が成立したと判定することを含む、
請求項1〜3の何れか一項に記載の位置算出方法。
【請求項7】
前記影響度合を設定することは、前記カップリング処理の結果が所定の精度良好条件を満たす場合に、前記所与の条件が成立したと判定することを含む、
請求項1〜3の何れか一項に記載の位置算出方法。
【請求項8】
移動体に備えられた衛星測位用ユニットの第1の計測結果と、前記移動体に備えられた慣性測位用ユニットの第2の計測結果とを用いて、前記移動体の位置を算出する位置算出装置であって、
位置算出を開始してから所与の条件が成立するまでの間は、前記第1の計測結果の前記第2の計測結果に対する影響度合を第1の度合に設定し、前記条件が成立した後は、前記影響度合を前記第1の度合よりも低い第2の度合に設定する影響度合設定部と、
前記影響度合に基づき、前記第1の計測結果と前記第2の計測結果とのカップリング処理を実行して前記移動体の位置を算出するカップリング処理部と、
を備えた位置算出装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【公開番号】特開2012−215485(P2012−215485A)
【公開日】平成24年11月8日(2012.11.8)
【国際特許分類】
【出願番号】特願2011−81418(P2011−81418)
【出願日】平成23年4月1日(2011.4.1)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
【公開日】平成24年11月8日(2012.11.8)
【国際特許分類】
【出願日】平成23年4月1日(2011.4.1)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
[ Back to top ]