JP2016033474A - 位置算出方法及び位置算出装置 - Google Patents

Abstract

【課題】頻繁なキャリブレーションを必要とせずとも、測定精度を一定程度に維持する新たな手法を提案すること。【解決手段】位置算出装置２０において、慣性航法演算部２１０は、ＩＭＵ３０の検出値を用いた慣性航法演算を行って当該装置の位置、速度及び姿勢角を算出し、カルマンフィルター部２２０は、慣性航法演算部２１０による演算結果を用いたカルマンフィルター処理を行って、当該装置の位置や速度、姿勢角等を推定的に算出する。また、ＫＦ初期パラメーター設定部２３０は、前回使用時の環境との差に応じて、カルマンフィルター処理に用いられる誤差指標値である誤差共分散行列Ｐの初期値を設定する。【選択図】図２

Description

本発明は、位置算出用のセンサーの検出結果を用いた位置算出装置等に関する。

加速度センサーやジャイロセンサーといった各種センサーの検出結果を用いて、自律的に移動体の位置や速度、姿勢角等を算出する慣性航法演算が広く利用されている。センサーの出力値には、ゼロ点バイアスに代表されるバイアス（誤差）が含まれており、このバイアスによって、慣性航法演算結果に大きな誤差が生じる可能性がある。このため、慣性航法演算の開始に先立ち、センサーのバイアスを算出（推定）するキャリブレーションを行う必要があった（例えば、特許文献１参照）。

特開２０１２−８０９６号公報

センサーのバイアスには、センサーの出力がゼロであるときのゼロ点バイアスのほか、電源投入時に生じるターンオンバイアスや、電源変動や温度変動などの外部要因に起因するランダムなドリフトが含まれる。このため、厳格にバイアスをキャンセルする場合には、電源を投入して使用を開始する毎にキャリブレーションを行う必要があった。キャリブレーションは、数秒〜数十秒程度の時間を要する。また、キャリブレーションは、ユーザーに、センサーを所定の姿勢として所定の動きをさせる（例えば、停止させる、回転させるなど）のが一般的であった。このため、日常的に使用する装置においてキャリブレーションを実施する場合には、ユーザーの負担となっていた。

また、キャリブレーションを行ったとしても、センサー検出値に含まれるバイアスを完全に算出して除去することはほぼ不可能であり、慣性航法演算結果には僅かながらも誤差が含まれ得た。

本発明は、上記事情に鑑みてなされたものであり、その目的とするところは、頻繁なキャリブレーションを必要とせずとも、測定精度を一定程度に維持する新たな手法を提案することである。

上記課題を解決するための第１の発明は、位置算出用の所定センサーの検出結果を用いて、当該センサーの検出値に係る誤差指標値を推定更新しつつ現在位置を算出する第１の位置算出処理を実行することと、前記第１の位置算出処理の算出時の環境情報を記憶することと、新たに位置算出を行う際の環境情報と前記記憶された環境情報との差に基づいて新たに位置算出を行う際の初期の誤差指標値を設定し、前記センサーの検出結果を用いて、当該誤差指標値の推定更新および現在位置の算出を行う第２の位置算出処理を実行することと、を含む位置算出方法である。

また、他の発明として、位置算出用の所定センサーと、前記センサーの検出値に係る誤差指標値を推定更新しつつ現在位置を算出する位置算出部と、前記位置算出部による前回算出時の環境情報を記憶部に記憶させ、前記位置算出部による新たな算出時の環境情報と当該記憶部に記憶された環境情報との差に基づいて、当該新たな算出時の初期の誤差指標値を設定する設定部と、を備えた位置算出装置を構成しても良い。

この第１の発明等によれば、新たな位置算出の際の環境情報と前回算出時の環境情報との差に基づいて、センサーの検出値に係る初期の誤差指標値を設定することができる。センサーの検出結果は、環境に影響される。このため、前回算出時からの環境変化に応じて誤差指標値を設定することで、検出値に係る誤差を低減させることができる。よって、キャリブレーションを実行せずとも、測定精度を一定程度に維持することが可能となる。

第２の発明として、第１の発明の位置算出方法であって、前記第１の位置算出処理および前記第２の位置算出処理は、前記誤差指標値に基づく誤差共分散行列を用いたカルマンフィルターを使用して現在位置を算出する処理である、位置算出方法を構成しても良い。

この第２の発明によれば、位置算出処理として、誤差指標値に基づく誤差共分散行列を用いたカルマンフィルターを使用して現在位置が算出される。

第３の発明として、第１又は第２の発明の位置算出方法であって、前記誤差指標値を設定することは、新たに位置算出を行う際の環境情報と前記記憶された環境情報との差が所定の許容差条件を満たさず、且つ、前記センサーのキャリブレーションを行わない場合に、新たに位置算出を行う際の初期の誤差指標値を、所定の最大誤差を示す誤差指標値に設定することを含む、位置算出方法を構成しても良い。

この第３の発明によれば、環境情報の差が所定の許容差条件を満たさず、且つ、センサーのキャリブレーションを行わない場合に、新たに位置算出を行う際の初期の誤差指標値を、所定の最大誤差を示す誤差指標値に設定することができる。

第４の発明として、第３の発明の位置算出方法であって、前記第２の位置算出処理を実行することは、初期の誤差指標値を前記最大誤差を示す誤差指標値とする場合には、前記センサーのバイアス誤差をゼロとして位置算出を行うことを含む、位置算出方法を構成しても良い。

この第４の発明によれば、初期の誤差指標値を最大誤差を示す誤差指標値とする場合には、センサーのバイアス誤差をゼロとして位置算出が行われる。

第５の発明として、第１〜第４の何れかの発明の位置算出方法であって、前記環境情報は、前記センサーの環境温度の情報を含み、前記誤差指標値を設定することは、新たに位置算出を行う際の環境温度と前記記憶された環境温度との差に応じて、前記誤差指標値を可変に設定することを含む、位置算出方法を構成しても良い。

この第５の発明によれば、環境情報はセンサーの環境温度の情報を含み、前回算出時の環境温度との差に応じて誤差指標値が可変に設定される。例えば、環境温度の差が大きいほど、誤差指標値を誤差が大きいことを示す値に設定することができる。

第６の発明として、第１〜第５の何れかの発明の位置算出方法であって、前記記憶することは、前記第１の位置算出処理の算出日時を記憶することを含み、前記誤差指標値を設定することは、新たに位置算出を行う際の日時と前記記憶された算出日時との差に応じて、前記誤差指標値を可変に設定することを含む、位置算出方法を構成しても良い。

この第６の発明によれば、環境情報は算出日時を含み、前回算出時の算出日時との差に応じて、誤差指標値が可変に設定される。例えば、算出日時の差が大きいほど、誤差指標値を誤差が大きいことを示す値に設定することができる。

位置算出装置の構成例。 位置算出装置の機能構成図。 位置算出処理のフローチャート。

［システム構成］
図１は、本実施形態における位置算出装置２０の構成例である。この位置算出装置２０は、例えばランニングの際に、ユーザー１０の腰部に装着して利用される携帯型の電子機器である。位置算出装置２０には、操作スイッチ２１やディスプレイ２２、スピーカー２３等が設けられているとともに、ＩＭＵ（Inertial Measurement Unit）３０と、ＣＰＵ（Central Processing Unit）やメモリーを実装した制御装置（不図示）とを内蔵している。

ＩＭＵ３０は、速度センサー及びジャイロセンサーを有する慣性センサーユニットである。加速度センサーは、当該センサーに対応付けられた三次元直交座標系（ｘ，ｙ，ｚ）であるセンサー座標系（ローカル座標系）における加速度を検出する。ジャイロセンサーは、当該センサーに対応付けられた三次元直交座標系（ｘ，ｙ，ｚ）であるセンサー座標系における角速度を検出する。

なお、加速度センサーのセンサー座標系とジャイロセンサーのセンサー座標系とは座標軸が同一として説明するが、異なる場合には、座標変換行列演算を行うことで一方の座標系を他方の座標系に変換することが可能である。座標変換行列演算については、公知の手法を適用することができる。

位置算出装置２０は、ＩＭＵ３０の検出値を用いた慣性航法演算を行って、当該装置の位置や速度、姿勢等を算出することができる。

ところで、ＩＭＵ３０の各センサー（加速度センサー、ジャイロセンサー）の出力値に含まれるバイアスによって、慣性航法演算結果に大きな誤差が生じる可能性がある。このため、予めＩＭＵ３０の各センサーのバイアスを算出（推定）し、算出したバイアスを考慮して慣性航法演算を行う必要がある。従来のバイアスの算出方法としては、例えば電源を投入した直後の当該装置の使用開始時に、ＩＭＵ３０の各センサーのバイアスを算出（推定）して校正するキャリブレーションを行っていた。本実施形態では、前回使用時の環境との違いをもとに、キャリブレーションが必要であるかを判断し、必要と判断した場合にキャリブレーションを行い、必要でないと判断した場合には、前回使用時のバイアスをそのまま用いる。

また、慣性航法演算による位置算出にカルマンフィルター処理を用いるが、このカルマンフィルター処理で用いられる誤差指標値である誤差共分散行列の初期値を、前回使用時の環境との違いに応じて設定する。

［機能構成］
図２は、位置算出装置２０の機能構成図である。図２によれば、位置算出装置２０は、センサー部４０と、測位ユニット５０と、操作部１１０と、表示部１２０と、音出力部１３０と、通信部１４０と、時計部１５０と、処理部２００と、記憶部３００とを備えて構成される。

センサー部４０は、位置算出用の所定センサーであるＩＭＵ３０と、環境センサーである温度センサー４２とを備える。ＩＭＵ３０は、加速度センサー３１と、ジャイロセンサー３２とを有する。センサー部４０の各センサー（加速度センサー３１、ジャイロセンサー３２、温度センサー４２）による検出値は、処理部２００に出力され、センサーデータ３２０として蓄積記憶される。

測位ユニット５０は、衛星測位システムを利用して測位を行うユニットであり、測位衛星から発信されている衛星信号を受信する受信アンテナや、受信信号を処理するＲＦ（Radio Frequency）回路、ベースバンド処理回路等を有して構成される。例えば、ＧＰＳ（Global Positioning System）ユニットによって実現される。

操作部１１０は、例えばタッチパネルやボタンスイッチ等の入力装置で実現され、なされた操作に応じた操作信号を処理部２００に出力する。図１の操作スイッチ２１がこれに該当する。表示部１２０は、例えばＬＣＤ（Liquid Crystal Display）等の表示装置で実現され、処理部２００からの表示信号に基づく各種表示を行う。図１のディスプレイ２２がこれに該当する。音出力部１３０は、例えばスピーカー等の音出力装置で実現され、処理部２００からの音信号に基づく各種音出力を行う。図１のスピーカー２３がこれに該当する。

通信部１４０は、無線ＬＡＮ（Local Area Network）やＢｌｕｅｔｏｏｔｈ（登録商標）等の無線通信装置によって実現され、外部装置との通信を行う。時計部１５０は、位置算出装置２０の内部時計であり、水晶発振器等を有する発振回路によって構成され、計時した現在時刻や、指定タイミングからの経過時間等の時間信号を、処理部２００に出力する。

処理部２００は、例えばＣＰＵ等の演算装置で実現され、記憶部に記憶されたプログラムやデータ、操作部からの操作信号等に基づいて、位置算出装置の全体制御を行う。また、処理部２００は、慣性航法演算部２１０と、カルマンフィルター部２２０と、ＫＦ初期パラメーター設定部２３０と、バイアス設定部２４０とを有し、位置算出プログラム３１０に従った位置算出処理（図３参照）を実行する。処理部２００は、位置算出部及び設定部に相当する機能部である。具体的には、慣性航法演算部２１０及びカルマンフィルター部２２０が位置算出部に相当し、ＫＦ初期パラメーター設定部２３０及びバイアス設定部２４０が設定部に相当する。

慣性航法演算部２１０は、ＩＭＵ３０の検出値（加速度センサー３１による加速度、ジャイロセンサー３２による角速度）を用いた公知の慣性航法演算を行って、絶対座標系（Ｘ，Ｙ，Ｚ）における当該装置の位置、速度及び姿勢角を算出する。また、この慣性航法演算では、ＩＭＵ３０の検出値を、バイアス設定部２４０によって設定されるバイアス値で補正した値を用いる。

カルマンフィルター部２２０は、カルマンフィルターの理論に基づく演算処理（カルマンフィルター処理）を行って、当該装置の位置や速度、姿勢角等を算出する。具体的には、慣性航法演算部２１０による位置、速度及び姿勢角を制御入力Ｕとし、測位ユニット５０による位置及び速度を観測量Ｚとし、位置、速度及び姿勢角を状態Ｘとする。そして、カルマンフィルターの予測演算（時刻更新）及び補正演算（観測更新）を繰り返し行って、状態Ｘの各成分の誤差の指標値である誤差共分散行列Ｐを推定更新しつつ、状態Ｘの推定値（状態推定値）を算出する。

ＫＦ初期パラメーター設定部２３０は、カルマンフィルター部２２０が行うカルマンフィルター処理に用いられる各種のパラメーターの一つである誤差共分散行列Ｐの初期値（以下、「ＫＦ初期パラメーター」という）を設定する。具体的には、当該装置を前回最後に使用したときである位置算出処理の前回実行時（以下単に「前回使用時」という）の環境値と、今回新たに当該装置を使用して新たに位置算出処理を実行するとき（以下単に「今回使用時」という）の環境値との差に応じて、ＫＦ初期パラメーターを設定する。例えば、日時差が大きいほど、或いは、温度差が大きいほど、ＫＦ初期パラメーターが誤差の許容度合の大きな値になるように設定する。また、後述の許容差条件を満たさず、且つ、キャリブレーションを行わない場合には、ＫＦ初期パラメーターを、誤差の許容度合が最も大きな値に設定する。

バイアス設定部２４０は、ＩＭＵ３０のバイアスを設定する。具体的には、前回使用時の環境値と今回使用時の環境値との差が所定の許容差条件を満たすかを判定する。環境値は、温度及び日時である。温度は、温度センサー４２によって検出され、日時は、時計部１５０によって計時される。前回使用時の環境値は、前回使用時データ３３０として記憶されている。許容差条件は、環境の変化が小さいとみなせる条件であり、例えば、「日時差が３０日以内」、「温度差が１５度以内」といったように定められる。

許容差条件を満たす場合には、前回使用時のバイアス値を、そのまま今回使用時のバイアス値として設定する。前回使用時のバイアス値は、前回使用時データ３３０として記憶されている。許容差条件を満たさない場合には、キャリブレーションを行い、キャリブレーションによって得られたバイアス値を設定する。また、許容差条件を満たさず、且つ、キャリブレーションを行わない場合には、バイアス値を「０」に設定する。

キャリブレーションは、例えば、次のように行う。すなわち、表示部１２０に所定メッセージを表示させる等によって、ユーザーに、当該装置を所定の姿勢（例えば、センサー座標系のｚ軸が鉛直下向きとなる姿勢）で装着し、所定の動作（例えば、静止する、その場で自転（時計回り方向或いは反時計回り方向に回転）する等）を行うように指示し、そのときにカルマンフィルター部２２０によって算出された位置、速度及び姿勢角をもとに、ＩＭＵ３０の各センサーのバイアスを算出する。このキャリブレーション自体は公知の技術を適用することができる。

記憶部３００は、ＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）等の記憶装置で実現され、処理部２００が位置算出装置２０を統合的に制御するためのプログラムやデータ等を記憶しているとともに、処理部２００の作業領域として用いられ、処理部２００が各種プログラムに従って実行した演算結果や、センサー部４０からの検出値等が一時的に格納される。本実施形態では、記憶部３００には、位置算出プログラム３１０と、センサーデータ３２０と、前回使用時データ３３０とが記憶される。

［処理の流れ］
図３は、位置算出処理の流れを説明するフローチャートである。この処理は、当該装置の電源投入後に開始される処理であり、処理部２００が位置算出プログラム３１０に従って実行する処理である。

先ず、処理部２００は、前回使用時の環境値及びバイアス値（以下包括して「前回使用値」という）が記憶されているかを判断する。前回使用値が記憶されているならば（ステップＳ１：ＹＥＳ）、ＫＦ初期パラメーター設定部２３０が、記憶されている前回使用時の環境値と、今回使用時の環境値との差に応じて、今回のＫＦ初期パラメーターを算出する（ステップＳ３）。

次いで、前回使用時の環境値と今回使用時の環境値との差が、所定の許容差条件を満たすかを判断する。許容差条件を満たすならば（ステップＳ５：ＹＥＳ）、バイアス設定部２４０が、記憶されている前回使用時のバイアス値を、今回使用時のバイアス値として慣性航法演算部２１０に設定する（ステップＳ７）。また、ＫＦ初期パラメーター設定部２３０がステップＳ３で算出したＫＦ初期パラメーターを、今回のＫＦ初期パラメーターとしてカルマンフィルター部２２０に設定する（ステップＳ９）。

一方、前回値が記憶されていない（ステップＳ１：ＮＯ）、或いは、許容差条件を満たさないならば（ステップＳ５：ＮＯ）、処理部２００は、表示部１２０に所定メッセージを表示する等して、ユーザーに対してキャリブレーションを行うかを問い（ステップＳ１１）、ユーザーの操作入力に従って、キャリブレーションを行うかを決定する。

キャリブレーションを行わないならば（ステップＳ１３：ＮＯ）、バイアス設定部２４０が、初期バイアス値を「０」として慣性航法演算部２１０に設定する（ステップＳ１５）。また、ＫＦ初期パラメーター設定部２３０が、ＫＦ初期パラメーターを誤差の許容度合を最も大きくする所定の最大値としてカルマンフィルター部２２０に設定する（ステップＳ１７）。

一方、キャリブレーションを行うならば（ステップＳ１３：ＹＥＳ）、ＫＦ初期パラメーター設定部２３０は、今回のＫＦ初期パラメーターを算出しているか（ステップＳ３を実行しているか）を判断する。算出しているならば（ステップＳ１９：ＹＥＳ）、ステップＳ３で算出したＫＦ初期パラメーターを、今回のＫＦ初期パラメーターとしてカルマンフィルター部２２０に設定する（ステップＳ２１）。算出していないならば（ステップＳ１９：ＮＯ）、所定のキャリブレーション用のパラメーター値である校正用初期値を、新たなＫＦ初期パラメーターとしてカルマンフィルター部２２０に設定する（ステップＳ２３）。

その後、バイアス設定部２４０は、キャリブレーション処理を行い（ステップＳ２５）、キャリブレーションによって算出されたバイアス値を、今回のバイアス値として慣性航法演算部２１０に設定する（ステップＳ２７）。また、ＫＦ初期パラメーター設定部２３０が、所定の測位用のパラメーター値を、新たなＫＦ初期パラメーターとしてカルマンフィルター部２２０に設定する（ステップＳ２９）。

ステップＳ９又はＳ２９の後、処理部２００は測位処理を行う（ステップＳ３１）。測位処理では、カルマンフィルター部２２０によって算出された位置、速度を、測位結果とする。測位処理が終了すると、処理部２００は、測位処理の終了時のバイアス値と、現在の環境値とを、前回値として記憶部３００に記憶する（ステップＳ３３）。以上の処理を行うと、位置算出処理を終了する。

［作用効果］
このように、本実施形態の位置算出装置２０によれば、前回使用時の環境値と今回使用時の環境値との差が所定の許容差条件を満たす場合には、キャリブレーションを不要として前回使用時のバイアス値を利用するとともに、前回使用時の環境値と今回使用時の環境値との差に応じて誤差指標値である誤差共分散の初期値を設定して、カルマンフィルターを利用した慣性航法演算を行う。これにより、キャリブレーションを実行せずとも、測定精度を一定程度に維持することが可能となる。

［変形例］
なお、本発明を適用可能な実施形態は上述の実施形態に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能なのは勿論である。

例えば、バイアス設定部２４０によるバイアスの設定先を慣性航法演算部２１０として説明したが、カルマンフィルター部２２０もバイアスの設定先としてもよい。すなわち、カルマンフィルター処理における状態Ｘの要素の中にＩＭＵ３０のセンサーバイアス（加速度バイアス、角速度バイアス）を含め、このセンサーバイアスの値を、バイアス設定部２４０が設定した値とすることとしてもよい。

また、許容差条件を温度及び日時としたが、何れか一方のみとしても良いし、これ以外としても良い。

また、カルマンフィルター処理における状態Ｘの要素は一例であり、別の要素を状態Ｘとしてもよいことは勿論である。

１０ ユーザー、２０ 位置算出装置、１１０ 操作部、１２０ 表示部、１３０ 音出力部、１４０ 通信部、１５０ 時計部、２００ 処理部、２１０ 慣性航法演算部、２２０ カルマンフィルター部、２３０ ＫＦ初期パラメーター設定部、２４０ バイアス設定部、３００ 記憶部、３１０ 位置算出プログラム、３２０ センサーデータ、３３０ 前回値データ、４０ センサー部、３０ ＩＭＵ、３１ 加速度センサー、３２ ジャイロセンサー、４２ 温度センサー、５０ 測位ユニット

Claims (7)

1. 位置算出用の所定センサーの検出結果を用いて、当該センサーの検出値に係る誤差指標値を推定更新しつつ現在位置を算出する第１の位置算出処理を実行することと、
   前記第１の位置算出処理の算出時の環境情報を記憶することと、
   新たに位置算出を行う際の環境情報と前記記憶された環境情報との差に基づいて新たに位置算出を行う際の初期の誤差指標値を設定し、前記センサーの検出結果を用いて、当該誤差指標値の推定更新および現在位置の算出を行う第２の位置算出処理を実行することと、
   を含む位置算出方法。
2. 前記第１の位置算出処理および前記第２の位置算出処理は、前記誤差指標値に基づく誤差共分散行列を用いたカルマンフィルターを使用して現在位置を算出する処理である、
   請求項１に記載の位置算出方法。
3. 前記誤差指標値を設定することは、新たに位置算出を行う際の環境情報と前記記憶された環境情報との差が所定の許容差条件を満たさず、且つ、前記センサーのキャリブレーションを行わない場合に、新たに位置算出を行う際の初期の誤差指標値を、所定の最大誤差を示す誤差指標値に設定することを含む、
   請求項１又は２に記載の位置算出方法。
4. 前記第２の位置算出処理を実行することは、初期の誤差指標値を前記最大誤差を示す誤差指標値とする場合には、前記センサーのバイアス誤差をゼロとして位置算出を行うことを含む、
   請求項３に記載の位置算出方法。
5. 前記環境情報は、前記センサーの環境温度の情報を含み、
   前記誤差指標値を設定することは、新たに位置算出を行う際の環境温度と前記記憶された環境温度との差に応じて、前記誤差指標値を可変に設定することを含む、
   請求項１〜４の何れか一項に記載の位置算出方法。
6. 前記記憶することは、前記第１の位置算出処理の算出日時を記憶することを含み、
   前記誤差指標値を設定することは、新たに位置算出を行う際の日時と前記記憶された算出日時との差に応じて、前記誤差指標値を可変に設定することを含む、
   請求項１〜５の何れか一項に記載の位置算出方法。
7. 位置算出用の所定センサーと、
   前記センサーの検出値に係る誤差指標値を推定更新しつつ現在位置を算出する位置算出部と、
   前記位置算出部による前回算出時の環境情報を記憶部に記憶させ、前記位置算出部による新たな算出時の環境情報と当該記憶部に記憶された環境情報との差に基づいて、当該新たな算出時の初期の誤差指標値を設定する設定部と、
   を備えた位置算出装置。

Images