説明

慣性航法演算方法及び慣性航法演算装置

【課題】慣性航法演算によって正確性の高い位置を算出するための新しい手法の提案。
【解決手段】第1演算システム1Aにおいて、運動に係る物理量を検出するセンサーとして、加速度センサー2A及びジャイロセンサー2Bの計測結果を用いて、INS演算部20が慣性航法演算を行う。この際、代表値算出部10は、所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うように設定される対象期間毎に、当該対象期間内に計測された計測結果の変動に基づいて算出方法を変更して計測結果を統計演算処理する。そして、INS演算部20が、代表値算出部10による統計演算処理の結果を用いて慣性航法演算を行う。

【発明の詳細な説明】
【技術分野】
【0001】
本発明は、運動に係る物理量を検出するセンサーの計測結果を用いて慣性航法演算を行う方法等に関する。
【背景技術】
【0002】
いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、センサーの活用が注目されている。このセンサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。センサーの計測結果を利用して慣性航法演算を行う慣性航法システム(以下、「INS(Inertial Navigation System)」と称する。)も考案されている。
【0003】
INSでは、センサーの計測結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献1には、INS計測結果とGPS(Global Positioning System)計測結果とを併用して位置算出を行う技術が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】米国特許出願公開第2010/0019963号明細書
【発明の概要】
【発明が解決しようとする課題】
【0005】
慣性航法演算では、加速度センサーやジャイロセンサー等のセンサーの計測結果を用いて移動体の位置を算出する演算を行う。しかし、これらのセンサーの計測結果には、瞬間的に大きな誤差が混入し得る。そのため、従来のように、センサーの計測結果を全面的に信用し、何らの工夫もせずにセンサーの計測結果をそのまま利用して位置を算出した場合、算出位置の正確性が大きく低下し得るという問題があった。
【0006】
本発明は上述した課題に鑑みて為されたものであり、慣性航法演算によって正確性の高い位置を算出するための新しい手法を提案することを目的とする。
【課題を解決するための手段】
【0007】
以上の課題を解決するための第1の形態は、運動に係る物理量を検出するセンサーの計測結果を用いて慣性航法演算を行う方法であって、所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うように設定される対象期間毎に、当該対象期間内に計測された前記計測結果の変動に基づいて算出方法を変更して前記計測結果を統計演算処理することと、前記統計演算処理の結果を用いて慣性航法演算を行うことと、を含む慣性航法演算方法である。
【0008】
また、他の形態として、運動に係る物理量を検出するセンサーの計測結果を用いて慣性航法演算を行う慣性航法演算装置であって、所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うように設定される対象期間毎に、当該対象期間内に計測された前記計測結果の変動に基づいて算出方法を変更して前記計測結果を統計演算処理する統計演算部と、前記統計演算処理の結果を用いて慣性航法演算を行う慣性航法演算部と、を備えた慣性航法演算装置を構成することとしてもよい。
【0009】
この第1の形態等によれば、所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うように設定される対象期間毎に、当該対象期間内にセンサーによって計測された計測結果の変動に基づいて算出方法を変更して計測結果を統計演算処理する。センサーによって計測される運動に係る物理量は誤差を含み得るため、その計測結果の変動に基づいて、慣性航法演算に用いる統計演算処理の結果の算出方法を変更する。この際、時間的な連続性を保たせるために、所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うような対象期間を設定して統計演算処理を行う。その結果、慣性航法演算によって正確性の高い位置を算出することが可能となる。
【0010】
また、第2の形態として、第1の形態の慣性航法演算方法において、前記所定の時間間隔を変更することを更に含み、前記慣性航法演算を行うことは、前記所定の時間間隔の長さに応じて定められた慣性航法演算方法で慣性航法演算を行うことを含む、慣性航法演算方法を構成することとしてもよい。
【0011】
この第2の形態によれば、上記の形態における所定の時間間隔を変更する。そして、所定の時間間隔の長さに応じて定められた慣性航法演算方法で慣性航法演算を行う。所定の時間間隔を固定的にするのではなく可変にし、その時間間隔の長さに応じた慣性航法演算方法を用いることで、慣性航法演算を適切に行うことが可能となる。
【0012】
また、第3の形態として、第2の形態の慣性航法演算方法において、前記慣性航法演算方法には、少なくとも、第1の演算方法と、演算負荷が前記第1の演算方法に比べて高く、かつ、算出位置の精度が前記第1の演算方法に比べて良い第2の演算方法とが含まれ、前記慣性航法演算を行うことは、前記所定の時間間隔が第1の時間間隔の場合には前記第1の演算方法を用いて慣性航法演算を行い、前記所定の時間間隔が第1の時間間隔より長い第2の時間間隔の場合には前記第2の演算方法を用いて慣性航法演算を行うことである、慣性航法演算方法を構成することとしてもよい。
【0013】
この第3の形態によれば、慣性航法演算方法には、少なくとも、第1の演算方法と、演算負荷が第1の演算方法に比べて高く、算出位置の精度が第1の演算方法に比べて良い第2の演算方法とが含まれる。そして、所定の時間間隔が第1の時間間隔の場合には第1の演算方法を用いて慣性航法演算を行い、所定の時間間隔が第1の時間間隔より長い第2の時間間隔の場合には第2の演算方法を用いて慣性航法演算を行う。これにより、所定の時間間隔の長さに見合った適切な演算方法を用いて慣性航法演算を行うことが可能となる。
【0014】
また、第4の形態として、第2又は第3の形態の慣性航法演算方法において、前記計測結果を用いて移動体の移動状況を判定することを更に含み、前記変更することは、前記移動状況の判定結果に基づいて前記所定の時間間隔を変更することを含む、慣性航法演算方法を構成することとしてもよい。
【0015】
この第4の形態によれば、運動に係る物理量を検出するセンサーの計測結果を用いて移動体の移動状況を判定し、その判定結果に基づいて所定の時間間隔を変更することで、移動体の移動状況に見合った適切な時間間隔で慣性航法演算を行うことが可能となる。
【0016】
より具体的には、第5の形態として、第4の形態の慣性航法演算方法において、前記移動状況を判定することは、移動量及び方向の変化を判定することを含み、前記変更することは、前記判定された移動量及び方向の変化が所定の変化量未満の場合に比べて、前記判定された移動量及び方向の変化が前記所定の変化量以上の場合の方が、時間間隔が短くなるように前記所定の時間間隔を変更することを含む、慣性航法演算方法を構成することとしてもよい。
【0017】
この第5の形態によれば、移動状況の判定として移動量及び方向の変化を判定する。そして、判定された移動量及び方向の変化が所定の変化量未満の場合に比べて、判定された移動量及び方向の変化が所定の変化量以上の場合の方が、時間間隔が短くなるように所定の時間間隔を変更する。移動量及び方向の変化が大きい場合の方が、小さい場合と比べて時間間隔を短くすることで、移動量及び方向の変化に応じて適正化した時間間隔で慣性航法演算を行うことが可能となる。
【0018】
また、第6の形態として、第5の形態の慣性航法演算方法において、前記センサーには、少なくとも加速度センサーが含まれ、前記移動状況を判定することは、少なくとも移動量を判定することを含み、前記変更することは、前記判定された移動量が所定の移動量未満の場合に比べて、前記判定された移動量が前記所定の移動量以上の場合の方が、前記加速度センサーに係る前記所定の時間間隔が短くなるように変更することを含む、慣性航法演算方法を構成することとしてもよい。
【0019】
この第6の形態によれば、少なくとも移動量を判定する。そして、判定された移動量が所定の移動量未満の場合に比べて、判定された移動量が所定の移動量以上の場合の方が、加速度センサーに係る所定の時間間隔が短くなるように変更する。移動量が大きい場合は、移動体の位置が大きく変化し得るため、加速度センサーに係る所定の時間間隔を短くすることで、位置の演算精度を向上させることができる。
【0020】
また、第7の形態として、第6の形態の慣性航法演算方法において、前記センサーには、少なくともジャイロセンサーが含まれ、前記移動状況を判定することは、少なくとも方向の変化を判定することを含み、前記変更することは、前記判定された方向の変化が所定の方向変化量未満の場合に比べて、前記判定された方向の変化が前記所定の方向変化量以上の場合の方が、ジャイロセンサーに係る所定の時間間隔が短くなるように変更することを含む、慣性航法演算方法を構成することとしてもよい。
【0021】
この第7の形態によれば、少なくとも方向の変化を判定する。そして、判定された方向の変化が所定の方向変化量未満の場合に比べて、判定された方向の変化が所定の方向変化量以上の場合の方が、ジャイロセンサーに係る所定の時間間隔が短くなるように変更する。方向の変化が大きい場合は、移動体の移動方向が大きく変化し得るため、ジャイロセンサーに係る所定の時間間隔を短くすることで、方向の演算精度を向上させることができる。
【図面の簡単な説明】
【0022】
【図1】第1演算システムのシステム構成図。
【図2】INS演算部の機能構成図。
【図3】代表値算出用テーブルのテーブル構成図。
【図4】ナビゲーションシステムのシステム構成図。
【図5】カーナビゲーション装置の機能構成図。
【図6】第1メイン処理の流れを示すフローチャート。
【図7】代表値算出処理の流れを示すフローチャート。
【図8】第2演算システムのシステム構成図。
【図9】第2実施形態におけるINS演算部の機能構成図。
【図10】演算周期決定用テーブルのテーブル構成図。
【図11】第2実施例における記憶部のデータ構成図。
【図12】第2メイン処理の流れを示すフローチャート。
【発明を実施するための形態】
【0023】
1.第1実施形態
1−1.システム構成
図1は、第1実施形態における第1演算システム1Aのシステム構成図である。第1演算システム1Aは、例えば移動体に備えられて、該移動体の位置や速度ベクトル、姿勢といった移動体の運動に係る諸量を演算するシステムである。移動体は、自動車やオートバイ、自転車、船、電車といった乗り物の他、人間自体であってもよい。人間が演算システムを携帯し、人間自身が演算システムを備えることとしてもよい。
【0024】
以下参照する図面では、センサーやユニットのブロックを二重の実線で示し、これらの計測結果を利用して演算処理を行う処理ブロックを1本の実線で示す。処理ブロックは、例えば電子機器に搭載されるプロセッサー(ホストプロセッサー)が処理主体となる。なお、各処理ブロックの処理の主体は、本発明を適用するシステムに応じて適宜設定可能である。
【0025】
第1演算システム1Aは、加速度センサー2Aと、ジャイロセンサー2Bと、GPSユニット3と、第1慣性航法演算装置5Aと、GPS演算部30と、カップリング処理部40とを有する。但し、これらの構成要素は一例として記載したものに過ぎず、必ずしもこれら全ての機能部を必須構成要素としなければならないわけではない。
【0026】
加速度センサー2Aは、加速度をローカル座標系で計測するセンサーである。また、ジャイロセンサー2Bは、角速度をローカル座標系で計測するセンサーである。これらのセンサーには、例えばMEMS(Micro Electro Mechanical Systems)の技術を利用したMEMSセンサーが用いられる。これらのセンサーは、運動に係る物理量を検出するセンサーの一例である。本実施形態では、加速度センサー2A及びジャイロセンサー2Bが、それぞれ10ミリ秒の時間間隔で加速度及び角速度のデータを出力するものとして説明する。
【0027】
本実施形態では、2種類の座標系を定義する。第1の座標系は、センサーに対応付けられた三次元直交座標系でなるローカル座標系(センサー座標系)である。本実施形態では、ローカル座標系の3軸をx軸、y軸及びz軸と表記する。第2の座標系は、慣性航法演算を行う際の基準とする空間座標系である絶対座標系である。絶対座標系は、例えば北東下座標系として知られるNED(North East Down)座標系であり、重力方向の軸を含む座標系である。本実施形態では、絶対座標系の3軸をX軸、Y軸及びZ軸と表記する。
【0028】
加速度や速度には、大きさの他に方向がある。本明細書では、原則として、加速度や速度と言ったときは加速度や速度の大きさ(スカラー量)を表すものとし、加速度ベクトルや速度ベクトルと言ったときは大きさ(スカラー量)に加えて方向を表すものとする。また、各座標系において定義される諸量を明確にするため、各諸量を表す文言の先頭に座標系の種類を付して説明する。例えば、ローカル座標系で表した加速度ベクトルのことを「ローカル座標加速度ベクトル」と称し、絶対座標系で表した加速度ベクトルのことを「絶対座標加速度ベクトル」と称する。他の諸量についても同様である。
【0029】
第1慣性航法演算装置5Aは、加速度センサー2A及びジャイロセンサー2Bの計測結果を利用して慣性航法演算を行うことにより、自立的な位置算出を行う装置である。本実施形態において、第1慣性航法演算装置5Aは、主要な機能ブロックとして、代表値算出部10と、INS演算部20とを有する。
【0030】
代表値算出部10は、加速度センサー2Aから出力される計測加速度d11及びジャイロセンサー2Bから出力される計測角速度d12に基づいて、INS演算部20が慣性航法演算に用いる代表値を算出する。具体的には、加速度の代表値である加速度代表値d13を算出する加速度代表値算出部10Aと、角速度の代表値である角速度代表値d14を算出する角速度代表値算出部10Bとを有し、これらの代表値をINS演算部20に出力する。
【0031】
図2は、INS演算部20の機能構成の一例を示す図である。INS演算部20は、代表値算出部10から出力される加速度代表値d13及び角速度代表値d14を用いて慣性航法演算を行う演算部であり、代表値算出部10による統計演算処理の結果を用いて慣性航法演算を行う慣性航法演算部に相当する。
【0032】
INS演算部20は、主要な処理ブロックとして、姿勢情報算出部21と、座標変換部23と、絶対座標速度ベクトル算出部25と、重力算出部27と、絶対座標位置算出部29とを有する。
【0033】
姿勢情報算出部21は、代表値算出部10から出力される角速度代表値d14を用いて、最新の姿勢情報d21を算出する。姿勢情報d21は、例えば、クォータニオンや方向余弦行列(以下、「DCM(Direction Cosine Matrix)」と称す。)、オイラー角といった、各種の姿勢表現に基づき定義される姿勢を示す情報のことを意味する。
【0034】
座標変換部23は、代表値算出部10から出力される加速度代表値d13と、姿勢情報算出部21から出力される姿勢情報d21とを用いて、最新の絶対座標加速度ベクトルd23を算出する。具体的には、ローカル座標系から絶対座標系への座標変換行列を用いて、3次元の加速度代表値d13を絶対座標加速度ベクトルd23に座標変換する。なお、座標変換行列は従来公知であるため、数式等を用いた説明は省略する。
【0035】
絶対座標速度ベクトル算出部25は、座標変換部23から出力される絶対座標加速度ベクトルd23と、重力算出部27から入力した重力情報d27とを用いて、絶対座標速度ベクトルd25を算出する。この際、重力方向及びコリオリの力を重力情報に基づいて補正する。地球は楕円形をしているため、地球中心から見た緯度と地理学上の緯度とは異なる。そのため、重力方向についての補正が必要であり、地球は宇宙の慣性空間に対して回転しているため、コリオリの力の補正も必要となる。
【0036】
絶対座標位置算出部29は、絶対座標速度ベクトル算出部25から出力される絶対座標速度ベクトルd25を最新の絶対座標位置に積算することで、絶対座標位置d29を算出する。
【0037】
重力算出部27は、絶対座標位置算出部29から出力される絶対座標位置d29を用いて、重力情報d27を算出して、絶対座標速度ベクトル算出部25に出力する。
【0038】
最終的に、INS演算部20からは、例えば、絶対座標位置d29と、絶対座標速度ベクトルd25と、姿勢情報d21と、加速度代表値d13と、角速度代表値d14とが、INS演算結果d3として出力される。
【0039】
図1に戻り、GPSユニット3は、衛星測位システムの一種であるGPSを利用して、GPS衛星から発信されているGPS衛星信号に係る諸量を計測する。GPSユニット3は、GPS衛星から発信されているGPS衛星信号を受信するためのGPSアンテナや、GPSアンテナで受信された信号を処理するRF(Radio Frequency)回路、ベースバンド処理回路等を有して構成される。
【0040】
GPSユニット3は、GPS衛星のサーチ(衛星サーチ)を行うことで取得したコード位相やドップラー周波数、擬似距離、擬似距離変化率といったGPSメジャメント情報d5を計測して出力する。本実施形態では、GPSユニット3が、1秒の時間間隔でGPSメジャメント情報d5を出力するものとして説明する。
【0041】
GPS演算部30は、GPSユニット3から出力されるGPSメジャメント情報d5を用いて、所定の位置及び速度計算を行って、移動体の絶対座標位置及び絶対座標速度ベクトルを演算する。そして、これらの演算結果をGPS演算結果d7としてカップリング処理部40に出力する。本実施形態では、GPS演算部30が、1秒の時間間隔でGPS演算を行ってGPS演算結果d7を出力するものとして説明する。
【0042】
カップリング処理部40は、INS演算部20から出力されるINS演算結果d3と、GPS演算部30から出力されるGPS演算結果d7とを結び付ける所定のカップリング処理を行う。例えば、カップリング処理部40は、カルマンフィルターを利用してINS演算結果d3とGPS演算結果d7とをカップリングする。カップリングとしては、単純平均や加重平均など種々の合成方法が知られているが、本実施形態では次のように行う。
【0043】
カップリング処理部40は、INS演算部20の演算周期と同じ100ミリ秒の時間間隔でカップリング演算を行って、カップリング結果d9を出力する。つまり、100ミリ秒間隔でINS演算部20からINS演算結果d3を入力すると、カップリング処理部40は、INS演算結果d3を制御入力とするカルマンフィルターの予測演算(時刻更新)を行う。そして、1秒間隔でGPS演算部30からGPS演算結果d7を入力すると、当該GPS演算結果d7を観測情報として用いて、カルマンフィルターの補正演算(観測更新)を行う。
【0044】
1−2.代表値の算出方法
加速度センサー2A及びジャイロセンサー2Bは、10ミリ秒の時間間隔で計測結果(計測加速度d11及び計測角速度d12)を代表値算出部10に出力する。代表値算出部10は、この時間間隔よりも長い100ミリ秒の時間間隔で、INS演算部20が慣性航法演算に用いる代表値を算出・決定して、INS演算部20に出力する。本実施形態では、この100ミリ秒の時間間隔が所定の時間間隔に相当する。
【0045】
本実施形態では、代表値算出部10は、100ミリ秒毎に到来する時刻を跨ぎ、互いに時間的に重なり合うように設定される対象期間毎に、当該対象期間内に計測されたジャイロセンサー2Bの計測結果の変動に基づいて算出方法を変更して計測結果を統計演算処理する。この際、ジャイロセンサー2Bの計測結果の変動に基づき定められた所定の判定条件が成立したか否かに基づいて、INS演算部20が慣性航法演算で用いる代表値の算出方法を変更する。代表値算出部10は、統計演算部に相当する。
【0046】
詳細に説明する。所定の時間間隔毎に到来する時刻を演算基準時刻とする。このとき、演算基準時刻を跨ぐように対象期間を設定する。但し、対象期間は、互いに時間的に重なり合うように設定する。本実施形態では、所定の時間間隔を100ミリ秒とするため、100ミリ秒毎に到来する時刻を演算基準時刻とする。このとき、例えば、演算基準時刻を中心とし、当該演算基準時刻の前後に60ミリ秒の時間幅を有する120ミリ秒の対象期間を設定する。このように対象期間を設定することで、演算基準時刻を跨ぎ、互いに時間的に重なり合う対象期間を設定することができる。
【0047】
なお、演算基準時刻を中心とする前後の時間幅は60ミリ秒に限られるわけではなく、70ミリ秒や80ミリ秒として、140ミリ秒や160ミリ秒の対象期間を設定するなどしてもよい。
【0048】
次に、対象期間内にジャイロセンサー2Bによって計測された角速度の平均値(以下、「角速度平均値」と称す。)を算出し、当該角速度平均値と、対象期間に含まれる各角速度それぞれとの差(以下、「角速度差」と称す。)を算出する。そして、算出した角速度差に基づいてジャイロセンサー2Bの計測結果の変動を判定し、当該変動に基づいて代表値の算出方法を変更する。
【0049】
図3は、代表値の算出方法の説明図である。代表値算出部10が代表値を算出するために用いる代表値算出用のテーブル(代表値算出用テーブル)を図示している。この代表値算出用テーブルには、条件の番号(条件No)と、判定条件と、代表値とが対応付けて定められている。
【0050】
第1番目の判定条件は「(角速度差<θLとなったデータ数)>N1 & 速度の大きさ<V1」が成立することである。但し、「θL」は角速度差に対する低閾値条件の判定用の閾値である。また、「N1」はデータ数に対する高閾値条件の判定用の閾値であり、「V1」は速度の大きさに対する低閾値条件の判定用の閾値である。
【0051】
この第1番目の判定条件を満たす場合の代表値として「平均値」が定められている。第1番目の判定条件が成立する場合は、角速度の変動が小さく、速度が小さいことから、移動体は停止している状況である可能性が高い。そこで、統計演算処理として、対象期間に含まれるデータの平均値を算出して代表値とすることが定められている。
【0052】
第2番目の判定条件は「(角速度差<θLとなったデータ数)>N1 且つ 速度の大きさ≧V1」が成立することである。そして、この場合の代表値として「最頻値」が定められている。第2番目の判定条件が成立する場合は、角速度の変動は小さいが、速度が大きいことから、移動体は定常的な走行状況(等速状況など)である可能性が高い。そこで、統計演算処理として、対象期間に含まれるデータの最頻値を求めて代表値とすることが定められている。
【0053】
第3番目の判定条件は「角速度差>θHとなったデータが存在 & そのデータ数>N2」が成立することである。但し、「θH」は角速度差に対する高閾値条件の判定用の閾値であり「θL≦θH」である。また、「N2」はデータ数に対する高閾値条件の判定用の閾値である。
【0054】
この第3番目の判定条件を満たす場合の代表値として「中央値」が定められている。第3番目の判定条件が成立する場合は、角速度の変動が大きいことから、移動体は加減速状況である可能性が高い。そこで、統計演算処理として、対象期間に含まれるデータの中央値を求めて代表値とすることが定められている。
【0055】
第4番目の判定条件は「角速度差>θHとなったデータが存在 & そのデータ数<N3」が成立することである。但し、「N3」はデータ数に対する低閾値条件の判定用の閾値であり「N3<N2」である。
【0056】
この第4番目の判定条件を満たす場合の代表値として「最大値及び最小値を除外した平均値」が定められている。この判定条件が成立する場合は、角速度の変動はそれほど大きくはないが、角速度差が大きいデータが混入していることから、ノイズが混入し易い状況(ノイジー状況)である可能性が高い。そこで、統計演算処理として、対象期間に含まれるデータのうち最大値及び最小値を除外したデータの平均値を求めて代表値とする。
【0057】
なお、ここでは、ジャイロセンサー2Bによって計測された角速度の変動に基づいて代表値の算出方法を変更する場合を例に挙げて説明したが、加速度センサー2Aによって計測された加速度の変動に基づいて代表値の算出方法を変更することも可能である。この場合は、加速度の変動に基づく判定条件を代表値と対応付けて、図3に示したような代表値算出用テーブルを定めておけばよい。また、加速度センサー2Aの計測値及びジャイロセンサー2Bの計測値に基づいて代表値の算出方法を変更することとしてもよい。
【0058】
また、本実施形態では、所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うような対象期間を設定して統計演算処理を行うものとして説明するが、これを次のようにしてもよい。所定の時間間隔が経過する毎に、当該所定の時間間隔の間に計測されたセンサーの計測結果の変動に基づいて算出方法を変更して、当該所定の時間間隔の間に計測された計測結果を統計演算処理する。本実施形態では所定の時間間隔を100ミリ秒とするため、100ミリ秒が経過する毎に、当該100ミリ秒の間に加速度センサー2Aやジャイロセンサー2Bによって計測された計測結果を統計演算処理することで、代表値を算出することとしてもよい。
【0059】
その他にも、例えば、100ミリ秒毎に到来する代表値の算出タイミングにおいて、当該算出タイミングから遡った過去所定期間(例えば過去150ミリ秒分の期間)を対象期間として設定し、当該対象期間内に加速度センサー2Aやジャイロセンサー2Bによって計測された計測結果を統計演算処理することで、代表値を算出することとしてもよい。
【0060】
1−3.第1実施例
次に、上記の慣性航法演算方法を適用した電子機器の実施例を説明する。ここでは、主に四輪自動車に搭載されて利用されるカーナビゲーション装置の実施例を説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【0061】
1−3−1.システム構成
図4は、本実施例におけるナビゲーションシステムのシステム構成の説明図である。ナビゲーションシステムは、移動体の一種である四輪自動車(以下、単に「自動車」と称す。)に、電子機器の一種であるカーナビゲーション装置1000が設置・構成されたシステムである。カーナビゲーション装置1000は、自動車に設置され、自動車の運転者に対するナビゲーションを行う。カーナビゲーション装置1000は、センサーユニットとして、IMU(Inertial Measurement Unit)2と、GPSユニット3とを備える。
【0062】
本実施例において、GPSユニット3は、GPSメジャメント情報を計測して出力する。また、IMU2は、INSメジャメント情報を、機体座標系(Body Frame)として知られるBフレームで計測して出力する。Bフレームは、例えば、移動体の前方を正とする前後方向をR軸(ロール軸)、右方を正とする左右方向をP軸(ピッチ軸)、鉛直下方を正とする上下方向をY軸(ヨー軸)とする三次元直交座標系である。
【0063】
カーナビゲーション装置1000は、GPSユニット3から取得したGPSメジャメント情報を用いてGPS演算処理を行い、且つ、IMU2から取得したINSメジャメント情報を用いてINS演算処理を行う。そして、これらの演算結果を併用したカップリング処理を実行して自動車の位置を算出する。そして、算出した位置をプロットしたナビゲーション画面を生成してディスプレイに表示させることで、運転者に対するナビゲーションを実現する。
【0064】
自動車の位置は、自動車の移動空間を定める絶対的な座標系であるNフレームにおいて演算する。Nフレームは、例えば、北東下座標系として知られるNED(North East Down)座標系や、東北上座標系として知られるENU(East North Up)座標系、地球中心地球固定座標系として知られるECEF(Earth Centered Earth Fixed)座標系として定義される。なお、BフレームからNフレームへの座標変換は、自動車の姿勢(姿勢角)に基づき定められる公知の座標変換行列を用いて実現可能である。
【0065】
1−3−2.機能構成
図5は、カーナビゲーション装置1000の機能構成の一例を示すブロック図である。カーナビゲーション装置1000は、IMU2と、GPSユニット3と、処理部100と、操作部200と、表示部300と、音出力部400と、記憶部500とを備えて構成される。
【0066】
処理部100は、記憶部500に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置1000の各部を統括的に制御する制御装置であり、CPU(Central Processing Unit)等のプロセッサーを有して構成される。処理部100は、記憶部500に記憶されたメインプログラムに従ってメイン処理を行い、自動車の現在位置を指し示した地図を表示部300に表示させる処理を行う。
【0067】
操作部200は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部100に出力する。この操作部200の操作により、目的地の設定等の各種指示入力がなされる。
【0068】
表示部300は、LCD(Liquid Crystal Display)等により構成され、処理部100から入力される表示信号に基づいた各種表示を行う表示装置である。表示部300には、ナビゲーション画面や時刻情報等が表示される。
【0069】
音出力部400は、スピーカー等を有して構成される音出力装置であり、処理部100から出力される音出力信号に基づいた各種音出力を行う。音出力部400からは、音声ガイダンスや音楽等が音出力される。
【0070】
記憶部500は、ROM(Read Only Memory)やフラッシュROM、RAM(Random Access Memory)等の記憶装置を有して構成される。記憶部500は、カーナビゲーション装置1000のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【0071】
記憶部500には、プログラムとして、処理部100により読み出され、第1メイン処理(図6参照)として実行される第1メインプログラム510が記憶されている。第1メインプログラム510は、代表値算出処理(図7参照)として実行される代表値算出プログラム511をサブルーチンとして含む。これらの処理については、フローチャートを用いて詳細に後述する。
【0072】
また、記憶部500には、データとして、代表値算出用テーブル520と、IMUメジャメントデータ530と、GPSメジャメントデータ540と、INS演算データ550と、GPS演算データ560と、カップリングデータ570とが記憶される。
【0073】
代表値算出用テーブル520は、慣性航法演算で用いる代表値を算出するためのテーブルであり、そのテーブル構成例は図3に示した通りである。
【0074】
IMUメジャメントデータ530は、IMU2の計測結果のデータである。IMU2によって計測されたIMUメジャメント情報(加速度、角速度)が時系列に記憶される。
【0075】
GPSメジャメントデータ540は、GPSユニット3の計測結果のデータである。GPSユニット3により演算されたGPS衛星信号に係るGPSメジャメント情報(コード位相、ドップラー周波数、擬似距離、擬似距離変化率等)が時系列に記憶される。
【0076】
INS演算データ550は、INSメジャメント情報を利用した慣性航法演算(INS演算)を行って算出した自動車の位置や速度ベクトル、姿勢角、加速度、角速度等のデータである。
【0077】
GPS演算データ560は、GPSメジャメント情報を利用した測位演算(GPS演算)を行って算出した自動車の位置や速度ベクトル等のデータである。
【0078】
カップリングデータ570は、GPS演算結果とINS演算結果とをカップリング処理して求めた自動車の位置や速度ベクトル、姿勢角、加速度、角速度等のデータである。
【0079】
1−3−3.処理の流れ
図6は、処理部100が、記憶部500に記憶された第1メインプログラム510に従って実行する第1メイン処理の流れを示すフローチャートである。
【0080】
最初に、処理部100は、IMU2及びGPSユニット3から、IMUメジャメント情報及びGPSメジャメント情報の取得を開始し、それぞれIMUメジャメントデータ530及びGPSメジャメントデータ540として記憶部500に記憶させる(ステップA1)。
【0081】
次いで、処理部100は、代表値の算出タイミングであるか否かを判定する(ステップA3)。具体的には、100ミリ秒毎の時間間隔の到来タイミングであるか否かを判定する。算出タイミングではないと判定した場合は(ステップA3;No)、処理部100は、ステップA9へと処理を移行する。また、算出タイミングであると判定した場合は(ステップA3;Yes)、処理部100は、記憶部500に記憶されている代表値算出プログラム511に従って代表値算出処理を行う(ステップA5)。
【0082】
図7は、代表値算出処理の流れを示すフローチャートである。
先ず、処理部100は、対象期間内のデータの平均処理を行う(ステップB1)。具体的には、対象期間内の角速度(或いは加速度)のデータを平均処理する。そして、処理部100は、データの平均値と、対象期間に含まれる各データそれぞれとの差を算出する(ステップB3)。
【0083】
次いで、処理部100は、記憶部500に記憶されている代表値算出用テーブル520を参照し、何れの判定条件が成立するかに応じて、ジャイロセンサー2Bの計測結果の変動を判定する(ステップB5)。そして、処理部100は、変動の判定結果に基づいて代表値の算出方法を決定し(ステップB7)、当該算出方法で代表値を算出した後(ステップB9)、代表値算出処理を終了する。
【0084】
図6の第1メイン処理に戻り、代表値算出処理を行った後、処理部100は、代表値算出処理で算出した代表値を用いて慣性航法演算処理(INS演算処理)を行い、そのINS演算の演算過程及び演算結果のデータをINS演算データ550として記憶部500に記憶させる(ステップA7)
【0085】
次いで、処理部100は、GPSの演算タイミングであるか否かを判定する(ステップA9)。具体的には、例えば1秒毎の時間間隔の到来タイミングであるか否かを判定する。演算タイミングではないと判定した場合は(ステップA9;No)、ステップA15へと処理を移行する。
【0086】
一方、演算タイミングであると判定した場合は(ステップA9;Yes)、処理部100は、GPSメジャメントデータ540に記憶されている1秒間分のGPSメジャメント情報を用いて所定の位置及び速度計算を行って、自動車の位置及び速度ベクトルを算出し、その算出結果をGPS演算データ560として記憶部500に記憶させる(ステップA11)。なお、GPSにおける位置及び速度計算は従来公知であるため、ここでは詳細な説明を省略する。
【0087】
次いで、処理部100は、カップリング処理を行う(ステップA13)。具体的には、INS演算処理で求めた最新のINS演算結果と、GPS演算処理で求めた最新のGPS演算結果とを、例えばカルマンフィルターを利用してカップリングする。そして、カップリング結果をカップリングデータ570として記憶部500に記憶させる。
【0088】
その後、処理部100は、最新のカップリング結果を出力する(ステップA15)。そして、処理部100は、処理を終了するか否かを判定する(ステップA17)。例えば、操作部200を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、処理を終了すると判定する。
【0089】
まだ処理を終了しないと判定した場合は(ステップA17;No)、処理部100は、ステップA3に戻る。また、処理を終了すると判定した場合は(ステップA17;Yes)、処理部100は、第1メイン処理を終了する。
【0090】
1−4.作用効果
第1実施形態によれば、運動に係る物理量を検出するセンサーとして、加速度センサー2A及びジャイロセンサー2Bの計測結果を用いて慣性航法演算を行う。この際、所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うように設定される対象期間毎に、当該対象期間内に計測された計測結果の変動に基づいて算出方法を変更して計測結果を統計演算処理する。そして、統計演算処理の結果を用いて慣性航法演算を行う。
【0091】
統計演算処理としては、対象期間内に計測された計測結果の平均値や最頻値、中央値を求める処理等を適用することができる。加速度センサー2A及びジャイロセンサー2Bの計測結果は誤差を含み得る。そのため、これらのセンサーの計測結果の変動に基づいて、慣性航法演算に用いる採用値を変更することで、移動体の移動状況に見合った採用値を用いて慣性航法演算を行うことが可能となる。この際、時間的な連続性を保たせるために、所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うような対象期間を設定して統計演算処理を行う。その結果、慣性航法演算によって正確性の高い位置を算出することができる。
【0092】
また、本実施形態では、加速度センサー2A及びジャイロセンサー2Bの計測結果を慣性航法演算に逐次利用するのではなく、計測結果を蓄積し、所定の対象期間内に計測された計測結果を統計演算処理して慣性航法演算に利用する。従って、加速度センサー2A及びジャイロセンサー2Bの計測結果を逐次利用して慣性航法演算を行う場合と比べて、慣性航法演算に係る演算量を削減することができ、プロセッサーの演算負荷を軽減させることができる。
【0093】
2.第2実施形態
2−1.システム構成
図8は、第2実施形態における第2演算システム1Bのシステム構成図である。なお、第1実施形態で説明した第1演算システム1Aの構成要素と同一の構成要素については同一の符号を付して、再度の説明を省略する。また、第1実施形態と同一のデータやフローチャートの同一のステップについても同一の符号を付して、再度の説明を省略する。
【0094】
第2演算システム1Bは、第1演算システム1Aと同様に、加速度センサー2Aと、ジャイロセンサー2Bと、GPSユニット3と、第2慣性航法演算装置5Bと、GPS演算部30と、カップリング処理部40とを有する。但し、これらの構成要素は一例として記載したものに過ぎず、必ずしもこれら全ての機能部を必須構成要素としなければならないわけではない。
【0095】
第2慣性航法演算装置5Bは、第1慣性航法演算装置5Aの機能ブロックの他に、演算周期決定部50を有して構成される。演算周期決定部50は、カップリング処理部40から出力されるカップリング処理の結果に基づいて、INS演算部20の演算周期を決定する。
【0096】
代表値算出部10の機能部である加速度代表値算出部10Aは、演算周期決定部50から出力される速度ベクトル演算周期SBに従って、第1実施形態と同様の手法で、角速度代表値を算出する。つまり、速度ベクトル演算周期SBに対応する時間間隔(Bミリ秒)で統計演算処理を行って加速度代表値を算出し、その算出結果をINS演算部20に出力する。
【0097】
同様に、代表値算出部10の機能部である角速度代表値算出部10Bは、演算周期決定部50から出力される姿勢演算周期SAに従って、第1実施形態と同様の手法で、角速度代表値を算出する。つまり、姿勢演算周期SAに対応する時間間隔(Aミリ秒)で統計演算処理を行って角速度代表値を算出し、その算出結果をINS演算部20に出力する。
【0098】
図9は、この場合におけるINS演算部20の機能構成の一例を示す図である。姿勢情報算出部21は、演算周期決定部50から出力される姿勢演算周期SAに従って、姿勢情報の演算を行う。座標変換部21及び絶対座標速度ベクトル算出部23は、演算周期決定部50から出力される速度ベクトル演算周期SBに従って、座標変換及び絶対座標速度ベクトルの演算を行う。また、絶対座標位置算出部29は、演算周期決定部50から出力される位置演算周期SCに従って、絶対座標位置の演算を行う。
【0099】
最終的に、INS演算部20は、絶対座標位置の演算周期(位置演算周期SC)に対応するCミリ秒の時間間隔でINS演算結果d3をカップリング処理部40に出力する。カップリング処理部40は、Cミリ秒の時間間隔でINS演算部20から入力したINS演算結果d3を入力(制御入力)として、カルマンフィルターの予測演算(時刻更新)を行う。そして、1秒の時間間隔でGPS演算部30からGPS演算結果d7を入力すると、当該GPS演算結果d7を観測情報として用いて、カルマンフィルターの補正演算(観測更新)を行う。
【0100】
2−2.演算周期の決定方法
第2実施形態では、演算周期決定部50が、INS演算部20が慣性航法演算を行う場合の演算周期を変更する。具体的には、演算周期決定部50は、移動体の移動状況として移動量及び方向の変化量を判定し、その判定結果に基づいて、加速度センサー2Aに係る演算周期及びジャイロセンサー2Bに係る演算周期を変更する。
【0101】
図10は、演算周期決定の原理の説明図である。ここでは、演算周期を決定するためのテーブルである演算周期決定用テーブルを図示している。演算周期決定用テーブルには、運動変化と、演算周期とが対応付けて定められている。
【0102】
運動変化には、移動体の方向の変化の大小を定量的に示す値である姿勢変化量と、移動体の位置の変化の大小を定量的に示す値である移動量とが定められている。そして、姿勢変化量及び移動量それぞれについて、変化が大きいものには「大」、変化が小さいものには「小」がそれぞれ定められている。
【0103】
演算周期は、INS演算部20が行う慣性航法演算に係る周期であり、姿勢の演算周期である姿勢演算周期SAと、速度ベクトルの演算周期である速度ベクトル演算周期SBと、位置の演算周期である位置演算周期SCとが定められている。そして、これらそれぞれについて、設定する演算周期の長さに応じて「長」又は「短」が定められている。なお、任意の長さを設定可能なものについては「−」が定められている。
【0104】
姿勢変化量及び移動量が何れも大きい場合は(姿勢変化量「大」 & 移動量「大」)、姿勢の演算周期SAと、速度ベクトルの演算周期SBと、位置の演算周期SCとの全てについて、短い演算周期を設定することが定められている(SA「短」、SB「短」、SC「短」)。姿勢変化量や移動量が大きくなる状況では、移動体の位置や速度ベクトル、姿勢といった諸量が急激に変化するため、各諸量に係る演算周期を短くして、演算精度を向上させる狙いがある。
【0105】
これとは逆に、姿勢変化量及び移動量が何れも小さい場合は(姿勢変化量「小」 & 移動量「小」)、姿勢の演算周期SAと、速度ベクトルの演算周期SBと、位置の演算周期SCとの全てについて、長い演算周期を設定することが定められている(SA「長」、SB「長」、SC「長」)。
【0106】
上記の演算周期の設定方法によれば、移動体の移動量及び方向の変化を判定し、その判定の結果が小さい場合に比べて、大きい場合の方が、演算周期(所定の時間間隔)が短くなるように演算周期を変更することになる。実際の処理としては、判定された移動量及び方向の変化が所定の変化量以上であるか未満であるかに応じて、演算周期を変更する。これは、判定された移動量及び方向の変化が所定の変化量未満の場合に比べて、判定された移動量及び方向の変化が所定の変化量以上の場合の方が、時間間隔が短くなるように所定の時間間隔を変更することに相当する。
【0107】
また、姿勢変化量が大きく、移動量が小さい場合は(姿勢変化量「大」 & 移動量「小」)、姿勢の演算周期SAを短くし、他の演算周期は任意の値(変更無し)とすることが定められている(SA「短」、SB「−」、SC「−」)。移動量は小さいが、姿勢変化量が大きい場合は、移動体の移動方向が大きく変化する状況であるため、姿勢の演算周期のみを短くする。実際の処理としては、判定された姿勢変化量が所定の方向変化量以上であるか未満であるかに応じて、姿勢の演算周期を変更する。これは、判定された方向の変化が所定の方向変化量未満の場合に比べて、判定された方向の変化が所定の方向変化量以上の場合の方が、ジャイロセンサーに係る所定の時間間隔が短くなるように変更することに相当する。
【0108】
また、姿勢変化量が小さく、移動量が大きい場合は(姿勢変化量「小」 & 移動量「大」)、速度ベクトルの演算周期SB及び位置の演算周期SCを短くし、他の演算周期は任意の値とすることが定められている(SA「−」、SB「短」、SC「短」)。つまり、姿勢変化量は小さいが、移動量が大きい場合は、移動体の位置が大きく変化する状況であるため、速度ベクトル及び位置の演算周期を短くする。実際の処理としては、判定された移動量が所定の移動量以上であるか未満であるかに応じて、速度ベクトル及び位置の演算周期を変更する。これは、判定された移動量が所定の移動量未満の場合に比べて、判定された移動量が所定の移動量以上の場合の方が、加速度センサーに係る所定の時間間隔が短くなるように変更することに相当する。
【0109】
演算周期決定部50は、カップリング処理部40から出力されるカップリング結果d9に基づいて、移動体の姿勢変化量及び移動量をそれぞれ算出する。そして、上記の演算周期決定用テーブルを参照し、姿勢変化量及び移動量の算出結果に対応付けて定められた姿勢、速度ベクトル及び位置それぞれの演算周期SA〜SCを読み出して設定する。
【0110】
なお、演算周期の具体的な値は設計事項であり、適宜設定可能であることは勿論である。例えば、上記の演算周期決定用テーブルにおいて「長」が定められた演算周期については、100ミリ秒の時間間隔を設定し、「短」が定められた演算周期については、10ミリ秒の時間間隔を設定するなどすればよい。
【0111】
2−3.慣性航法演算方法
第2実施形態では、上記の演算周期決定の原理に従って決定した演算周期に応じて、慣性航法演算を行う方法を切り替えて慣性航法演算を行う。より具体的には、演算周期が長いほど、演算負荷が高く、算出位置の精度の良い演算方法に従って慣性航法演算を行う。これは、所定の時間間隔の長さに応じて定められた慣性航法演算方法で慣性航法演算を行うことに相当する。
【0112】
慣性航法演算を行う場合、最初に姿勢情報の算出を行う。つまり、ジャイロセンサー2Bによって計測された角速度を積分することで姿勢情報を演算し、その演算結果を用いて、加速度センサー2Aによって計測された加速度を絶対座標系に座標変換する。例えば、姿勢情報を方向余弦行列で書き表す場合、姿勢情報の演算式は、演算周期に相当する時間間隔を変数とする多項式で近似することが可能である。なお、演算式それ自体は公知であるため、数式の記載は省略する。
【0113】
本実施形態では、この姿勢演算に係る多項式の次数を、姿勢演算周期SAに基づいて変更する。具体的には、図10の演算周期決定用テーブルに基づいて、姿勢演算周期SAを設定した場合において、姿勢演算周期SAとして10ミリ秒といった比較的短い時間間隔を設定した場合には、1次の近似式を用いて姿勢情報を演算する。他方、姿勢演算周期SAとして100ミリ秒といった比較的長い時間間隔を設定した場合には、2次の近似式を用いて姿勢情報を演算する。
【0114】
姿勢演算に係る演算式について、2次の近似式は1次の近似式と比べて演算負荷が高い。しかし、2次の近似式は、1次の近似式と比べて姿勢の演算精度が高い。姿勢の演算精度が高いと、ローカル座標加速度を正しく座標変換することができるため、絶対座標加速度ベクトルの算出精度が向上する。これは、絶対座標速度ベクトルの算出精度の向上に繋がり、最終的に位置算出の精度向上に繋がる。
【0115】
従って、上記のように1次及び2次の近似式を切り替えて姿勢演算を行うことは、第1の演算方法と、演算負荷が第1の演算方法に比べて高く、かつ、算出位置の精度が第1の演算方法に比べて良い第2の演算方法との何れかを用いて慣性航法演算を行うことに相当する。つまり、所定の時間間隔(例えば姿勢演算周期)が第1の時間間隔(例えば10ミリ秒)の場合には第1の演算方法(例えば1次近似式)を用いて慣性航法演算を行い、所定の時間間隔が第1の時間間隔より長い第2の時間間隔(例えば100ミリ秒)の場合には第2の演算方法(例えば2次近似式)を用いて慣性航法演算を行うことを意味する。
【0116】
なお、ここでは姿勢演算を一例として説明したが、速度ベクトル演算周期SB及び位置演算周期SCに基づいて、速度ベクトル及び位置の演算方法を変更しながら、移動体の速度ベクトルや位置の演算を行うこととしてもよい。
【0117】
2−4.第2実施例
2−4−1.データ構成
図11は、第2実施例において、図4のカーナビゲーション装置1000の記憶部500に格納されるデータの一例を示す図である。記憶部500には、プログラムとして、処理部100によって第2メイン処理(図12参照)として実行される第2メインプログラム515が格納されている。第2メインプログラム515は、代表値算出プログラム511をサブルーチンとして含む。
【0118】
また、記憶部500には、データとして、代表値算出用テーブル520と、IMUメジャメントデータ530と、GPSメジャメントデータ540と、INS演算データ550と、GPS演算データ560と、カップリングデータ570と、演算周期決定用テーブル580と、姿勢演算式データ590と、演算周期決定用データ595とがこれに含まれる。
【0119】
演算周期決定用テーブル580は、処理部100が演算周期を決定するための用いるテーブルであり、そのテーブル構成例は図10に示した通りである。
【0120】
姿勢演算式データ590は、姿勢の演算式が定められたデータであり、例えば、前述した1次近似式590A及び2次近似式590Bがこれに含まれる。
【0121】
演算周期決定用データ595は、処理部100が演算周期を決定するために用いるデータであり、例えば、前述した姿勢変化量595A及び移動量595Bがこれに含まれる。
【0122】
2−4−2.処理の流れ
図12は、処理部100が、図11の記憶部500に記憶された第2メインプログラム515に従って実行する第2メイン処理の流れを示すフローチャートである。
【0123】
処理部100は、ステップA1を行った後、又は、ステップA17において処理を継続すると判定した場合に、INS演算部20の演算周期を決定する演算周期決定処理を行う(ステップC2)。
【0124】
具体的には、初回の処理では、姿勢、速度ベクトル及び位置の演算周期として、それぞれ所定の初期値を設定する。また、2回目以降の処理では、前回のカップリング結果を用いて、移動体の姿勢変化量595A及び移動量595Bを算出して演算周期決定用データ595に記憶させる。そして、記憶部500に記憶された演算周期決定用テーブル580を参照し、算出した姿勢変化量595A及び移動量595Bに対応する演算周期SA〜SCを読み出して設定する。
【0125】
この場合、ステップA5の後に行うINS演算処理では、ステップC2において決定した姿勢の演算周期SA、速度ベクトルの演算周期SB及び位置の演算周期SCに従って、それぞれ姿勢演算、速度ベクトル演算及び位置演算を行う。
【0126】
例えば、姿勢演算については、姿勢演算周期SAとして短い演算周期を設定した場合は、姿勢演算式データ590に定められた1次近似式590Aに従って姿勢を演算・更新する。それに対し、姿勢演算周期SAとして長い演算周期を設定した場合は、姿勢演算式データ590に定められた2次近似式590Bに従って姿勢を演算・更新する。
【0127】
2−5.作用効果
第2実施形態によれば、運動に係る物理量を検出するセンサーとして、加速度センサー2A及びジャイロセンサー2Bの計測結果を用いて慣性航法演算を行う。この際、第1実施形態と同様に、所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うように設定される対象期間毎に、当該対象期間内に計測された計測結果の変動に基づいて算出方法を変更して計測結果を統計演算処理する。この際、上記の所定の時間間隔を変更する。そして、所定の時間間隔の長さに応じて定められた慣性航法演算方法で慣性航法演算を行う。
【0128】
より具体的には、移動体の方向の変化及び移動量を判定し、その判定結果に基づいて、所定の時間間隔としての姿勢演算周期、速度ベクトル演算周期及び位置演算周期を変更する。特に、方向の変化が大きい場合は、小さい場合と比べて、姿勢の演算周期が短くなるように変更する。また、移動量が大きい場合は、小さい場合と比べて、速度ベクトルの演算周期が短くなるように変更する。これにより、移動量及び方向の変化に応じて適正化した時間間隔で慣性航法演算を行うことが可能となり、慣性航法演算の精度を向上させることができる。
【0129】
例えば、姿勢演算周期が短い場合は、姿勢演算の精度は劣るが、演算負荷の低い1次近似式で近似される姿勢演算方法を用いて姿勢を演算する。それに対し、姿勢演算周期が長い場合は、演算負荷は高いが、姿勢算出の精度が高い2次近似式で近似される姿勢演算方法を用いて姿勢を演算する。これにより、演算周期に応じて慣性航法演算に係る演算周期を適正化することができる。
【0130】
3.変形例
本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明するが、上記の実施例と同一の構成要素については同一の符号を付して説明を省略し、上記の実施例とは異なる部分を中心に説明する。
【0131】
3−1.運動に係る物理量を検出するセンサー
上記の実施形態では、運動に係る物理量を検出するセンサーとして、加速度センサー2A及びジャイロセンサー2Bを例に挙げて説明したが、これら以外のセンサーを用いて物理量を検出することとしてもよい。例えば、自動車に搭載される速度発電機等の速度センサーを移動体に設け、当該速度センサーの計測結果の変動に基づいて算出方法を変更して、上記の実施形態と同様に統計演算処理を行うこととしてもよい。
【0132】
また、上記の実施形態では、加速度センサー2A及びジャイロセンサー2Bを必須の構成要素として説明したが、これらのセンサーのうちの何れか一方のセンサーのみを備える構成としてもよい。つまり、センサーには、少なくともジャイロセンサーが含まれ、移動状況として、少なくとも方向の変化を判定し、その判定結果に基づいて、姿勢演算周期を変更することとしてもよい。また、センサーには、少なくとも加速度センサー2Aが含まれ、移動状況として、少なくとも移動量を判定し、その判定結果に基づいて、速度ベクトル演算周期を変更することとしてもよい。
【0133】
3−2.演算周期の変更
上記の実施形態では、移動体の移動状況の判定として移動量や方向の変化を判定し、その判定結果に基づいて演算周期を変更した。しかし、「停止状況」や「回転状況」、「加減速状況」といった移動体の移動状況に基づいて演算周期を変更することとしてもよい。
【0134】
例えば、移動体の移動状況が「停止状況」である場合は、移動体の位置はそれほど大きくは変化しないと考えられるため、速度ベクトル演算周期や位置演算周期を長くする。移動体の移動状況が「回転状況」である場合は、移動体の姿勢が大きく変化し得るため、姿勢演算周期を短くする。また、移動体の移動状況が「加減速状況」である場合は、移動体の位置が大きく変化し得るため、速度ベクトル演算周期や位置演算周期を短くする。
【0135】
3−3.演算周期
上記の実施形態では、姿勢や速度ベクトル、位置の演算周期を、10ミリ秒といった短い時間間隔と、100ミリ秒といった長い時間間隔との2段階の何れかの時間間隔に設定するものとして説明した。しかし、演算周期として設定可能な時間間隔はこれらに限らず、適宜変更可能であることは勿論である。
【0136】
具体的には、3段以上の多段階で演算周期を設定することも可能である。この場合は、姿勢変化量や移動量に対する閾値範囲を段階的に定めておき、この段階的に定められた閾値範囲それぞれについて演算周期を対応付けて定めておく。そして、算出した姿勢変化量や移動量に対応付けられた演算周期を読み出して設定するようにすればよい。この場合、姿勢演算に着目して説明すると、姿勢演算周期が長くなるほど、姿勢演算に係る近似式の次数を上げていけばよい。
【0137】
例えば、姿勢演算周期の段数を4段とし、第1段階の演算周期を10ミリ秒、第2段階の演算周期を50ミリ秒、第3段階の演算周期を100ミリ秒、第4段階の演算周期を200ミリ秒とする。この場合、演算周期が10ミリ秒である場合は1次近似式、50ミリ秒である場合は2次近似式、100ミリ秒である場合は3次近似式、200ミリ秒である場合は4次近似式といったように、演算周期が長くなるにつれて、姿勢演算に係る演算式の次数を上げていくようにしてもよい。
【0138】
3−4.統計演算処理
上記の実施形態では、ジャイロセンサー等のセンサーの計測結果の変動に基づいて、統計演算処理の種類を変えて代表値を算出するものとして説明したが、計測結果の変動に関わらず固定的な統計演算処理を行って代表値を算出することとしてもよい。例えば、所定期間分のセンサーの計測結果の平均値を算出して代表値とすることとしてもよい。
【0139】
例えば、10ミリ秒毎にセンサーから出力される計測結果のデータを用いて100ミリ秒の時間間隔で代表値を算出して出力する場合を考える。この場合、10ミリ秒毎にセンサーから出力される計測結果を蓄積的に記憶していき、100ミリ秒が経過したタイミングで蓄積された計測結果を読み出して平均値を算出してもよい。つまり、一括処理型の演算方法で平均値を算出してもよい。
【0140】
また、一括処理型の演算方法ではなく、逐次処理型の演算方法で平均値を算出してもよい。つまり、10ミリ秒毎にセンサーから出力される計測結果を用いて平均値を逐次的に演算していき、100ミリ秒が経過したタイミングで、演算した平均値を代表値として出力する構成としてもよい。
【0141】
また、上記の実施形態では、移動体の状況がノイジー状況であると推定される場合において、対象期間に含まれるデータの最大値及び最小値を除外した平均値を求めて代表値としたが、これを次のようにしてもよい。例えば、対象期間に含まれるデータのうち、平均値を中心とする所定の信頼区間(例えば1σや2σ)に含まれるデータの平均値を求めて代表値としてもよい。
【0142】
3−5.カップリング処理
上記の実施形態では、カップリング処理としてカルマンフィルター処理を例に挙げて説明したが、カップリング処理はこれに限られない。例えば、GPS計測結果とINS計測結果との平均演算を行う平均処理をカップリング処理に含めてもよい。平均演算としては、単純な算術平均や幾何平均を適用してもよいし、加重平均を適用してもよい。
【0143】
加重平均を適用する場合において、GPS計測結果をINS計測結果よりも重視したい場合は、GPS計測結果の重みをINS計測結果の重みよりも大きく設定して演算すればよい。逆に、INS計測結果をGPS計測結果よりも重視したいのであれば、GPS計測結果の重みをINS計測結果の重みよりも小さく設定して演算すればよい。
【0144】
3−6.衛星測位ユニット
上記の実施形態では、衛星測位ユニットとして、GPSを適用したGPSユニット3を例に挙げて説明したが、WAAS(Wide Area Augmentation System)、QZSS(Quasi Zenith Satellite System)、GLONASS(GLObal NAvigation Satellite System)、GALILEO等の他の衛星測位システムを適用したユニットとしてもよいことは勿論である。
【0145】
3−7.処理主体
上記の実施例では、電子機器の処理部100が、GPSユニット3から取得したGPSメジャメント情報を用いてGPS演算処理を行うこととして説明した。また、処理部100が、IMU2から取得したINSメジャメント情報を用いてINS演算処理を行うこととして説明した。つまり、GPS演算処理、INS演算処理及びカップリング処理の実行主体が、全て電子機器の処理部100であるものとして説明した。この構成を次のようにしてもよい。
【0146】
GPSユニット3は、GPSメジャメント情報を用いたGPS演算処理を行ってGPS演算結果を求めて、処理部100に出力する。また、IMU2は、INSメジャメント情報を用いたINS演算処理を行ってINS演算結果を求めて、処理部100に出力する。そして、処理部100は、各ユニットから取得したGPS演算結果とINS演算結果とをカップリングするカップリング処理を実行する。
【0147】
つまり、この場合は、GPS演算処理及びINS演算処理の実行主体が、それぞれGPSユニット3及びIMU2となり、カップリング処理(位置算出処理)の実行主体が、電子機器の処理部100となる。
【0148】
3−8.電子機器
上記の実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【0149】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、PDA(Personal Digital Assistant)といった他の電子機器についても本発明を同様に適用して、当該電子機器の位置算出を実現することが可能である。
【符号の説明】
【0150】
1,1A,1B 演算システム、 2 IMU、 2A 加速度センサー、 2B ジャイロセンサー、 5 慣性航法演算装置、 10 代表値算出部、 20 INS演算部、 30 GPS演算部、 40 カップリング処理部、 50 演算周期決定部、 100 処理部、 200 操作部、 300 表示部、 400 音出力部、 500 記憶部、 1000 カーナビゲーション装置

【特許請求の範囲】
【請求項1】
運動に係る物理量を検出するセンサーの計測結果を用いて慣性航法演算を行う方法であって、
所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うように設定される対象期間毎に、当該対象期間内に計測された前記計測結果の変動に基づいて算出方法を変更して前記計測結果を統計演算処理することと、
前記統計演算処理の結果を用いて慣性航法演算を行うことと、
を含む慣性航法演算方法。
【請求項2】
前記所定の時間間隔を変更することを更に含み、
前記慣性航法演算を行うことは、前記所定の時間間隔の長さに応じて定められた慣性航法演算方法で慣性航法演算を行うことを含む、
請求項1に記載の慣性航法演算方法。
【請求項3】
前記慣性航法演算方法には、少なくとも、第1の演算方法と、演算負荷が前記第1の演算方法に比べて高く、かつ、算出位置の精度が前記第1の演算方法に比べて良い第2の演算方法とが含まれ、
前記慣性航法演算を行うことは、前記所定の時間間隔が第1の時間間隔の場合には前記第1の演算方法を用いて慣性航法演算を行い、前記所定の時間間隔が第1の時間間隔より長い第2の時間間隔の場合には前記第2の演算方法を用いて慣性航法演算を行うことである、
請求項2に記載の慣性航法演算方法。
【請求項4】
前記計測結果を用いて移動体の移動状況を判定することを更に含み、
前記変更することは、前記移動状況の判定結果に基づいて前記所定の時間間隔を変更することを含む、
請求項2又は3に記載の慣性航法演算方法。
【請求項5】
前記移動状況を判定することは、移動量及び方向の変化を判定することを含み、
前記変更することは、前記判定された移動量及び方向の変化が所定の変化量未満の場合に比べて、前記判定された移動量及び方向の変化が前記所定の変化量以上の場合の方が、時間間隔が短くなるように前記所定の時間間隔を変更することを含む、
請求項4に記載の慣性航法演算方法。
【請求項6】
前記センサーには、少なくとも加速度センサーが含まれ、
前記移動状況を判定することは、少なくとも移動量を判定することを含み、
前記変更することは、前記判定された移動量が所定の移動量未満の場合に比べて、前記判定された移動量が前記所定の移動量以上の場合の方が、前記加速度センサーに係る前記所定の時間間隔が短くなるように変更することを含む、
請求項5に記載の慣性航法演算方法。
【請求項7】
前記センサーには、少なくともジャイロセンサーが含まれ、
前記移動状況を判定することは、少なくとも方向の変化を判定することを含み、
前記変更することは、前記判定された方向の変化が所定の方向変化量未満の場合に比べて、前記判定された方向の変化が前記所定の方向変化量以上の場合の方が、前記ジャイロセンサーに係る前記所定の時間間隔が短くなるように変更することを含む、
請求項6に記載の慣性航法演算方法。
【請求項8】
運動に係る物理量を検出するセンサーの計測結果を用いて慣性航法演算を行う慣性航法演算装置であって、
所定の時間間隔毎に到来する時刻を跨ぎ、互いに時間的に重なり合うように設定される対象期間毎に、当該対象期間内に計測された前記計測結果の変動に基づいて算出方法を変更して前記計測結果を統計演算処理する統計演算部と、
前記統計演算処理の結果を用いて慣性航法演算を行う慣性航法演算部と、
を備えた慣性航法演算装置。

【図1】
image rotate

【図2】
image rotate

【図3】
image rotate

【図4】
image rotate

【図5】
image rotate

【図6】
image rotate

【図7】
image rotate

【図8】
image rotate

【図9】
image rotate

【図10】
image rotate

【図11】
image rotate

【図12】
image rotate


【公開番号】特開2013−108930(P2013−108930A)
【公開日】平成25年6月6日(2013.6.6)
【国際特許分類】
【出願番号】特願2011−256040(P2011−256040)
【出願日】平成23年11月24日(2011.11.24)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】