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

Abstract

【課題】装置の拡張や変更等に対して柔軟に対応可能な位置算出装置を実現するための手法を提案すること。
【解決手段】位置算出装置の一種である位置算出システム１には、ＩＭＵ５０と、ＧＰＳユニット１００を組み込み可能な第１スロット６０及び第２スロット７０とが具備されている。そして、何れのスロットにもＧＰＳユニット１００が組み込まれていない場合には、ＩＭＵ５０の計測結果を用いて位置を算出し、何れかのスロットにＧＰＳユニット１００組み込まれた場合には、ＩＭＵ５０の計測結果とＧＰＳユニット１００の計測結果とを用いて位置を算出する。
【選択図】図１

Description

本発明は、位置算出方法及び位置算出装置に関する。

測位用信号を利用した測位システムとしては、ＧＰＳ（Global Positioning System）が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された位置算出装置に利用されている。ＧＰＳでは、複数のＧＰＳ衛星の位置や各ＧＰＳ衛星から位置算出装置までの擬似距離等の情報に基づいて位置算出装置の位置を示す３次元の座標値と時計誤差とを求める位置算出処理を行う。

また、位置算出の正確性を向上させることを目的として、加速度センサーやジャイロセンサーといった慣性航法用のセンサーの計測結果を利用してＧＰＳにより算出された位置を補正して、位置算出を行う技術が考案されている（例えば、特許文献１）。

特表２００３−５０９６７１号公報

しかし、従来の位置算出装置は、例えば特許文献１に開示されているように、ＧＰＳの受信回路と各種センサーとを搭載することが前提の装置であり、両者の計測結果を利用した予め定められた演算処理を行って位置算出を行うように構成されている。そのため、装置の拡張や変更等に融通が利かない固定的なシステムであった。

本発明は、上述した課題に鑑みて為されたものであり、その目的とするところは、装置の拡張や変更等に対して柔軟に対応可能な位置算出装置を実現するための手法を提案することにある。

以上の課題を解決するための第１の形態は、慣性計測ユニットを備え、更に、位置算出に利用可能な物理的事象を検出する少なくとも１つのセンサーユニットを組み込み可能に構成された位置算出装置が行う位置算出方法であって、前記センサーユニットが組み込まれている場合に、前記センサーユニットの種類を認識することと、前記センサーユニットが組み込まれていない場合に、前記慣性計測ユニットの出力を用いて位置を算出することと、前記センサーユニットが組み込まれている場合に、前記認識された前記センサーユニットの種類に応じて、前記センサーユニットの出力と前記慣性計測ユニットの出力とを用いて位置を算出することと、を含む位置算出方法である。

また、他の形態として、慣性計測ユニットと、位置算出に利用可能な物理的事象を検出するセンサーユニットを組み込み可能な組み込み部と、前記センサーユニットが組み込まれた場合に、前記センサーユニットの種類を認識する認識部と、前記センサーユニットが組み込まれていない場合には、前記慣性計測ユニットの出力を用いて位置を算出し、前記センサーユニットが組み込まれた場合には、前記認識された前記センサーユニットの種類に応じて、前記センサーユニットの出力と、前記慣性計測ユニットの出力とを用いて位置を算出する位置算出部と、を備えた位置算出装置を構成してもよい。

この第１の形態等によれば、位置算出に利用可能な物理的事象を検出する少なくとも１つのセンサーユニットが組み込まれた場合に、組み込まれたセンサーユニットの種類を認識する。そして、センサーユニットが組み込まれていない場合は、慣性計測ユニットの出力を用いて位置を算出し、センサーユニットが組み込まれた場合は、認識されたセンサーユニットの種類に応じて、当該センサーユニットの出力と、慣性計測ユニットの出力とを用いて位置を算出する。これにより、センサーユニットの組み込み有無に応じて適切な演算処理を行って位置算出を行うことが可能となり、装置の拡張や変更等に対して柔軟に対応することができる。

また、第２の形態として、第１の形態の位置算出方法であって、前記センサーユニットの較正を行うときに用いる較正基準情報の種別を表す較正基準種別情報を取得することを更に含み、前記センサーユニットが組み込まれている場合に、前記較正基準種別情報と前記慣性計測ユニットの出力とに基づいて、前記較正基準情報を算出することと、前記較正基準情報を用いて前記センサーユニットの較正を行うことと、を含む位置算出方法を構成してもよい。

この第２の形態によれば、センサーユニットの較正を行うときに用いる較正基準情報の種別を表す較正基準種別情報を取得する。そして、センサーユニットが組み込まれている場合に、取得した較正基準種別情報と慣性計測ユニットの出力とに基づいて較正基準情報を算出し、当該較正基準情報を用いてセンサーユニットの較正を行う。センサーユニットが組み込まれている場合に、当該センサーユニットの較正を行うために必要な情報の種別を当該センサーユニットから取得し、取得した種別の情報を算出して較正に使用することで、組み込まれたセンサーユニットの較正を適切に行うことが可能となる。

また、第３の形態として、第１又は第２の形態の位置算出方法であって、前記センサーユニットが組み込まれた場合に、前記センサーユニットの出力を用いたフィルター処理によって位置を算出することを更に含む位置算出方法を構成してもよい。

この第３の形態によれば、センサーユニットが組み込まれた場合に、センサーユニットの出力を用いたフィルター処理によって位置を算出するため、より正確な位置を算出することが可能となる。

この場合、第４の形態として、第３の形態の位置算出方法であって、前記センサーユニットは、位置算出用衛星からの衛星信号を用いて位置算出が可能な衛星位置算出ユニットであり、前記フィルター処理によって位置を算出することは、前記衛星位置算出ユニットが算出する位置以外の値を用いて位置を予測することと、前記予測した位置を、前記衛星位置算出ユニットが算出する位置に基づいて補正することと、を含む位置算出方法を構成することも可能である。

この第４の形態によれば、センサーユニットとして衛星位置算出ユニットが組み込まれた場合に、フィルター処理として、衛星位置算出ユニットが算出する位置以外の値を用いて位置を予測し、予測した位置を、衛星位置算出ユニットが算出する位置に基づいて補正する。衛星位置算出ユニットの算出位置以外の値を用いて算出した予測位置を、衛星位置算出ユニットの算出位置に基づいて補正することで、より真位置に近い位置を算出することができる。

また、第５の形態として、第４の形態の位置算出方法であって、前記衛星位置算出ユニットが位置を算出する時間間隔を表す情報を取得することを更に含み、前記予測することは、前記時間間隔の間に繰り返し位置を予測することを含み、前記補正することは、前記予測した位置を前記時間間隔で補正することを含む位置算出方法を構成してもよい。

この第５の形態によれば、衛星位置算出ユニットが組み込まれている場合に、衛星位置算出ユニットが位置を算出する時間間隔を表す情報を取得する。そして、フィルター処理では、当該時間間隔の間に繰り返し位置を予測し、予測した位置を当該時間間隔で補正する。これにより、衛星位置算出ユニットが位置を１回取得する間に、衛星位置算出ユニットの算出位置以外の値を利用して位置の予測を複数回行うことが可能となる。

位置算出システムの構成を示すブロック図。 認識処理の流れを示すフローチャート。 位置算出処理の流れを示すフローチャート。 位置算出処理の流れを示すフローチャート。 観測値テーブルのテーブル構成の一例を示す図。 ＧＰＡ位置統合処理の流れを示すフローチャート。 ＧＰＡ予測処理の流れを示すフローチャート。 ＧＰＡ補正処理の流れを示すフローチャート。 慣性航法演算結果較正処理の流れを示すフローチャート。 ＩＭＵ予測処理の流れを示すフローチャート。 ＩＭＵ補正処理の流れを示すフローチャート。 変形例における位置算出システムの構成を示すブロック図。 変形例における観測値テーブルのテーブル構成の一例を示す図。 変形例における観測値テーブルのテーブル構成の一例を示す図。

以下、図面を参照して、本発明の好適な実施形態について説明する。尚、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。

１．システム構成
図１は、本実施形態における位置算出システム１（位置算出装置）の機能構成を示す図である。位置算出システム１は、処理部１０と、入力部２０と、表示部３０と、通信部４０と、ＩＭＵ（Inertial Measurement Unit）５０と、第１スロット６０と、第２スロット７０とを備えて構成されるコンピューターシステムである。処理部１０と、入力部２０と、表示部３０と、通信部４０とは、第１データバス８０を介して接続されている。また、処理部１０と、ＩＭＵ５０と、第１スロット６０と、第２スロット７０とは、第２データバス９０を介して接続されている。

位置算出システム１は、位置算出装置の一種であり、例えばスマートフォンを含む携帯型電話機やノート型パソコン、ＰＤＡ（Personal Digital Assistant）、カーナビゲーション装置、携帯型ナビゲーション装置といったＧＰＳ（Global Positioning System）ユニットを搭載可能な各種の電子機器に適用されるシステム（装置）である。

処理部１０は、位置算出システム１を統括的に制御する制御装置であり、その機能部として、ＩＭＵ用位置算出部１１と、スロット用Ｉ／Ｆ（Inter Face）部１３と、スロット用制御部１５と、位置情報統合処理部（以下、「ＧＰＡ（Generic Position Algorithm）」と称す。）１７とを備えて構成されている。

ＩＭＵ用位置算出部１１は、ＩＭＵ５０から出力される加速度及び角速度のデータを利用して、慣性航法演算処理を行って位置や速度を算出する機能部である。この機能は、例えばＣＰＵ（Central Processing Unit）やＤＳＰ（Digital Signal Processor）等のプロセッサーにより実現される。ＩＭＵ用位置算出部１１は、スロット用制御部１５との間で各種データのやり取りを行う。

スロット用Ｉ／Ｆ部１３は、第１スロット６０及び第２スロット７０に組み込まれたユニット（以下、「組込ユニット」と称す。）との間の入出力インターフェースを実現するための機能部である。データの流れとしては、スロット用Ｉ／Ｆ部１３は、組込ユニットとスロット用制御部１５との間を橋渡す役目を担う。

スロット用制御部１５は、スロット用Ｉ／Ｆ部１３を介して組込ユニットから出力情報を取得したり、ＩＭＵ用位置算出部１１が位置算出を行うための情報を算出してＩＭＵ用位置算出部１１に出力したり、ＩＭＵ用位置算出部１１からの出力情報やＧＰＡ１７からの出力情報を用いて、組込ユニットを較正するための基準となる情報（以下、「較正基準情報」と称す。）を算出したりする処理を行う機能部である。この機能は、例えばＣＰＵやＤＳＰ等のプロセッサーにより実現される。

ＧＰＡ１７は、ＩＭＵ用位置算出部１１からの出力情報及びスロット用制御部１５からの出力情報を用いて、所定のフィルター処理を行って位置や速度を統合・算出する機能部である。この機能は、例えばＣＰＵやＤＳＰ等のプロセッサーにより実現される。本実施形態では、ＧＰＡ１７は、フィルター処理の一例として、カルマンフィルターを利用した位置統合処理を行って位置を算出する。

入力部２０は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部１０に出力する。この入力部２０からの操作入力により、位置算出要求等の各種指示入力がなされる。

表示部３０は、ＬＣＤ（Liquid Crystal Display）等により構成され、処理部１０から入力される表示信号に基づいた各種表示を行う表示装置である。表示部３０には、位置表示画面や時刻情報等が表示される。

通信部４０は、処理部１０の制御に従って、位置算出システム１が外部装置との間で通信を行うための通信装置である。この機能は、例えば、有線通信用のモジュールや、IEEE802.11による無線ＬＡＮやスペクトラム拡散方式による無線通信等の無線通信モジュール等により実現される。また、位置算出システム１を携帯型電話機に適用するケースでは、通信部４０は携帯電話用の通信装置でもよい。

ＩＭＵ５０は、加速度センサー５１及びジャイロセンサー５３を備え、直交３軸それぞれの軸方向の加速度及び各軸の軸回りの角速度を検出可能に構成された慣性計測ユニットである。加速度センサー５１及びジャイロセンサー５３は、それぞれ加速度及び角速度を計測する。また、計測した加速度に含まれ得る誤差（以下、「加速度誤差」と称す。）及び角速度に含まれ得る誤差（以下、「角速度誤差」と称す。）を計測する、或いは、これらの値が予め設定・記憶されている。

ＩＭＵ５０は、加速度、角速度、加速度誤差及び角速度誤差と、これらの計測を行った時刻（計測時刻）とを、ＩＭＵ用位置算出部１１に出力する。本実施形態では、ＩＭＵ５０は、「０．０１秒」の時間間隔でデータを出力する。すなわち、ＩＭＵ５０のデータの出力時間間隔は「０.０１秒」である。尚、加速度センサー５１及びジャイロセンサー５３は、それぞれが独立したセンサーであってもよいし、一体型のセンサーであってもよい。

第１スロット６０及び第２スロット７０は、位置算出用のユニットや、各種センサーユニットを組込可能に構成された一種のソケットとも言える。本実施形態では、これらのスロットには、位置算出に利用可能な物理的事象を検出するセンサーユニットの一例として、ＧＰＳユニット１００又は方位センサーユニット１１０が組み込まれる。

ＧＰＳユニット１００は、衛星位置算出ユニットの一種であり、位置算出用衛星の一種であるＧＰＳ衛星から発信されているＧＰＳ衛星信号を利用して、所定の位置算出計算を行って位置を算出するユニットである。ＧＰＳユニット１００は、第１スロット６０又は第２スロット７０への組込時に、位置算出システム１との間でデータの授受を行う組込時用処理部１０１を備えて構成される。

方位センサーユニット１１０は、例えば地磁気センサーを備えて構成され、地磁気を検出することによって磁方位を検出するように構成されたユニットである。方位センサーユニット１１０も、ＧＰＳユニット１００と同様、組込時用処理部１１１を備えて構成されている。

２．処理の流れ
図２は、位置算出システム１が行う組込ユニットの認識処理の流れを示すフローチャートである。ここでは、第１スロット６０にＧＰＳユニット１００が、第２スロット７０に方位センサーユニット１１０がそれぞれ組み込まれた場合を例に挙げて説明する。図の左側にスロット用制御部１５が行う処理の流れを、図の中央にＧＰＳユニット１００の組込時用処理部１０１が行う処理の流れを、図の右側に方位センサーユニット１１０の組込時用処理部１１１が行う処理の流れをそれぞれ示している。

先ず、スロット用制御部１５は、第１スロット６０又は第２スロット７０へのユニットの組み込み有無を判定し（ステップＡ１）、組み込まれていると判定した場合は（ステップＡ１；Ｙｅｓ）、当該スロットに組み込まれたユニットに対して、ユニット情報の問い合わせを行う（ステップＡ３）。第１スロット６０及び第２スロット７０へのユニット組み込みを検出する方法は、例えば、ユニットが組み込まれることによる電気的な接続を感知する方法等により実現することができる。

スロット用制御部１５からの問い合わせを受けて、第１スロット６０に組み込まれたＧＰＳユニット１００の組込時用処理部１０１は、識別コードと、出力仕様情報と、較正基準種別情報と、タイミング情報とを、ユニット情報としてスロット用制御部１５に通知する（ステップＢ１〜Ｂ７）。

識別コードは、各ユニットの種類をユニークに識別するための識別情報であり、予めユニットの種類別に割り当てられている。出力仕様情報は、当該ユニットが出力可能なデータの種別を示す情報である。較正基準種別情報は、当該ユニットの較正（キャリブレーション）を行うために必要となるデータの種別を示す情報である。較正基準種別情報で表される種別のデータ（当該種別のパラメーター値）のことを「較正基準情報」と称し、較正基準種別情報と区別する。また、タイミング情報は、当該ユニットがデータを出力する時間間隔である出力時間間隔と、当該ユニットが較正を行う時間間隔である較正時間間隔とを含む情報である。

本実施形態において、ＧＰＳユニット１００の組込時用処理部１０１は、識別コードとして「ＧＰＳ」を通知する（ステップＢ１）。また、ＧＰＳユニット１００の出力仕様情報として「位置、速度、時刻、位置誤差、速度誤差」を通知する（ステップＢ３）。すなわち、ＧＰＳユニット１００が出力可能なデータの種類は、位置、速度、時刻、位置誤差及び速度誤差である。

また、ＧＰＳユニット１００の組込時用処理部１０１は、較正基準種別情報として「ドップラー速度、位置、速度、速度誤差」を通知する（ステップＢ５）。すなわち、ＧＰＳユニット１００がキャリブレーションのために必要なデータの種類は、ドップラー速度、位置、速度及び速度誤差である。

また、ＧＰＳユニット１００の組込時用処理部１０１は、タイミング情報として「出力時間間隔：１秒、較正時間間隔：０．１秒」を通知する（ステップＢ７）。すなわち、ＧＰＳユニット１００は、データを１秒間隔で出力し、キャリブレーションを０．１秒間隔で行うことになる。

同様に、方位センサーユニット１１０の組込時用処理部１１１は、識別コードとして「方位センサー」を通知する（ステップＣ１）。また、方位センサーユニット１１０の出力仕様情報として「方位」を通知し（ステップＣ３）、較正基準種別情報として「位置」を通知する（ステップＣ５）。また、組込時用処理部１１１は、タイミング情報として「出力時間間隔：０．０１秒、較正時間間隔：１秒」を通知する（ステップＣ７）。本実施形態において特徴的であることの１つは、ユニット毎に出力時間間隔及び較正時間間隔を決めることができる点である。

各スロットに組み込まれたユニットからユニット情報の通知を受けると、スロット用制御部１５は、組込ユニットの認識が完了したか否かを判定し（ステップＡ５）、まだ完了していないと判定した場合は（ステップＡ５；Ｎｏ）、ステップＡ３に戻る。また、認識が完了したと判定した場合は（ステップＡ５；Ｙｅｓ）、認識処理を終了する。

図３及び図４は、本実施形態における位置算出処理の流れを示すフローチャートである。ここでも、第１スロット６０にＧＰＳユニット１００が、第２スロット７０に方位センサーユニット１１０がそれぞれ組み込まれた場合を例に挙げて説明する。図の左側から右側に向かって、ＧＰＡ１７が行う処理の流れ、ＩＭＵ用位置算出部１１が行う処理の流れ、スロット用制御部１５が行う処理の流れ、ＩＭＵ５０が行う処理の流れ、第１スロット６０に組み込まれたＧＰＳユニット１００の組込時用処理部１０１が行う処理の流れ、第２スロット７０に組み込まれた方位センサーユニット１１０の組込時用処理部１１１が行う処理の流れをそれぞれ示している。

先ず、スロット用制御部１５は、各組込ユニットから取得したユニット情報をＧＰＡ１７に出力する（ステップＦ１）。ＧＰＡ１７は、スロット用制御部１５からユニット情報を取得すると、当該ユニット情報を用いて観測値テーブルを生成する（ステップＤ１）。観測値テーブルは、ＧＰＡ１７が後述するＧＰＡ位置統合処理において、カルマンフィルターを利用した補正処理を行う際に観測値（外部観測量）として使用するパラメーターが設定されたテーブルである。

図５は、観測値テーブルのテーブル構成の一例を示す図である。観測値テーブルには、観測値の候補となる複数のパラメーターの種別と対応付けて、ユニット毎に、当該パラメーターの観測値としての使用有無が設定されている。観測値として使用するパラメーターには「○」、観測値として使用しないパラメーターには「×」が設定されている。

例えば、図５の観測値テーブルでは、ＧＰＳユニット１００と対応付けられた位置及び速度に「○」が設定されており、ＧＰＳユニット１００により計測された位置及び速度がＧＰＡ統合処理において観測値として使用されることを意味する。本実施形態では、ＧＰＳユニット１００の計測結果を観測値として使用し、ＩＭＵ５０及び方位センサーユニット１１０の計測結果は観測値として使用しない。

図３の説明に戻って、ＧＰＡ１７は、ステップＤ１で設定した観測値テーブルを用いて、補正タイミングを設定する（ステップＤ３）。具体的には、観測値テーブルにおいて、観測値に「○」が設定されているユニットを特定する。そして、特定したユニットからの計測結果を観測値として用いて補正処理を行うタイミングを、当該ユニットのデータ出力の時間間隔のタイミングに設定する。

例えば、図５の観測値テーブルでは、ＧＰＳユニット１００について、位置及び速度に「○」が設定されている。そのため、ＧＰＳユニット１００からの計測結果を観測値として用いて補正処理を行うことになる。その補正処理のタイミングを、ＧＰＳユニット１００のデータの出力時間間隔である「１秒」毎のタイミングに設定する。

次いで、スロット用制御部１５は、ＧＰＳユニット１００の初期化用の情報であるＧＰＳ初期化情報をＧＰＳユニット１００に出力する（ステップＦ３）。具体的には、「初期位置、速度、時刻」の情報をＧＰＳ初期化情報としてＧＰＳユニット１００に出力する。ＧＰＳユニット１００の組込時用処理部１０１は、スロット用制御部１５からＧＰＳ初期化情報を取得すると、当該情報を用いてＧＰＳユニット１００を初期化する処理を行う（ステップＨ１）。

また、スロット用制御部１５は、方位センサーユニット１１０の初期化用の情報である方位センサー初期化情報を方位センサーユニット１１０に出力する（ステップＦ５）。具体的には、「時刻」の情報を方位センサー初期化情報として方位センサーユニット１１０に出力する。方位センサーユニット１１０の組込時用処理部１１１は、スロット用制御部１５から方位センサー初期化情報を取得すると、当該情報を用いて方位センサーユニット１１０を初期化する処理を行う（ステップＪ１）。

また、ＩＭＵ用位置算出部１１は、ＩＭＵ５０の初期化用の情報であるＩＭＵ初期化情報をＩＭＵ５０に出力する（ステップＥ１）。具体的には、「時刻」の情報をＩＭＵ初期化情報としてＩＭＵ５０に出力する。そして、ＩＭＵ５０の処理部は、ＩＭＵ５０を初期化する処理を行う（ステップＧ１）。

その後、方位センサーユニット１１０の組込時用処理部１１１は、方位センサーの計測結果である方位と計測時刻とを、方位センサー計測結果としてスロット用Ｉ／Ｆ部１３を通じてスロット用制御部１５に出力する（ステップＪ３）。

スロット用制御部１５は、方位センサーユニット１１０から計測結果を取得すると、前回取得した方位と今回取得した方位とに基づいて角速度を算出する処理を行う（ステップＦ７）。例えば、単位時間当たりの方位の変化量を求めるといった微分計算により角速度を算出する。そして、スロット用制御部１５は、算出した角速度をＩＭＵ用位置算出部１１に出力する（ステップＦ８）。一方、ＩＭＵ５０は、ジャイロセンサー５３により計測された角速度をＩＭＵ用位置算出部１１に出力する（ステップＧ２）。

ＩＭＵ用位置算出部１１は、ＩＭＵ５０とスロット用制御部１５とからそれぞれ角速度を取得すると、取得した角速度を用いて、ＩＭＵ５０用の較正基準情報であるＩＭＵ較正基準情報を算出する（ステップＥ３）。具体的には、スロット用制御部１５から取得した角速度と、ＩＭＵ５０から取得した角速度の値とを用いて、公知の誤差算出演算を行って、ジャイロセンサー５３の誤差パラメーターの一種である角速度バイアス及び角速度スケールファクターを算出する。

尚、センサーのバイアス及びスケールファクターについては公知であるため詳細な説明を省略するが、バイアスとは、ある動作条件（例えば静止時）におけるセンサーの出力値を意味し、スケールファクターとは、センサーの感度、すなわち計測しようとする物理量の入力変化に対する出力変化の比を意味する。

その後、ＩＭＵ用位置算出部１１は、算出したＩＭＵ較正基準情報をＩＭＵ５０に出力する（ステップＥ５）。ＩＭＵ５０は、ＩＭＵ用位置算出部１１からＩＭＵ較正基準情報を取得すると、ジャイロセンサー５３の較正処理を行う（ステップＧ３）。

具体的には、ＩＭＵ５０は、装置内部で記憶している角速度バイアスを、ＩＭＵ用位置算出部１１から取得した角速度バイアスで更新する。また、記憶している角速度スケールファクターを、取得した角速度スケールファクターで更新する。これにより、ジャイロセンサー５３により計測される角速度は、この更新された角速度バイアス及び角速度スケールファクターを用いて較正されることになる。

ＩＭＵ５０は、その後、加速度センサー５１及びジャイロセンサー５３による計測結果である加速度、角速度、加速度誤差及び角速度誤差と、計測時刻とを、ＩＭＵ計測結果としてＩＭＵ用位置算出部１１に出力する（ステップＧ５）。ＩＭＵ用位置算出部１１は、ＩＭＵ計測結果を取得すると、当該ＩＭＵ計測結果を用いて公知の慣性航法演算処理を行って、位置及び速度と、これらに含まれ得る誤差とを算出する（ステップＥ７）。そして、算出した位置及び速度と、位置誤差及び速度誤差と、算出時刻とを、ＩＭＵ演算結果としてＧＰＡ１７及びスロット用制御部１５に出力する（ステップＥ９）。

スロット用制御部１５は、ＩＭＵ用位置算出部１１からＩＭＵ演算結果を取得すると、当該ＩＭＵ演算結果を用いてＧＰＳ較正基準情報を算出するＧＰＳ較正基準情報算出処理を行う（ステップＦ９）。具体的には、ＩＭＵ演算結果に含まれる位置及び速度と、ＧＰＳ衛星の衛星位置及び衛星速度とを用いて、位置算出システム１とＧＰＳ衛星との相対速度であるドップラー速度を算出する。

尚、衛星位置及び衛星速度は、予め取得した衛星軌道情報（アルマナックやエフェメリス）と現在時刻とを用いて算出することができる。そして、スロット用制御部１５は、算出したドップラー速度と、ＩＭＵ演算結果に含まれる位置、速度及び速度誤差とを、ＧＰＳ較正基準情報としてＧＰＳユニット１００に出力する（ステップＦ１１）。

ＧＰＳユニット１００の組込時用処理部１０１は、スロット用制御部１５からＧＰＳ較正基準情報を取得すると、当該ＧＰＳ較正基準情報を用いて衛星捕捉処理を行う（ステップＨ３）。具体的には、ＧＰＳ較正基準情報に含まれるドップラー速度を用いて、ＧＰＳ衛星信号のサーチ中心周波数を決定するとともに、周波数のサーチ範囲を決定する。

詳細に説明すると、ＧＰＳ衛星信号の搬送波周波数である１．５７５４２［ＧＨｚ］にドップラー速度分の周波数誤差を加味した周波数を、サーチ中心周波数に決定する。これにより、位置算出システム１の移動に伴うドップラー速度を加味したより正確なサーチ中心周波数を設定することが可能となり、ＧＰＳ衛星信号の捕捉に要する時間を短縮することができる。

さらに、ＧＰＳ較正基準情報に含まれる速度及び速度誤差を用いて、ＧＰＳ衛星信号の周波数のサーチ範囲を決定する。ＧＰＳ衛星信号を捕捉する場合は、ＧＰＳ衛星信号のサーチ周波数を変化させながら、レプリカコードとＧＰＳ衛星信号との相関演算を行って相関値のピーク値を算出する演算を行う。従って、ＧＰＳ較正基準情報に含まれる位置算出システム１の速度及び速度誤差を加味して周波数のサーチ幅を算出し、サーチ中心周波数を中心とし、算出したサーチ幅だけの幅を有する周波数範囲を、周波数のサーチ範囲に決定する。

次いで、ＧＰＳユニット１００の組込時用処理部１０１は、ＧＰＳ位置算出処理を行う（ステップＨ５）。具体的には、ＧＰＳ較正基準情報に含まれる位置を初期位置として、擬似距離を用いた公知の位置算出計算（位置収束演算）を行って位置を算出する。この際、衛星軌道情報を用いて算出したＧＰＳ衛星の衛星位置と、ＧＰＳ較正基準情報に含まれる位置算出システム１の位置間の距離を求め、求めた距離を光速で除算することで、擬似距離の整数部分を算出する。そして、ＧＰＳ衛星信号とレプリカコードとの相関演算を行うことで得られたコード位相から擬似距離の端数部分を求め、整数部分と端数部分とを加算して擬似距離を算出する。

ＩＭＵ用位置算出部１１が慣性航法演算処理を行うことで取得した位置を初期位置として位置算出計算を行うとともに、当該位置を用いて擬似距離の整数部分を算出するようにしたことで、位置収束演算における収束性が向上するとともに、算出される位置もより正確なものとなる。尚、詳細な説明は省略するが、ＧＰＳ位置算出処理では、ドップラー周波数の時間変化を利用した公知の速度算出計算を行って位置算出システム１の速度を算出する。また、公知の誤差算出計算を行って、算出した位置及び速度に含まれ得る誤差を算出する。

ＧＰＳ位置算出処理を行った後、ＧＰＳユニット１００の組込時用処理部１０１は、算出した位置、速度、位置誤差、速度誤差及び時刻を、ＧＰＳ演算結果としてスロット用制御部１５に出力する（ステップＨ７）。そして、スロット用制御部１５は、ＧＰＳユニット１００から取得したＧＰＳ演算結果をＧＰＡ１７に出力する（ステップＦ１３）。

ＧＰＡ１７は、スロット用制御部１５からＧＰＳ演算結果を取得すると、ＧＰＡ位置統合処理を行う（ステップＤ５）。ＧＰＡ位置統合処理は、カルマンフィルターを利用して、ＩＭＵ位置算出部１１により算出された位置及び速度と、ＧＰＳユニット１００により算出された位置及び速度とを統合する処理である。

尚、図３及び図４は、図２の処理に続く各機能部の処理フローである。従って、図３及び図４は、位置算出システム１にＧＰＳユニット１００が組み込まれた場合の処理の流れである。もし、位置算出システム１にＧＰＳユニット１００が組み込まれていない場合は、ＧＰＳユニット１００から位置及び速度の情報がＧＰＡ１７に出力されず、ＩＭＵ用位置算出部１１が慣性航法演算処理を行うことで算出した位置及び速度のみがＧＰＡ１７に出力されることになる。この場合は、ＧＰＡ１７は、位置統合処理を行わずに、ＩＭＵ用位置算出部１１の出力を用いて位置及び速度を算出する。例えば、ＩＭＵ用位置算出部１１から出力された位置及び速度をそのまま最終的な算出結果としてもよいし、ＩＭＵ用位置算出部１１から出力された位置及び速度を、過去の位置及び速度の算出結果の履歴に基づいて補正して、最終的な算出結果としてもよい。

図６は、ＧＰＡ位置統合処理の流れを示すフローチャートである。
先ず、ＧＰＡ１７は、位置の変化量及び速度を成分とする次式（１）で表される状態ベクトル「Ｘ₁」を設定する（ステップＫ１）。

式（１）において、「Δｐ_east」、「Δｐ_north」及び「Δｐ_up」は、それぞれ東西方向、南北方向及び天頂方向の位置の変化量を示している。また、「ｖ_east」、「ｖ_north」及び「ｖ_up」は、それぞれ東西方向、南北方向及び天頂方向の速度を示している。

次いで、ＧＰＡ１７は、式（２）で表される状態遷移行列「φ₁」を設定する（ステップＫ３）。状態遷移行列「φ」は、状態ベクトル「Ｘ」に含まれる各成分の遷移を表す行列である。

式（２）において、「Δｔ」は、ＩＭＵ５０からのデータの出力時間間隔である。

そして、ＧＰＡ１７は、カルマンフィルターを利用した予測演算を行う処理であるＧＰＡ予測処理を行う（ステップＫ５）。

図７は、ＧＰＡ予測処理の流れを示すフローチャートである。
先ず、ＧＰＡ１７は、ＩＭＵ用位置算出部１１から今回取得したＩＭＵ演算結果に含まれる速度と、ＩＭＵ用位置算出部１１から前回取得したＩＭＵ演算結果に含まれる速度との差を、速度の変化量「Δｖ₁」として算出する（ステップＬ１）。

そして、ＧＰＡ１７は、現在の状態ベクトル「Ｘ₁」と、ステップＫ３で設定した状態遷移行列「φ₁」と、ステップＬ１で算出した速度の変化量「Δｖ₁」とを用いて、次式（３）に従って状態ベクトルの予測値「Ｘ⁻ ₁」を算出する（ステップＬ３）。

式（３）において、上付きの「−」は予測値であることを示している。下付きの「ｔ」は時刻であり、ＩＭＵ５０からのデータの出力時間間隔毎の時刻を示している。また、「Ｘ^IMU」は、ＩＭＵの演算結果を用いて設定される状態ベクトルのバイアスを示している。「Ｘ^IMU」に含まれる「Δｖ^IMU _east」、「Δｖ^IMU _north」及び「Δｖ^IMU _up」は、それぞれ東西方向、南北方向、天頂方向の速度の変化量（Δｖ₁の各成分）を示している。また、「Ｘ^IMU」では、東西方向、南北方向、天頂方向の位置成分を全て「０」としている。すなわち、位置変化がないものと仮定して予測演算を行うことにしている。

また、ＧＰＡ１７は、カルマンフィルターの理論に基づいて誤差共分散行列「Ｐ₁」を予測する演算を行う（ステップＬ５）。そして、ＧＰＡ１７は、ＧＰＡ予測処理を終了する。

図６の説明に戻って、ＧＰＡ予測処理を行った後、ＧＰＡ１７は、補正タイミングであるか否かを判定する（ステップＫ７）。すなわち、ステップＤ３で設定した補正タイミングが到来したか否かを判定する。そして、まだ補正タイミングではないと判定した場合は（ステップＫ７；Ｎｏ）、ステップＫ５に戻り、再びＧＰＡ予測処理を行う。

本実施形態では、ＧＰＳユニット１００のデータの出力時間間隔である「１秒」毎のタイミングを補正タイミングとしている。一方、ＩＭＵ用位置算出部１１からは、「０．０１秒」の時間間隔でＩＭＵ演算結果が出力される。従って、ＩＭＵ用位置算出部１１から０．０１秒の時間間隔で出力されるＩＭＵ演算結果を用いて、１秒間に１００回の予測演算を行うことが可能である。そのため、本実施形態では、１回の補正タイミングが到来するまでの間に、ＧＰＡ予測処理を１００回繰り返し実行する。

一方、ステップＫ７において補正タイミングであると判定した場合は（ステップＫ７；Ｙｅｓ）、ＧＰＡ１７は、ＧＰＡ補正処理を行う（ステップＫ９）。

図８は、ＧＰＡ補正処理の流れを示すフローチャートである。
先ず、ＧＰＡ１７は、ステップＤ１で設定した観測値テーブルを参照し、当該補正タイミングにおいて観測値とするパラメーターの値が設定された観測ベクトル「Ｚ₁」を生成する（ステップＭ１）。本実施形態では、ＧＰＳユニット１００により算出された位置及び速度を用いて、次式（４）に示すような観測ベクトル「Ｚ₁」を生成する。

但し、「Δｐ^GPS _east」、「Δｐ^GPS _north」及び「Δｐ^GPS _up」は、ＧＰＳユニット１００により算出された東西方向、南北方向及び天頂方向の位置の変化量をそれぞれ示している。また、「ｖ^GPS _east」、「ｖ^GPS _north」及び「ｖ^GPS _up」は、ＧＰＳユニット１００により算出された東西方向、南北方向及び天頂方向の速度をそれぞれ示している。

次いで、ＧＰＡ１７は、次式（５）に示すような観測行列「Ｈ₁」を設定する（ステップＭ３）。

観測行列「Ｈ」は、状態ベクトルの予測値「Ｘ⁻」に作用させることで、観測ベクトル「Ｚ」に含まれる各成分、すなわち各観測値の予測値を算出するための行列である。本実施形態では、式（５）に示すように、対角成分が全て「１」であり、その他の成分が「０」であるような観測行列「Ｈ₁」を用いて計算を行う。

次いで、ＧＰＡ１７は、次式（６）に従ってカルマンゲイン「Ｋ₁」を算出する（ステップＭ５）。

但し、「Ｒ₁」は、観測ベクトル「Ｚ₁」の各成分、すなわち各観測値に含まれ得る誤差の大きさを示す測定誤差行列である。

その後、ＧＰＡ１７は、ＧＰＡ予測処理で算出された状態ベクトルの予測値「Ｘ⁻ ₁」に、ステップＭ３で設定した観測行列「Ｈ₁」を作用させることで、予測された観測ベクトルである予測観測ベクトル「Ｈ₁Ｘ⁻ ₁」を算出する（ステップＭ７）。

そして、ＧＰＡ１７は、次式（７）に従って状態ベクトルの予測値「Ｘ⁻ ₁」を補正するとともに（ステップＭ９）、次式（８）に従って誤差共分散行列の予測値「Ｐ⁻ ₁」を補正する（ステップＭ１１）。そして、ＧＰＡ１７は、ＧＰＡ補正処理を終了する。

但し、上付きの「＋」は補正値であることを示している。また、「Ｉ」は単位行列である。

図６の説明に戻って、ＧＰＡ補正処理を行った後、ＧＰＡ１７は、状態ベクトル「Ｘ₁」の補正値「Ｘ^＋ ₁」に含まれる位置の変化量「Δｐ」をＩＭＵ用位置算出部１１から取得したＩＭＵ演算結果に含まれる位置に加算して、位置の算出結果とする（ステップＫ１１）。また、ＧＰＡ１７は、状態ベクトル「Ｘ₁」の補正値「Ｘ^＋ ₁」に含まれる速度「ｖ」を、速度の算出結果とする（ステップＫ１３）。そして、ＧＰＡ１７は、ＧＰＡ位置統合処理を終了する。

図４の位置算出処理に戻って、ＧＰＡ位置統合処理を行った後、ＧＰＡ１７は、ＧＰＡ位置統合処理の演算結果を、ＧＰＡ演算結果としてＩＭＵ用位置算出部１１及び方位センサーユニット１１０に出力する（ステップＤ７）。

ＩＭＵ用位置算出部１１は、ＧＰＡ１７からＧＰＡ演算結果を取得すると、慣性航法演算結果較正処理を行う（ステップＥ１１）。慣性航法演算結果較正処理は、ＧＰＡ１７が位置統合処理を行うことで算出した位置及び速度を用いて、ステップＥ７の慣性航法演算処理を行うことで算出した位置及び速度を較正する処理である。

図９は、慣性航法演算結果較正処理の流れを示すフローチャートである。
先ず、ＩＭＵ用位置算出部１１は、慣性航法演算処理で得られた位置及び速度を成分とする状態ベクトル「Ｘ₂」を設定する（ステップＮ１）。すなわち、次式（９）に示すような状態ベクトル「Ｘ₂」を設定する。

その後、ＩＭＵ用位置算出部１１は、ＩＭＵ予測処理を行う（ステップＮ３）。

図１０は、ＩＭＵ予測処理の流れを示すフローチャートである。
先ず、ＩＭＵ用位置算出部１１は、ＩＭＵ５０から取得した角速度を時間積分して姿勢角を算出する（ステップＰ１）。

次いで、ＩＭＵ用位置算出部１１は、算出した姿勢角を用いて、ＩＭＵ５０から取得したローカル座標系における加速度を、ＥＮＵ座標系（東北上座標系）における加速度に変換する（ステップＰ３）。この加速度の変換は、方位センサーユニット１１０により検出された方位を用いて、公知の座標変換演算によって実現することができる。

その後、ＩＭＵ用位置算出部１１は、ステップＰ３において変換された加速度に対して時間積分を行って速度を算出する（ステップＰ５）。そして、ＩＭＵ用位置算出部１１は、算出した速度を時間積分して位置を算出する（ステップＰ７）。

その後、ＩＭＵ用位置算出部１１は、ステップＰ５で算出された位置及び速度を状態ベクトルの予測値「Ｘ⁻ ₂」とする（ステップＰ９）。また、ＩＭＵ用位置算出部１１は、カルマンフィルターの理論に基づく予測演算を行って誤差共分散行列の予測値「Ｐ⁻ ₂」を算出する（ステップＰ１１）。そして、ＩＭＵ用位置算出部１１は、ＩＭＵ予測処理を終了する。

図９の説明に戻って、ＩＭＵ予測処理を行った後、ＩＭＵ用位置算出部１１は、ＩＭＵ補正処理を行う（ステップＮ５）。

図１１は、ＩＭＵ補正処理の流れを示すフローチャートである。
先ず、ＩＭＵ用位置算出部１１は、ＧＰＡ１７から取得したＧＰＡ演算結果に含まれる位置及び速度と、ＩＭＵ予測処理のステップＰ９で予測された状態ベクトルに含まれる位置及び速度の予測値「Ｘ⁻ ₂」との差分をそれぞれ算出して、観測ベクトル「Ｚ₂」を生成する（ステップＱ１）。すなわち、次式（１０）に示すような観測ベクトル「Ｚ₂」を生成する。

但し、上付きの添え字の「ＩＭＵ」はＩＭＵ予測処理で予測した値、「ＧＰＡ」はＧＰＡ演算結果に含まれる値であることをそれぞれ示している。

次いで、ＩＭＵ用位置算出部１１は、次式（１１）に示すような観測行列「Ｈ₂」を設定する（ステップＱ３）。

そして、ＩＭＵ用位置算出部１１は、次式（１２）に従ってカルマンゲイン「Ｋ₂」を算出する（ステップＱ５）。

次いで、ＩＭＵ用位置算出部１１は、次式（１３）に示すように、ステップＱ５で設定したカルマンゲイン「Ｋ₂」を、ステップＱ１で生成した観測ベクトル「Ｚ₂」に作用させることで、状態ベクトルの修正量「ΔＸ₂」を算出する（ステップＱ７）。

そして、ＩＭＵ用位置算出部１１は、ステップＱ７で算出した状態ベクトルの修正量「ΔＸ₂」を用いて、次式（１４）に従って状態ベクトル「Ｘ₂」を補正する（ステップＱ９）。そして、ＩＭＵ用位置算出部１１は、ＩＭＵ補正処理を終了する。

図９の説明に戻って、ＩＭＵ補正処理を行った後、ＩＭＵ用位置算出部１１は、状態ベクトル「Ｘ₂」の補正値「Ｘ^＋ ₂」に含まれる位置「ｐ」及び速度「ｖ」を、位置及び速度の較正結果とする（ステップＮ７）。そして、ＩＭＵ用位置算出部１１は、慣性航法演算結果較正処理を終了する。

図４の位置算出処理に戻って、慣性航法演算結果較正処理を行った後、ＩＭＵ用位置算出部１１は、ＩＭＵ較正基準情報算出処理を行って、ＩＭＵ５０を較正するための情報を算出する処理を行う（ステップＥ１３）。具体的には、ＧＰＡ１７から前回取得した速度と今回取得した速度とに基づいて加速度を算出する処理を行う。例えば、単位時間当たりの速度の変化量を求めるといった微分計算により加速度を算出する。そして、算出した加速度と、ＩＭＵ５０から取得した加速度とを用いて、加速度センサー５１の誤差パラメーターの一種である加速度バイアス及び加速度スケールファクターを算出して、ＩＭＵ較正基準情報とする。

次いで、ＩＭＵ用位置算出部１１は、ステップＥ１３で算出したＩＭＵ較正基準情報をＩＭＵ５０に出力する（ステップＥ１５）。ＩＭＵ５０は、ＩＭＵ用位置算出部１１からＩＭＵ較正基準情報を取得すると、加速度センサー５１の較正処理を行う（ステップＧ７）。

具体的には、ＩＭＵ５０は、内部で記憶している加速度バイアスを、取得したＩＭＵ較正基準情報に含まれる加速度バイアスで更新する。また、内部で記憶している加速度スケールファクターを、取得したＩＭＵ較正基準情報に含まれる加速度スケールファクターで更新する。これにより、加速度センサー５１により計測される加速度は、この更新された加速度バイアス及び加速度スケールファクターにより較正されることになる。

また、スロット用制御部１５は、ＧＰＡ１７からＧＰＡ演算結果を取得すると、当該ＧＰＡ演算結果に含まれる位置を、方位センサー較正基準情報として方位センサーユニット１１０に出力する（ステップＦ１５）。方位センサーユニット１１０の組込時用処理部１１１は、スロット用制御部１５から方位センサー較正基準情報を取得すると、これを用いて方位センサー較正処理を行う（ステップＪ５）。

方位センサーの較正は、例えば特開２００１−９１２５７号公報に開示されている技術を適用することができる。すなわち、予め記憶された地磁気モデルの偏差分布式に、方位センサー較正基準情報に含まれる位置を代入して、当該位置における磁気偏差を算出する。そして、方位センサーにより検出される磁方位と算出した磁気偏差とを用いて真北方位を算出し、方位センサーにより検出される磁方位を、算出した真北方位に基づいて修正する。

これらの一連の処理を行った後、ＧＰＡ１７は、ステップＤ５に戻り、ＩＭＵ用位置算出部１１は、ステップＥ３に戻り、スロット用制御部１５は、ステップＦ７に戻る。また、ＩＭＵ５０は、ステップＧ３に戻り、ＧＰＳユニット１００はステップＨ３に戻り、方位センサーユニット１１０は、ステップＪ３に戻る。

３．作用効果
位置算出装置の一種である位置算出システム１には、加速度センサー５１及びジャイロセンサー５３を具備したＩＭＵ５０が備えられている。また、位置算出システム１には、ユニット組み込み部である第１スロット６０及び第２スロット７０が備えられており、位置算出に利用可能な物理的事象を検出するセンサーユニットの一種として、ＧＰＳユニット１００又は方位センサーユニット１１０が組み込み可能に構成されている。

２つのスロットの何れかにセンサーユニットが組み込まれたことを検出すると、スロット用制御部１５は、認識処理を行って組込ユニットを判定する。具体的には、組み込まれたセンサーユニットに対してユニット情報の問い合わせを行い、組込ユニットから、識別コードと、出力仕様情報と、較正基準種別情報と、タイミング情報とを取得する。スロット用制御部１５は、取得した識別コードに応じて、組み込まれたセンサーユニットの種類を識別する。

ＧＰＳユニット１００及び方位センサーユニット１１０の両方が組み込まれたことを検出した場合は、ＩＭＵ用位置算出部１１は、方位センサーユニット１１０により検出された方位の時間変化に基づいて角速度を算出する。そして、ＩＭＵ５０のジャイロセンサー５３により検出される角速度と比較し、ジャイロセンサー５３の較正に使用する誤差パラメーターとして、角速度バイアス及び角速度スケールファクターを算出して、ＩＭＵ５０に出力する。ジャイロセンサー５３は、ＩＭＵ用位置算出部１１から取得した角速度バイアス及び角速度スケールファクターを用いて計測結果を較正する。

ＩＭＵ用位置算出部１１は、ＩＭＵ５０から取得した加速度及び角速度の計測結果を用いて、公知の慣性航法演算処理を行って位置、速度及びこれらに含まれ得る誤差を演算する。そして、演算結果をスロット用制御部１５に出力するとともに、ＧＰＡ１７に出力する。スロット用制御部１５は、ＩＭＵ演算結果を用いて、ＧＰＳユニット１００のキャリブレーション用の較正基準情報であるＧＰＳ較正基準情報を算出し、ＧＰＳユニット１００に出力する。そして、ＧＰＳユニット１００は、当該ＧＰＳ較正基準情報を用いて、衛星捕捉処理及び位置算出処理を行う。

ＧＰＳユニット１００は、位置算出処理で求めた位置及び速度の演算結果を、スロット用制御部１５を通じてＧＰＡ１７に出力する。そして、ＧＰＡ１７は、ＩＭＵ用位置算出部１１の演算結果及びＧＰＳユニット１００の演算結果を用いて、フィルター処理の一種であるカルマンフィルターを利用した位置統合処理を行って位置を算出・統合する。この際、ＧＰＳユニット１００からデータが出力されるまでの間、ＩＭＵ用位置算出部１１により算出された速度の変化量を用いて、ＩＭＵ用位置算出部１１により算出された位置及び速度を予測する予測処理を繰り返し実行する。そして、ＧＰＳユニット１００からのデータの出力タイミングにおいて、ＧＰＳユニット１００から取得した位置及び速度を観測値として、予測位置及び予測速度を補正する補正処理を行う。

一方、位置算出システム１にＧＰＳユニット１００が組み込まれていない場合は、ＧＰＳユニット１００から位置及び速度の情報がＧＰＡ１７に出力されず、ＩＭＵ用位置算出部１１が慣性航法演算処理を行うことで算出した位置及び速度のみがＧＰＡ１７に出力される。従って、この場合は、ＧＰＡ１７は、位置統合処理を行わずに、ＩＭＵ用位置算出部１１の出力を用いて位置及び速度を算出する。

このように、スロットにＧＰＳユニット１００が組み込まれていない場合は、ＩＭＵ５０の計測結果を利用して慣性航法演算処理を行って位置を算出する。他方、ＧＰＳユニット１００が組み込まれている場合は、認識処理で認識されたＧＰＳユニット１００の識別コードに応じて、ＧＰＳユニット１００の計測結果とＩＭＵ５０の計測結果とを用いて位置を算出する。これにより、ユニットの組み込み有無に応じた適切な演算処理を行って位置算出を行うことが可能となり、システム（装置）の拡張や変更等に対して柔軟に対応することができる。

また、ＧＰＡ１７が、カルマンフィルターを利用した位置統合処理を行うようにし、ＩＭＵ５０の計測結果に基づいて予測した位置を、ＧＰＳユニット１００の計測結果を用いて補正するようにした。このように、予め搭載されたユニットの計測結果を用いて予測した位置を、新たに組み込まれたユニットの計測結果を用いてカルマンフィルターにより補正することで、位置算出の正確性を向上させ、より真位置に近い位置を求めることが可能となる。

さらに、方位センサーユニット１１０の計測結果を利用して算出した角速度バイアス及び角速度スケールファクターを用いてＩＭＵ５０のジャイロセンサー５３の較正を行うとともに、ＧＰＡ１７の演算結果を利用して算出した加速度バイアス及び加速度スケールファクターを用いてＩＭＵ５０の加速度センサー５１の較正を行うようにした。これにより、組み込まれたセンサーユニットの計測結果や、位置統合処理を行うことで得られた信頼性の高い演算結果を利用してＩＭＵ５０の較正を行うことが可能となる。

４．変形例
４−１．組込ユニットの組合せ
上述した実施形態では、第１スロット６０にＧＰＳユニット１００が、第２スロット７０に方位センサーユニット１１０がそれぞれ組み込まれた場合を例に挙げて説明した。しかし、ＧＰＳユニット１００のみが組み込まれた場合や、方位センサーユニット１１０のみが組み込まれた場合についても同様に処理を行うことができる。以下、それぞれの場合について具体的に説明する。

（１）ＧＰＳユニットのみが組み込まれた場合
（Ａ）認識処理
図２の認識処理において、スロット用制御部１５は、第１スロット６０又は第２スロット７０にユニットが組み込まれたことを検出すると、当該スロットにユニット情報の問い合わせを行う。そして、ＧＰＳユニット１００からのユニット情報の通知を受けて、組込ユニットがＧＰＳユニット１００であることを認識する。

（Ｂ）位置算出処理
図３及び図４の位置算出処理において、ＧＰＳユニット１００のみが組み込まれた場合は、スロット用制御部１５は、ステップＦ５〜Ｆ８の処理を行わない。すなわち、方位センサーユニット１１０が組み込まれておらず、方位情報を取得することができないため、方位センサー初期化情報出力、角速度算出処理及び角速度出力の各ステップは実行しない。これに伴い、ＩＭＵ用位置算出部１１は、ステップＥ３及びＥ５の処理を行わず、ＩＭＵ５０も、ステップＧ３の処理を行わない。また、この場合は、方位センサーの較正に係る処理（ステップＦ１５、ステップＪ５）も実行しない。

（２）方位センサーユニットのみが組み込まれた場合
（Ａ）認識処理
図２の認識処理において、スロット用制御部１５は、第１スロット６０又は第２スロット７０にユニットが組み込まれたことを検出すると、当該スロットにユニット情報の問い合わせを行う。そして、方位センサーユニット１１０からのユニット情報の通知を受けて、組込ユニットが方位センサーユニット１１０であることを認識する。

（Ｂ）位置算出処理
図３及び図４の位置算出処理において、方位センサーユニット１１０のみが組み込まれた場合は、スロット用制御部１５は、ステップＦ３、Ｆ９、Ｆ１１の処理を行わない。すなわち、ＧＰＳユニット１００が組み込まれていないため、ＧＰＳユニット１００の初期化情報出力、ＧＰＳユニット１００の較正基準情報の算出及び出力は行わない。また、この場合は、ＧＰＳユニット１００から位置及び速度の演算結果を取得することができない。そのため、ＧＰＡ１７は、ＩＭＵ位置算出部１１から取得したＩＭＵ演算結果を用いて位置及び速度を算出する。

４−２．組込ユニットの種類
また、各スロットに組み込み可能なユニットは、これらのユニットに限られるわけではない。例えば、位置算出に利用可能な物理的事象を検出可能なセンサーユニットとして、速度センサーユニットや加速度センサーユニット、ジャイロセンサーユニット等のセンサーユニットを組み込むことも可能である。また、ＲＦＩＤ（Radio Frequency IDentification）を用いて位置を検出するユニット（位置検出ユニット）を組み込むことも可能である。

さらには、第１スロット６０及び第２スロット７０それぞれに同種のユニット（例えばＧＰＳユニット）を組み込むことも可能である。例えば、第１及び第２ＧＰＳユニットの２つのＧＰＳユニットを組み込むことが考えられる。この場合は、スロット用制御部１５は、第１及び第２ＧＰＳユニットそれぞれについて、当該ＧＰＳユニットが衛星捕捉処理及び位置算出処理を行うために必要な情報を算出し、ＧＰＳ較正基準情報として当該ＧＰＳユニットに出力する。そして、第１及び第２ＧＰＳユニットは、それぞれスロット用制御部１５から取得したＧＰＳ較正基準情報を利用して、衛星捕捉処理及び位置算出処理を行う。

また、ＧＰＡ１７は、第１及び第２ＧＰＳユニットそれぞれから演算結果を取得する。そして、ＩＭＵ用位置算出部１１から取得したＩＭＵ演算結果と、第１ＧＰＳユニットから取得した第１ＧＰＳ演算結果と、第２ＧＰＳユニットから取得した第２ＧＰＳ演算結果とを用いて、カルマンフィルターを利用した位置統合処理を行って位置を統合する。この場合の具体的処理については後述する。

また、上述した実施形態では、慣性計測ユニット（ＩＭＵ）を位置算出システム１に搭載しておくものとして説明したが、ＩＭＵ５０の計測結果を利用して慣性航法演算を行って位置及び速度を算出する慣性航法装置（ＩＮＳ（Inertial Navigation System））を搭載しておくことにしてもよい。

図１２は、この場合における位置算出システム２の構成の一例を示すブロック図である。尚、図１２において、先に説明した実施形態と同様の構成部分については、先に説明した図１と同様の符号を付して、その詳細な説明を省略する。位置算出システム２では、ＩＭＵ５０の代わりに、ＩＭＵ５０を有するＩＮＳ２００が具備されている。また、処理部１０には、ＩＮＳ２００へのデータの入出力を制御する機能部として、ＩＭＵ用位置算出部１１の代わりにＩＮＳ用制御部１９が具備されている。位置算出システム２では、ＩＮＳ２００が慣性航法演算処理を行って位置及び速度を算出して、ＩＮＳ用制御部１９に出力する。

ＩＮＳ用制御部１９は、ＩＮＳ２００から取得した計測結果をＧＰＡ１７に出力する。そして、ＧＰＡ１７は、スロット用制御部１５を介して取得したＧＰＳユニット１００の計測結果と、ＩＮＳ用制御部１９を介してＩＮＳ２００から取得した計測結果とを用いて、図６で説明したＧＰＡ位置統合処理を行って位置及び速度を算出する。

また、ＧＰＡ１７は、位置統合処理を行うことで算出した位置及び速度をＩＮＳ用制御部１９に出力する。ＩＮＳ用制御部１９は、ＧＰＡ１７から位置及び速度の演算結果を取得すると、当該演算結果をＩＮＳ２００に出力する。そして、ＩＮＳ２００は、図９で説明した慣性航法演算結果較正処理を行って、慣性航法演算で求めた位置及び速度を較正する。

４−３．ＧＰＡ位置統合処理
上述した実施形態では、ＧＰＡ１７が実行するＧＰＡ位置統合処理において、ＩＭＵ５０の計測結果を利用して位置及び速度を予測し、ＧＰＳユニット１００の計測結果を観測値として利用して予測値を補正するものとして説明した。この関係を逆転させて、例えば、ＧＰＳユニット１００の計測結果を利用して位置及び速度を予測し、ＩＭＵ５０の計測結果を観測値として利用して予測値を補正することとしてもよい。

また、ＧＰＳユニット１００の計測結果を観測値として利用するのみならず、他のセンサーユニットの計測結果を観測値として利用してＧＰＡ補正処理を行うことにしてもよい。例えば、位置算出システムにＧＰＳユニットと速度センサーユニットとが組み込まれた場合は、ＧＰＳユニット１００の計測結果を観測値として予測値を補正する第１ＧＰＡ補正処理を行うとともに、速度センサーユニットの計測結果を観測値として予測値を補正する第２ＧＰＡ補正処理を行う。

より具体的には、ＧＰＡ１７は、図３の位置算出処理のステップＤ１において、例えば図１３に示すような観測値テーブルを生成する。すなわち、ＧＰＳユニット１００については位置及び速度に「○」が設定され、速度センサーユニットについては速度に「○」が設定された観測値テーブルを生成する。また、ＧＰＡ１７は、位置算出処理のステップＤ３において、第１ＧＰＡ補正処理の実行タイミング（以下、「第１補正タイミング」と称す。）として、ＧＰＳユニット１００のデータの出力時間間隔毎のタイミングを設定し、第２ＧＰＡ補正処理の実行タイミング（以下、「第２補正タイミング」と称す。）として、速度センサーユニットのデータの出力時間間隔毎のタイミングを設定する。

そして、図６のＧＰＡ位置統合処理のステップＫ７において、ステップＤ３で設定した第１補正タイミング又は第２補正タイミングが到来したか否かを判定し、第１補正タイミングが到来したと判定した場合は第１ＧＰＡ補正処理を、第２補正タイミングが到来したと判定した場合は第２ＧＰＡ補正処理をそれぞれ実行する。第１ＧＰＡ補正処理では、図１３の観測値テーブルに従って、ＧＰＳユニット１００から取得した位置及び速度を成分とする観測ベクトルを用いて補正演算を行う。また、第２ＧＰＡ補正処理では、速度センサーユニットから取得した速度を観測値として用いて補正演算を行う。

また、同種のセンサーユニットからの計測結果をそれぞれ観測値として利用して、ＧＰＡ補正処理を行うことも可能である。例えば、各スロットに第１ＧＰＳユニットと第２ＧＰＳユニットとが組み込まれた場合は、第１ＧＰＳユニットの計測結果を観測値として予測位置及び予測速度を補正する第１ＧＰＡ補正処理を行うとともに、第２ＧＰＳユニットの計測結果を観測値として予測位置及び予測速度を補正する第２ＧＰＡ補正処理を行う。

例えば、第１ＧＰＳユニットが、位置及び速度を出力可能に構成されたユニットであり、第２ＧＰＳユニットが、位置のみを出力可能に構成されたユニットであるとする。この場合、ＧＰＡ１７は、図３の位置算出処理のステップＤ１において、図１４に示すような観測値テーブルを生成する。すなわち、第１ＧＰＳユニットについては位置及び速度に「○」が設定され、第２ＧＰＳユニットについては位置に「○」が設定された観測値テーブルを生成する。また、ＧＰＡ１７は、位置算出処理のステップＤ３において、第１補正タイミングとして第１ＧＰＳユニットのデータの出力時間間隔のタイミングを設定し、第２補正タイミングとして第２ＧＰＳユニットのデータの出力時間間隔のタイミングを設定する。

そして、図６のＧＰＡ位置統合処理のステップＫ７において、ステップＤ３で設定した
第１補正タイミング又は第２補正タイミングが到来したか否かを判定し、第１補正タイミングが到来したと判定した場合は第１ＧＰＡ補正処理を、第２補正タイミングが到来したと判定した場合は第２ＧＰＡ補正処理をそれぞれ実行する。第１ＧＰＡ補正処理では、図１４の観測値テーブルに従って、第１ＧＰＳユニットから取得した位置及び速度を成分とする観測ベクトルを用いて補正演算を行う。また、第２ＧＰＡ補正処理では、第２ＧＰＳユニットから取得した位置を観測値として用いて補正演算を行う。

４−４．ＩＭＵの較正処理
上述した実施形態では、ＩＭＵ５０内部で記憶しているバイアス及びスケールファクターを、ＩＭＵ用位置算出部１１により算出されたバイアス及びスケールファクターに更新することで、ＩＭＵ５０の較正を行うものとして説明したが、この処理を次のようにしてもよい。

すなわち、ＩＭＵ位置算出部１１から取得したバイアス及びスケールファクターの履歴を蓄積記憶しておき、公知の線形補間演算を行って、現在のバイアス及びスケールファクターの値を推定する。そして、内部で記憶しているバイアス及びスケールファクターの値を、推定したバイアス及びスケールファクターの値に変更することで、ＩＭＵ５０の較正を行う。

４−５．フィルター処理
上述した実施形態では、カルマンフィルター処理によって位置を算出する例を説明したが、使用するフィルターとして、Ｐａｒｔｉｃｌｅフィルター（粒子フィルター）、Ｓｉｇｍａポイントフィルター（シグマポイントフィルター）、最小二乗法によるフィルター等を採用し、これらのフィルターを用いたフィルター処理を行って位置を算出することにしてもよい。

４−６．衛星位置算出ユニット
また、上述した実施形態では、衛星位置算出ユニットとしてＧＰＳユニットを例に挙げて説明したが、ＷＡＡＳ（Wide Area Augmentation System）、ＱＺＳＳ（Quasi Zenith Satellite System）、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の他の衛星位置算出システムを利用した衛星位置算出ユニットであってもよい。

１ 位置算出システム、 １０ 処理部、 １１ ＩＭＵ用位置算出部、
１３ スロット用Ｉ／Ｆ部、 １５ スロット用制御部、
１７ 位置情報統合処理部（ＧＰＡ）、 ２０ 入力部、 ３０ 表示部、
４０ 通信部、 ５０ ＩＭＵ、 ６０ 第１スロット、 ７０ 第２スロット、
８０ 第１バス、 ９０ 第２バス、 １００ ＧＰＳユニット、
１１０ 方位センサーユニット

Claims (6)

1. 慣性計測ユニットを備え、更に、位置算出に利用可能な物理的事象を検出する少なくとも１つのセンサーユニットを組み込み可能に構成された位置算出装置が行う位置算出方法であって、
   前記センサーユニットが組み込まれている場合に、前記センサーユニットの種類を認識することと、
   前記センサーユニットが組み込まれていない場合に、前記慣性計測ユニットの出力を用いて位置を算出することと、
   前記センサーユニットが組み込まれている場合に、前記認識された前記センサーユニットの種類に応じて、前記センサーユニットの出力と前記慣性計測ユニットの出力とを用いて位置を算出することと、
   を含む位置算出方法。
2. 前記センサーユニットの較正を行うときに用いる較正基準情報の種別を表す較正基準種別情報を取得することを更に含み、
   前記センサーユニットが組み込まれている場合に、前記較正基準種別情報と前記慣性計測ユニットの出力とに基づいて、前記較正基準情報を算出することと、
   前記較正基準情報を用いて前記センサーユニットの較正を行うことと、
   を含む請求項１に記載の位置算出方法。
3. 前記センサーユニットが組み込まれた場合に、前記センサーユニットの出力を用いたフィルター処理によって位置を算出すること、
   を更に含む請求項１又は２に記載の位置算出方法。
4. 前記センサーユニットは、位置算出用衛星からの衛星信号を用いて位置算出が可能な衛星位置算出ユニットであり、
   前記フィルター処理によって位置を算出することは、
   前記衛星位置算出ユニットが算出する位置以外の値を用いて位置を予測することと、
   前記予測した位置を、前記衛星位置算出ユニットが算出する位置に基づいて補正することと、
   を含む、
   請求項３に記載の位置算出方法。
5. 前記衛星位置算出ユニットが位置を算出する時間間隔を表す情報を取得することを更に含み、
   前記予測することは、前記時間間隔の間に繰り返し位置を予測することを含み、
   前記補正することは、前記予測した位置を前記時間間隔で補正することを含む、
   請求項４に記載の位置算出方法。
6. 慣性計測ユニットと、
   位置算出に利用可能な物理的事象を検出するセンサーユニットを組み込み可能な組み込み部と、
   前記センサーユニットが組み込まれた場合に、前記センサーユニットの種類を認識する認識部と、
   前記センサーユニットが組み込まれていない場合には、前記慣性計測ユニットの出力を用いて位置を算出し、前記センサーユニットが組み込まれた場合には、前記認識された前記センサーユニットの種類に応じて、前記センサーユニットの出力と、前記慣性計測ユニットの出力とを用いて位置を算出する位置算出部と、
   を備えた位置算出装置。

Images