慣性航法装置およびその誤差補正方法
【課題】ＡＨＲＳとＩＮＳで構成を共通化し、温度計を使わずジャイロの温度ドリフト誤差を補正し、基準となる姿勢・方位データを使わずジャイロセンサ誤差と加速度計誤差を補正する慣性航法装置。
【解決手段】推定センサ誤差データからジャイロと加速度計の出力を補正するセンサ誤差補正計算部と、補正後の移動体の位置や姿勢を計算し、推定姿勢／方位誤差データより移動体の位置や姿勢を補正する航法・姿勢方位計算部と、補正後の位置や姿勢の情報と移動体上のＧＰＳセンサの絶対位置と絶対速度と比較して、ジャイロにおける温度ドリフトの影響を分離する温度誤差推定部と、分離後の温度ドリフト等の影響よりジャイロと加速度計の出力の誤差を推定しセンサ誤差補正計算部で使用するセンサ誤差データを校正し、補正後の位置や姿勢の出力の誤差を推定し航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正するセンサ固有誤差推定部とを有する。
【解決手段】推定センサ誤差データからジャイロと加速度計の出力を補正するセンサ誤差補正計算部と、補正後の移動体の位置や姿勢を計算し、推定姿勢／方位誤差データより移動体の位置や姿勢を補正する航法・姿勢方位計算部と、補正後の位置や姿勢の情報と移動体上のＧＰＳセンサの絶対位置と絶対速度と比較して、ジャイロにおける温度ドリフトの影響を分離する温度誤差推定部と、分離後の温度ドリフト等の影響よりジャイロと加速度計の出力の誤差を推定しセンサ誤差補正計算部で使用するセンサ誤差データを校正し、補正後の位置や姿勢の出力の誤差を推定し航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正するセンサ固有誤差推定部とを有する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の現在位置をナビゲーションする装置において、定量的に求めることができないセンサ誤差を補償して精度の高い現在位置を提供することができる慣性航法装置およびその誤差補正方法に関するものである。
【背景技術】
【０００２】
図３は従来の慣性航法装置およびその誤差補正方法の一例を示すブロック図であり、航空機の搭乗者が頭部に装着したヘッドトラッカの構成を示したものである。ヘッドトラッカとは、一般にヘルメット装着者頭部の姿勢や方位角を提供するＡＨＲＳ（Attitude Heading Reference System：姿勢方位基準装置）またはＩＮＳ（Inertial Navigation System：慣性航法装置）である。
【０００３】
図３（ａ）はＡＨＲＳの場合、（ｂ）はＩＮＳの場合を示している。図３（ａ）において、１１と２１はセンサ誤差補正計算部、１２と２２は航法・姿勢方位計算部、１３と２３は誤差推定計算部、１４は基準姿勢計算部、１５と２５はＭＥＭＳ（Micro Electro Mechanical System：微小電気機械システム）を利用した加速度計およびジャイロセンサからなるＭＥＭＳセンサ部、１６と２６はＭＥＭＳを利用したＧＰＳセンサ部、１７と２７は温度計、１８は磁気方位センサで外部の地磁気の観測から方位を検出する外部基準方位センサ部である。なお、加速度計とジャイロセンサは、ロール、ピッチ、ヨーの３軸につき一つずつセンサを設ける。また、温度計１７はジャイロセンサそれぞれにつき一つずつ計３個設ける。
【０００４】
図３（ａ）に示すＡＨＲＳの動作の流れを簡単に説明する。ＭＥＭＳセンサ部１５のジャイロセンサと加速度計で角速度と加速度を検出し、センサ誤差補正計算部１１に出力する。センサ誤差補正計算部１１では、予め推定された誤差推定値に基づいて、入力された角速度と加速度の値をリアルタイムで補正し、航法・姿勢方位計算部１２に出力する。航法・姿勢方位計算部１２ではその補正後の角速度と加速度を用いて移動体の姿勢と方位を計算し、パイロットと誤差推定計算部１３に出力する。姿勢／方位計算の際には予め推定された姿勢、方位の誤差推定値を利用する。また、誤差推定計算部１３での誤差推定に利用するため、航法・姿勢方位計算部１２では移動体の速度と位置も計算し、あわせて誤差推定計算部１３に出力する。基準姿勢計算部１４はＭＥＭＳセンサ部１５から出力される加速度のデータを直接利用して移動体の姿勢を計算し、誤差推定計算部１３に出力する。また、ＧＰＳセンサ部１６からは移動体の速度および位置データが、温度計１７からは環境温度データが、外部基準方位センサ部１８からは移動体の方位データが、それぞれ誤差推定計算部１３に出力される。
基準姿勢計算部１４、ＧＰＳセンサ部１６、外部基準方位センサ部１８の出力は、誤差が発散しないため、それぞれ移動体の姿勢、速度、位置、方位データの計算値に含まれる誤差を測る際の基準データとして利用することができる。
誤差推定計算部１３は航法・姿勢方位計算部１２から入力される姿勢、方位、速度、位置データの計算値と、各種基準データを比較することによって、加速度計とジャイロセンサが有する誤差や、航法・姿勢方位計算部１２の計算した姿勢／方位データが含む誤差の大きさをリアルタイムで推定する。そして、誤差の推定値をセンサ誤差補正計算部１１と航法・姿勢方位計算部１２に出力し、新たな誤差推定値として補正計算に利用する。
【０００５】
次に、図３（ｂ）に示すＩＮＳについて簡単に説明する。ＩＮＳは、移動体の姿勢や方位を計算し出力する点でＡＨＲＳと同質の装置である。そのため、基本的な機能ブロックの構成や各ブロックの動作は同じである。ただし、一般にＩＮＳは姿勢や方位のほかに、移動体の速度や位置も出力する機能を有する。また、ＩＮＳはＡＨＲＳに比べ１〜２桁程度要求される精度が高く、ジャイロセンサや加速度計も高精度のものが使用される。図３（ａ）にある基準姿勢計算部１４や外部基準方位センサ部１８に相当するブロックは設けられていない。これは、これらのブロックから出力される基準データの精度が低いためにＩＮＳの要求精度に見合わず、かえって姿勢や方位データの計算値に悪影響を及ぼすためである。
【０００６】
図４は前記ヘッドトラッカの概念図である。遭難者を捜索中の救助ヘリコプタの中に、このヘリコプタを操縦するパイロットと、遭難者を目視で捜索する捜索者が搭乗している。捜索者は、ヘッドトラッカを装備したヘルメットを装着している。図４中、縦軸ＸＮは北、横軸ＹＥは東を意味する。ＸｐとＹｐはヘリコプタ機体座標のロール軸、ピッチ軸であり、ＸとＹは捜索者のヘルメットの頭部座標のロール軸、ピッチ軸である。Ψｐは機体の方位角、Ψはヘルメットの方位角である。
【０００７】
ヘッドトラッカは装着者（捜索者）の頭部の姿勢および方位を計算し、パイロットに情報を出力する。捜索者が遭難者を発見した際には、パイロットはヘッドトラッカが計算した情報に従い、マニュアルあるいは自動（オートパイロット）でヘッドトラッカの姿勢および方位の方向に自機を誘導する。すなわち、機体の方位角Ψｐとヘルメットの方位角Ψが一致するように機体を制御することにより、捜索者が遭難者に向いている方向に機体を誘導することができる。
【０００８】
図３に戻りこのヘッドトラッカの動作をさらに説明する。図３（ａ）、（ｂ）ともに、ＭＥＭＳセンサ部１５（および２５）は、ＭＥＭＳ加速度計３個とＭＥＭＳジャイロセンサ３個で構成され、ヘッドトラッカ装着者の頭部運動３軸（ロール、ピッチ、ヨー）方向加速度と３軸周りの角速度を検出し、そのデジタルデータをＲＳ２３２Ｃ等のデータバスでセンサ誤差補正計算部１１（および２１）へ出力する。
【０００９】
センサ誤差補正計算部１１（および２１）は、加速度計とジャイロセンサの誤差（バイアス安定性誤差）を校正するための誤差推定データが誤差推定計算部１３（および２３）から入力され、次式のような補正計算（校正計算の意で一般にはキャリブレーションと呼ばれる）を行なう。
Ａｂ，Ｗｂは３軸加速度ベクトルと角速度ベクトルである。
Ａｂｃ，Ｗｂｃは誤差校正後の加速度および角速度であり、航法・姿勢方位計算部１２（および２２）へ出力される。
【００１０】
航法・姿勢方位計算部１２（および２２）は、校正後の加速度と角速度データから、ヘッドトラッカの装着者（捜索者）の頭部の姿勢および方位を計算し、パイロットに情報を出力する。また、姿勢および方位データは誤差推定計算部１３（および２３）にも出力される。
【００１１】
誤差推定計算部１３（および２３）は、（ａ）に示す場合は、航法・姿勢方位計算部１２で計算された姿勢および方位データと外部基準方位センサ部１６から出力された基準方位データとを比較し、バッチシ−ケンシャル最小二乗法により、加速度計およびジャイロセンサのセンサ誤差と航法誤差、姿勢方位誤差等を推定する。（ｂ）に示す場合は、航法・姿勢方位計算部２２で計算された速度および位置データとＧＰＳセンサ部２６から出力された基準速度および位置データとを比較し、バッチシ−ケンシャル最小二乗法により、加速度計およびジャイロセンサのセンサ誤差と航法誤差、姿勢方位誤差等を推定する。
【００１２】
以下にＡＨＲＳに適用した場合、ＩＮＳに適用した場合のそれぞれについて簡単に説明する。
【００１３】
まずＡＨＲＳに適用した場合について説明する。
航法・姿勢方位計算部１２で計算された姿勢および方位データは、
基準姿勢計算部１４から出力される基準姿勢データと外部基準方位センサ部１６から出力される基準方位データは、
と表される。ここで＊印項は真値であり、Δ印項は誤差を示す。
式（２）と式（３）の差をとると、
となる。
【００１４】
ΔΘはジャイロセンサ誤差（バイアス）により時間とともに増大するが、ΔΘrは増大しない。そのため、比較し差をとるタイミングｔの時間を充分大きくとる（運用精度仕様で決まる）ことによりΔΘを取り出すことができる。ＡＨＲＳ場合は速度誤差と位置誤差の推定が不要であるため、ΔΘのみを考慮すれば充分である。
【００１５】
ΔΘは表現を簡略化すると、
の微分で表される。Δｗはジャイロセンサ誤差のバイアス成分であり、ΔΘの時間的増大が理解できる。
【００１６】
次にＩＮＳに適用する場合について説明する。
航法・姿勢方位計算部２２で計算された速度（位置は省く）は、
ＧＰＳセンサ部２６から出力される基準速度データは、
と表される。
式（６）と式（７）の差をとると、
となる。
【００１７】
ΔＶは、ジャイロセンサ誤差および加速度計誤差により時間とともに増大するが、ΔＶrは増大しない。そのため、比較し差をとるタイミング時間ｔを充分大きくとる（運用精度仕様で決まる）ことによりΔＶを取り出すことが出来る。
【００１８】
ΔＶは表現を簡略化すると、
のように微分で表される。Δａは加速度計誤差、Δｗはジャイロセンサ誤差、ｇは重力加速度である。
【００１９】
式（９）からΔＶを取り出せば、間接的にΔΘを取り出すことができる。以下ジャイロセンサ誤差に限定して推定の原理を説明する。Δa,ΔＶも数式で表現し推定できる。
【００２０】
ジャイロセンサ誤差は次式のモデルで数式表現することができる。
ここで、式（１０）の右辺第１項はバイアス（直流）誤差であり、第２項は環境温度変化による誤差である。バイアス誤差は一定値であるため、容易に推定することができる。
【００２１】
温度変化による誤差は
のように温度変化の多項式で近似する。
ここで、係数Ｋｉは実験的に予め決めておく。
ΔＴは温度計２７の出力を用いる。このようにしてジャイロセンサ誤差Δｗを推定計算する。Δｗの推定から式（５）（ＡＨＲＳ）あるいは式（９）（ＩＮＳ）モデルでΔΘを推定する。推定手法はカルマンフィルタ や最小二乗法が用いられている。
【００２２】
【特許文献１】特開平５−６６７１３号公報
【発明の開示】
【発明が解決しようとする課題】
【００２３】
しかし、ジャイロセンサの温度ドリフト誤差の補正に温度計を使用しているため、機器の構成が複雑化してしまう上に信頼性が悪い。また、ジャイロセンサの温度ドリフト誤差補正に温度変化の多項式誤差モデルを使用しているが、温度計にも個体差があるため再現性が悪い。さらに、同じタイプのジャイロセンサでも個々に特性が異なるため、モデル係数がばらつきモデル決定が難しい。そのため温度ドリフト補正が悪く精度の高いジャイロセンサの校正効果が得られない。
【００２４】
姿勢および方位基準データ取得において、ＡＨＲＳの場合は加速度計出力と磁気センサ出力を用い、システム構成が統一されていないため開発効率が悪くなってしまう。
【００２５】
特にＡＨＲＳの場合は、運動加速度の影響で姿勢基準が大きく振られるため、基準姿勢計算部１４の出力誤差が大きく、加速度計誤差の補正精度が悪くなる。さらに、地磁気は場所によって変動するため、外部基準方位センサ部１８の出力の安定性が悪く、ジャイロセンサ誤差補正の精度が低い。
【００２６】
本発明は、このような従来の慣性航法装置およびその誤差補正方法が有していた問題を解決しようとするものであり、ＡＨＲＳとＩＮＳでシステム構成を共通化することができ、温度計を使用することなくジャイロセンサの温度ドリフト誤差を補正することができ、基準となる姿勢・方位データを使わずにジャイロセンサ誤差と加速度計誤差を補正することができる慣性航法装置およびその誤差補正方法を実現することを目的とする。
【課題を解決するための手段】
【００２７】
上記のような目的を達成するために、本発明の請求項１では、移動体に取り付けられたジャイロセンサと加速度計の出力を利用して移動体の位置や姿勢を求める慣性航法装置において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算部と、
前記センサ誤差補正計算部により補正されたジャイロセンサおよび加速度計の出力を利用して移動体の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記移動体の位置や姿勢の計算結果を補正する航法・姿勢方位計算部と、
前記航法・姿勢方位計算部から出力された位置や姿勢の情報と移動体に取り付けられたＧＰＳセンサより得られる絶対位置および絶対速度の情報とを比較して、前記ジャイロセンサにおける温度ドリフトの影響を分離する温度誤差推定部と、
前記温度誤差推定部により分離した温度ドリフトの影響などを利用して、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算部で使用するセンサ誤差データを校正するとともに、前記計算で得られた位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正するセンサ固有誤差推定部と、
を有することを特徴とする。
【００２８】
請求項２では、請求項１に記載の慣性航法装置において、前記温度誤差推定部は、温度ドリフトの影響を時間の多項式あるいは自己回帰モデルで近似し、低次の項を利用して温度誤差の影響を抽出することを特徴とする。
【００２９】
請求項３では、請求項１または２に記載の慣性航法装置において、前記移動体は、使用者の頭部に装着され、頭部の位置や姿勢を検出するヘッドトラッカであることを特徴とする。
【００３０】
請求項４では、移動体に取り付けられたジャイロセンサと加速度計の出力を利用して移動体の位置や姿勢を求める慣性航法装置の誤差補正方法において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算ステップと、
前記センサ誤差補正計算ステップにより補正されたジャイロセンサおよび加速度計の出力を利用して移動体の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記移動体の位置や姿勢の計算結果を補正する航法・姿勢方位計算ステップと、
前記航法・姿勢方位計算ステップから出力された位置や姿勢の情報と移動体に取り付けられたＧＰＳセンサより得られる絶対位置および絶対速度の情報とを比較して、前記ジャイロセンサにおける温度ドリフトの影響を分離する温度誤差推定ステップと、
前記温度誤差推定ステップにより分離した温度ドリフトの影響などを利用して、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算ステップで使用するセンサ誤差データを校正するとともに、前記計算で得られた位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算ステップで使用する姿勢／位置誤差データを校正するセンサ固有誤差推定ステップと、
を有することを特徴とする。
【００３１】
請求項５では、請求項４に記載の慣性航法装置の誤差補正方法において、前記温度誤差推定ステップは、温度ドリフトの影響を時間の多項式あるいは自己回帰モデルで近似し、低次の項を利用して温度誤差の影響を抽出することを特徴とする。
【００３２】
請求項６では、請求項４または５に記載の慣性航法装置の誤差補正方法において、前記移動体は、使用者の頭部に装着され、頭部の位置や姿勢を検出するヘッドトラッカであることを特徴とする。
【発明の効果】
【００３３】
このように、航法計算出力の速度および位置データとＧＰＳセンサ部から出力される基準速度および位置データのみから、計算でジャイロセンサの温度ドリフトの影響を分離して推定し、その推定値を用いて補正をかけることにより、ＡＨＲＳとＩＮＳでシステム構成を共通化することができ、温度計を使用することなくジャイロセンサの温度ドリフト誤差を補正することができ、基準となる姿勢・方位データを使わずにジャイロセンサ誤差と加速度計誤差を補正することができる慣性航法装置およびその誤差補正方法を実現することができる。
【００３４】
温度計が不要となるため構成が簡易化され、機器の信頼性を向上させることができる。また、再現性の悪い温度ドリフトを有するジャイロセンサでも機器に採用することができるようになるため、製品コストを低減できる。
【００３５】
ＡＨＲＳでもＩＮＳでも全く同じシステム構成となるため、同一製品で広範な分野へ応用可能である。
【００３６】
加速度計による姿勢基準データの計算が不要となるため、補正の精度は移動体の運動に左右されない。さらに、磁気センサが不要なため安定したセンサ誤差補正ができるとともに、機器の構成が簡易化し、システム全体の信頼性を向上させることができる。
【発明を実施するための最良の形態】
【００３７】
以下、図面を用いて本発明の慣性航法装置およびその誤差補正方法を説明する。
【実施例１】
【００３８】
図１は本発明による慣性航法装置およびその誤差補正方法の一実施例を示す構成図である。図中、１はセンサ誤差補正計算部、２は航法・姿勢方位計算部、３は温度誤差推定計算部、４はセンサ固有誤差推定計算部、５はＭＥＭＳを利用した加速度計およびジャイロセンサからなるＭＥＭＳセンサ部、６はＭＥＭＳを利用したＧＰＳセンサ部である。
【００３９】
本発明例のセンサ誤差補正計算部１および航法・姿勢方位計算部２、ＭＥＭＳセンサ部５、ＧＰＳセンサ部６は図３に示した従来例と同様の動作を行うものである。以下は温度誤差推定計算部３の誤差推定計算（Batch Sequential Filter）について、その構成と動作について説明する。
【００４０】
航法・姿勢方位計算部２で計算された速度データとＧＰＳセンサ部６から得られる基準速度データの比較から、ジャイロセンサ誤差と加速度計誤差に起因する速度誤差を取り出す計算は従来例の式（８）で求められ、その微分は式（９）で表される。
【００４１】
ここで、加速度計誤差Δａとジャイロセンサ誤差Δｗは
である。
【００４２】
ΔｗＴ（ｔ,ΔＴ）は環境温度変化ΔＴに左右されるが、その変動は時間のみで表すことができるため、式（１２）は、
と表現できる。Δａｂ，Δｗｂは加速度計バイアス誤差とジャイロセンサバイアス誤差であり、一定値である。
【００４３】
ここで、Δａｂ，Δｗｂをいかに正確に推定して補正するかということ、そしてΔＶとΔΘをいかに正確に推定しその誤差をフィードバック制御するかということが重要となる。この推定の妨げ要因となるのがジャイロセンサ温度ドリフトΔｗＴ（ｔ）である。温度誤差推定計算部３の目的は、可能な限りこの温度ドリフトの影響を推定し削除することである。
【００４４】
式（９）、式（１３）を積分すると、
が得られる。第１項目はセンサ固有誤差推定部４のカルマンフィルタで推定すべき項であり、この推定精度は第２項目の推定精度に左右される。
【００４５】
【００４６】
もし、この推定が完全（真値＝推定値）なら、
より、
となり、温度ドリフトの影響を完全に削除することができる。この結果ΔＶを温度誤差推定部３からセンサ固有誤差推定部４に出力し、センサ固有誤差推定部４で、ジャイロセンサバイアス誤差、加速度計バイアス誤差、速度および位置誤差、姿勢および方位角誤差等を高分解能で推定することができる。
【００４７】
以下、この温度ドリフトの影響の推定原理について式（１４）にもどり説明する。
なお、
である。
【００４８】
ここで、ΔＶｂ１，ΔＶｂ２，ΔＶＴの特性は、式（１８）と式（１９）に着目すると、バイアス項の一回積分、二回積分、そして温度ドリフト周期変動項積分であることから、各時間の１次増大傾向、時間の２次増大傾向、時間周期変動傾向のように違うことが分かる。この時間増大傾向は厳密には８４分周期（シューラー周期）を描きながら増大する。一方温度ドリフトの影響は８４分より短い短周期変動であるため、上記の時間増大傾向を有する成分と区別することが可能である。
【００４９】
この性質を利用して温度ドリフトの影響ΔＶＴ（ｔ）を次式のように、時間の多項式あるいは自己回帰モデルで近似し、ΔＶデータからモデル係数を推定する。以下は時間の多項式近似で説明する。
この係数推定はあるバッチ区間毎に行い、サンプリング時間毎に進行するバッチシーケンシャル型の最小二乗法を用いる。
【００５０】
複雑な周期変動もバッチ区間を短く取ることにより、単純な変動とみなすことができる。モデル次数とバッチ区間は温度変動周期と運用精度から決定する。また、ｃ０はバイアス項であり、バイアス誤差も推定する。
【００５１】
図２は、ジャイロセンサの温度ドリフト誤差補正について従来例と本発明を比較した図である。図２（ａ）は従来例、（ｂ）は本発明による結果を示している。横軸は時間、縦軸はジャイロセンサ誤差の大きさである。（ａ）の従来例では、温度センサや磁気方位センサを使用しているため、誤差の収束までに時間がかかり、補正の精度も悪い。また、地磁気の変化やセンサの安定性の悪さに影響され、誤差の大きさも変動する。一方（ｂ）の本発明では、誤差の収束までにかかる時間も短くなり、精度も向上する。
【００５２】
なお、本発明は水平面を機械サーボ制御で実現する従来のプラットフォーム方式にも適用可能である。また、本実施例では救難ヘリコプタ搭乗者用のヘルメットに装備されたヘッドトラッカを示したが、これ以外にも、陸上の移動体（人含む）などへの適用も考えられる。
【００５３】
また、本発明の他への応用としては、広く移動体の小型ナビゲーション装置に応用できる。たとえば、一般民生機器ではカーナビやロボットの速度／位置装置や姿勢／方位装置が考えられる。防衛関連分野では航空機や艦艇および陸上用特車の航法装置、さらにはミサイルや誘導弾用の慣性誘導装置が考えられる。
【図面の簡単な説明】
【００５４】
【図１】図１は本発明による慣性航法装置およびその誤差補正方法の一実施例を示す構成図。
【図２】図２はジャイロセンサの温度ドリフト誤差補正について従来例と本発明を比較した図。
【図３】図３は従来の慣性航法装置およびその誤差補正方法の一例を示す構成図。
【図４】図４は航空機の搭乗者が頭部に装着したヘッドトラッカの概念図。
【符号の説明】
【００５５】
１，１１，２１ センサ誤差補正計算部
２，１２，２２ 航法・姿勢方位計算部
３ 温度誤差推定計算部
４ センサ固有誤差推定部
５，１５，２５ ＭＥＭＳセンサ部
６，１６，２６ ＧＰＳセンサ部
１３，２３ 誤差推定部
１４ 基準姿勢計算部
１７，２７ 温度計
１８ 外部基準方位センサ部
【技術分野】
【０００１】
本発明は、移動体の現在位置をナビゲーションする装置において、定量的に求めることができないセンサ誤差を補償して精度の高い現在位置を提供することができる慣性航法装置およびその誤差補正方法に関するものである。
【背景技術】
【０００２】
図３は従来の慣性航法装置およびその誤差補正方法の一例を示すブロック図であり、航空機の搭乗者が頭部に装着したヘッドトラッカの構成を示したものである。ヘッドトラッカとは、一般にヘルメット装着者頭部の姿勢や方位角を提供するＡＨＲＳ（Attitude Heading Reference System：姿勢方位基準装置）またはＩＮＳ（Inertial Navigation System：慣性航法装置）である。
【０００３】
図３（ａ）はＡＨＲＳの場合、（ｂ）はＩＮＳの場合を示している。図３（ａ）において、１１と２１はセンサ誤差補正計算部、１２と２２は航法・姿勢方位計算部、１３と２３は誤差推定計算部、１４は基準姿勢計算部、１５と２５はＭＥＭＳ（Micro Electro Mechanical System：微小電気機械システム）を利用した加速度計およびジャイロセンサからなるＭＥＭＳセンサ部、１６と２６はＭＥＭＳを利用したＧＰＳセンサ部、１７と２７は温度計、１８は磁気方位センサで外部の地磁気の観測から方位を検出する外部基準方位センサ部である。なお、加速度計とジャイロセンサは、ロール、ピッチ、ヨーの３軸につき一つずつセンサを設ける。また、温度計１７はジャイロセンサそれぞれにつき一つずつ計３個設ける。
【０００４】
図３（ａ）に示すＡＨＲＳの動作の流れを簡単に説明する。ＭＥＭＳセンサ部１５のジャイロセンサと加速度計で角速度と加速度を検出し、センサ誤差補正計算部１１に出力する。センサ誤差補正計算部１１では、予め推定された誤差推定値に基づいて、入力された角速度と加速度の値をリアルタイムで補正し、航法・姿勢方位計算部１２に出力する。航法・姿勢方位計算部１２ではその補正後の角速度と加速度を用いて移動体の姿勢と方位を計算し、パイロットと誤差推定計算部１３に出力する。姿勢／方位計算の際には予め推定された姿勢、方位の誤差推定値を利用する。また、誤差推定計算部１３での誤差推定に利用するため、航法・姿勢方位計算部１２では移動体の速度と位置も計算し、あわせて誤差推定計算部１３に出力する。基準姿勢計算部１４はＭＥＭＳセンサ部１５から出力される加速度のデータを直接利用して移動体の姿勢を計算し、誤差推定計算部１３に出力する。また、ＧＰＳセンサ部１６からは移動体の速度および位置データが、温度計１７からは環境温度データが、外部基準方位センサ部１８からは移動体の方位データが、それぞれ誤差推定計算部１３に出力される。
基準姿勢計算部１４、ＧＰＳセンサ部１６、外部基準方位センサ部１８の出力は、誤差が発散しないため、それぞれ移動体の姿勢、速度、位置、方位データの計算値に含まれる誤差を測る際の基準データとして利用することができる。
誤差推定計算部１３は航法・姿勢方位計算部１２から入力される姿勢、方位、速度、位置データの計算値と、各種基準データを比較することによって、加速度計とジャイロセンサが有する誤差や、航法・姿勢方位計算部１２の計算した姿勢／方位データが含む誤差の大きさをリアルタイムで推定する。そして、誤差の推定値をセンサ誤差補正計算部１１と航法・姿勢方位計算部１２に出力し、新たな誤差推定値として補正計算に利用する。
【０００５】
次に、図３（ｂ）に示すＩＮＳについて簡単に説明する。ＩＮＳは、移動体の姿勢や方位を計算し出力する点でＡＨＲＳと同質の装置である。そのため、基本的な機能ブロックの構成や各ブロックの動作は同じである。ただし、一般にＩＮＳは姿勢や方位のほかに、移動体の速度や位置も出力する機能を有する。また、ＩＮＳはＡＨＲＳに比べ１〜２桁程度要求される精度が高く、ジャイロセンサや加速度計も高精度のものが使用される。図３（ａ）にある基準姿勢計算部１４や外部基準方位センサ部１８に相当するブロックは設けられていない。これは、これらのブロックから出力される基準データの精度が低いためにＩＮＳの要求精度に見合わず、かえって姿勢や方位データの計算値に悪影響を及ぼすためである。
【０００６】
図４は前記ヘッドトラッカの概念図である。遭難者を捜索中の救助ヘリコプタの中に、このヘリコプタを操縦するパイロットと、遭難者を目視で捜索する捜索者が搭乗している。捜索者は、ヘッドトラッカを装備したヘルメットを装着している。図４中、縦軸ＸＮは北、横軸ＹＥは東を意味する。ＸｐとＹｐはヘリコプタ機体座標のロール軸、ピッチ軸であり、ＸとＹは捜索者のヘルメットの頭部座標のロール軸、ピッチ軸である。Ψｐは機体の方位角、Ψはヘルメットの方位角である。
【０００７】
ヘッドトラッカは装着者（捜索者）の頭部の姿勢および方位を計算し、パイロットに情報を出力する。捜索者が遭難者を発見した際には、パイロットはヘッドトラッカが計算した情報に従い、マニュアルあるいは自動（オートパイロット）でヘッドトラッカの姿勢および方位の方向に自機を誘導する。すなわち、機体の方位角Ψｐとヘルメットの方位角Ψが一致するように機体を制御することにより、捜索者が遭難者に向いている方向に機体を誘導することができる。
【０００８】
図３に戻りこのヘッドトラッカの動作をさらに説明する。図３（ａ）、（ｂ）ともに、ＭＥＭＳセンサ部１５（および２５）は、ＭＥＭＳ加速度計３個とＭＥＭＳジャイロセンサ３個で構成され、ヘッドトラッカ装着者の頭部運動３軸（ロール、ピッチ、ヨー）方向加速度と３軸周りの角速度を検出し、そのデジタルデータをＲＳ２３２Ｃ等のデータバスでセンサ誤差補正計算部１１（および２１）へ出力する。
【０００９】
センサ誤差補正計算部１１（および２１）は、加速度計とジャイロセンサの誤差（バイアス安定性誤差）を校正するための誤差推定データが誤差推定計算部１３（および２３）から入力され、次式のような補正計算（校正計算の意で一般にはキャリブレーションと呼ばれる）を行なう。
Ａｂ，Ｗｂは３軸加速度ベクトルと角速度ベクトルである。
Ａｂｃ，Ｗｂｃは誤差校正後の加速度および角速度であり、航法・姿勢方位計算部１２（および２２）へ出力される。
【００１０】
航法・姿勢方位計算部１２（および２２）は、校正後の加速度と角速度データから、ヘッドトラッカの装着者（捜索者）の頭部の姿勢および方位を計算し、パイロットに情報を出力する。また、姿勢および方位データは誤差推定計算部１３（および２３）にも出力される。
【００１１】
誤差推定計算部１３（および２３）は、（ａ）に示す場合は、航法・姿勢方位計算部１２で計算された姿勢および方位データと外部基準方位センサ部１６から出力された基準方位データとを比較し、バッチシ−ケンシャル最小二乗法により、加速度計およびジャイロセンサのセンサ誤差と航法誤差、姿勢方位誤差等を推定する。（ｂ）に示す場合は、航法・姿勢方位計算部２２で計算された速度および位置データとＧＰＳセンサ部２６から出力された基準速度および位置データとを比較し、バッチシ−ケンシャル最小二乗法により、加速度計およびジャイロセンサのセンサ誤差と航法誤差、姿勢方位誤差等を推定する。
【００１２】
以下にＡＨＲＳに適用した場合、ＩＮＳに適用した場合のそれぞれについて簡単に説明する。
【００１３】
まずＡＨＲＳに適用した場合について説明する。
航法・姿勢方位計算部１２で計算された姿勢および方位データは、
基準姿勢計算部１４から出力される基準姿勢データと外部基準方位センサ部１６から出力される基準方位データは、
と表される。ここで＊印項は真値であり、Δ印項は誤差を示す。
式（２）と式（３）の差をとると、
となる。
【００１４】
ΔΘはジャイロセンサ誤差（バイアス）により時間とともに増大するが、ΔΘrは増大しない。そのため、比較し差をとるタイミングｔの時間を充分大きくとる（運用精度仕様で決まる）ことによりΔΘを取り出すことができる。ＡＨＲＳ場合は速度誤差と位置誤差の推定が不要であるため、ΔΘのみを考慮すれば充分である。
【００１５】
ΔΘは表現を簡略化すると、
の微分で表される。Δｗはジャイロセンサ誤差のバイアス成分であり、ΔΘの時間的増大が理解できる。
【００１６】
次にＩＮＳに適用する場合について説明する。
航法・姿勢方位計算部２２で計算された速度（位置は省く）は、
ＧＰＳセンサ部２６から出力される基準速度データは、
と表される。
式（６）と式（７）の差をとると、
となる。
【００１７】
ΔＶは、ジャイロセンサ誤差および加速度計誤差により時間とともに増大するが、ΔＶrは増大しない。そのため、比較し差をとるタイミング時間ｔを充分大きくとる（運用精度仕様で決まる）ことによりΔＶを取り出すことが出来る。
【００１８】
ΔＶは表現を簡略化すると、
のように微分で表される。Δａは加速度計誤差、Δｗはジャイロセンサ誤差、ｇは重力加速度である。
【００１９】
式（９）からΔＶを取り出せば、間接的にΔΘを取り出すことができる。以下ジャイロセンサ誤差に限定して推定の原理を説明する。Δa,ΔＶも数式で表現し推定できる。
【００２０】
ジャイロセンサ誤差は次式のモデルで数式表現することができる。
ここで、式（１０）の右辺第１項はバイアス（直流）誤差であり、第２項は環境温度変化による誤差である。バイアス誤差は一定値であるため、容易に推定することができる。
【００２１】
温度変化による誤差は
のように温度変化の多項式で近似する。
ここで、係数Ｋｉは実験的に予め決めておく。
ΔＴは温度計２７の出力を用いる。このようにしてジャイロセンサ誤差Δｗを推定計算する。Δｗの推定から式（５）（ＡＨＲＳ）あるいは式（９）（ＩＮＳ）モデルでΔΘを推定する。推定手法はカルマンフィルタ や最小二乗法が用いられている。
【００２２】
【特許文献１】特開平５−６６７１３号公報
【発明の開示】
【発明が解決しようとする課題】
【００２３】
しかし、ジャイロセンサの温度ドリフト誤差の補正に温度計を使用しているため、機器の構成が複雑化してしまう上に信頼性が悪い。また、ジャイロセンサの温度ドリフト誤差補正に温度変化の多項式誤差モデルを使用しているが、温度計にも個体差があるため再現性が悪い。さらに、同じタイプのジャイロセンサでも個々に特性が異なるため、モデル係数がばらつきモデル決定が難しい。そのため温度ドリフト補正が悪く精度の高いジャイロセンサの校正効果が得られない。
【００２４】
姿勢および方位基準データ取得において、ＡＨＲＳの場合は加速度計出力と磁気センサ出力を用い、システム構成が統一されていないため開発効率が悪くなってしまう。
【００２５】
特にＡＨＲＳの場合は、運動加速度の影響で姿勢基準が大きく振られるため、基準姿勢計算部１４の出力誤差が大きく、加速度計誤差の補正精度が悪くなる。さらに、地磁気は場所によって変動するため、外部基準方位センサ部１８の出力の安定性が悪く、ジャイロセンサ誤差補正の精度が低い。
【００２６】
本発明は、このような従来の慣性航法装置およびその誤差補正方法が有していた問題を解決しようとするものであり、ＡＨＲＳとＩＮＳでシステム構成を共通化することができ、温度計を使用することなくジャイロセンサの温度ドリフト誤差を補正することができ、基準となる姿勢・方位データを使わずにジャイロセンサ誤差と加速度計誤差を補正することができる慣性航法装置およびその誤差補正方法を実現することを目的とする。
【課題を解決するための手段】
【００２７】
上記のような目的を達成するために、本発明の請求項１では、移動体に取り付けられたジャイロセンサと加速度計の出力を利用して移動体の位置や姿勢を求める慣性航法装置において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算部と、
前記センサ誤差補正計算部により補正されたジャイロセンサおよび加速度計の出力を利用して移動体の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記移動体の位置や姿勢の計算結果を補正する航法・姿勢方位計算部と、
前記航法・姿勢方位計算部から出力された位置や姿勢の情報と移動体に取り付けられたＧＰＳセンサより得られる絶対位置および絶対速度の情報とを比較して、前記ジャイロセンサにおける温度ドリフトの影響を分離する温度誤差推定部と、
前記温度誤差推定部により分離した温度ドリフトの影響などを利用して、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算部で使用するセンサ誤差データを校正するとともに、前記計算で得られた位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正するセンサ固有誤差推定部と、
を有することを特徴とする。
【００２８】
請求項２では、請求項１に記載の慣性航法装置において、前記温度誤差推定部は、温度ドリフトの影響を時間の多項式あるいは自己回帰モデルで近似し、低次の項を利用して温度誤差の影響を抽出することを特徴とする。
【００２９】
請求項３では、請求項１または２に記載の慣性航法装置において、前記移動体は、使用者の頭部に装着され、頭部の位置や姿勢を検出するヘッドトラッカであることを特徴とする。
【００３０】
請求項４では、移動体に取り付けられたジャイロセンサと加速度計の出力を利用して移動体の位置や姿勢を求める慣性航法装置の誤差補正方法において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算ステップと、
前記センサ誤差補正計算ステップにより補正されたジャイロセンサおよび加速度計の出力を利用して移動体の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記移動体の位置や姿勢の計算結果を補正する航法・姿勢方位計算ステップと、
前記航法・姿勢方位計算ステップから出力された位置や姿勢の情報と移動体に取り付けられたＧＰＳセンサより得られる絶対位置および絶対速度の情報とを比較して、前記ジャイロセンサにおける温度ドリフトの影響を分離する温度誤差推定ステップと、
前記温度誤差推定ステップにより分離した温度ドリフトの影響などを利用して、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算ステップで使用するセンサ誤差データを校正するとともに、前記計算で得られた位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算ステップで使用する姿勢／位置誤差データを校正するセンサ固有誤差推定ステップと、
を有することを特徴とする。
【００３１】
請求項５では、請求項４に記載の慣性航法装置の誤差補正方法において、前記温度誤差推定ステップは、温度ドリフトの影響を時間の多項式あるいは自己回帰モデルで近似し、低次の項を利用して温度誤差の影響を抽出することを特徴とする。
【００３２】
請求項６では、請求項４または５に記載の慣性航法装置の誤差補正方法において、前記移動体は、使用者の頭部に装着され、頭部の位置や姿勢を検出するヘッドトラッカであることを特徴とする。
【発明の効果】
【００３３】
このように、航法計算出力の速度および位置データとＧＰＳセンサ部から出力される基準速度および位置データのみから、計算でジャイロセンサの温度ドリフトの影響を分離して推定し、その推定値を用いて補正をかけることにより、ＡＨＲＳとＩＮＳでシステム構成を共通化することができ、温度計を使用することなくジャイロセンサの温度ドリフト誤差を補正することができ、基準となる姿勢・方位データを使わずにジャイロセンサ誤差と加速度計誤差を補正することができる慣性航法装置およびその誤差補正方法を実現することができる。
【００３４】
温度計が不要となるため構成が簡易化され、機器の信頼性を向上させることができる。また、再現性の悪い温度ドリフトを有するジャイロセンサでも機器に採用することができるようになるため、製品コストを低減できる。
【００３５】
ＡＨＲＳでもＩＮＳでも全く同じシステム構成となるため、同一製品で広範な分野へ応用可能である。
【００３６】
加速度計による姿勢基準データの計算が不要となるため、補正の精度は移動体の運動に左右されない。さらに、磁気センサが不要なため安定したセンサ誤差補正ができるとともに、機器の構成が簡易化し、システム全体の信頼性を向上させることができる。
【発明を実施するための最良の形態】
【００３７】
以下、図面を用いて本発明の慣性航法装置およびその誤差補正方法を説明する。
【実施例１】
【００３８】
図１は本発明による慣性航法装置およびその誤差補正方法の一実施例を示す構成図である。図中、１はセンサ誤差補正計算部、２は航法・姿勢方位計算部、３は温度誤差推定計算部、４はセンサ固有誤差推定計算部、５はＭＥＭＳを利用した加速度計およびジャイロセンサからなるＭＥＭＳセンサ部、６はＭＥＭＳを利用したＧＰＳセンサ部である。
【００３９】
本発明例のセンサ誤差補正計算部１および航法・姿勢方位計算部２、ＭＥＭＳセンサ部５、ＧＰＳセンサ部６は図３に示した従来例と同様の動作を行うものである。以下は温度誤差推定計算部３の誤差推定計算（Batch Sequential Filter）について、その構成と動作について説明する。
【００４０】
航法・姿勢方位計算部２で計算された速度データとＧＰＳセンサ部６から得られる基準速度データの比較から、ジャイロセンサ誤差と加速度計誤差に起因する速度誤差を取り出す計算は従来例の式（８）で求められ、その微分は式（９）で表される。
【００４１】
ここで、加速度計誤差Δａとジャイロセンサ誤差Δｗは
である。
【００４２】
ΔｗＴ（ｔ,ΔＴ）は環境温度変化ΔＴに左右されるが、その変動は時間のみで表すことができるため、式（１２）は、
と表現できる。Δａｂ，Δｗｂは加速度計バイアス誤差とジャイロセンサバイアス誤差であり、一定値である。
【００４３】
ここで、Δａｂ，Δｗｂをいかに正確に推定して補正するかということ、そしてΔＶとΔΘをいかに正確に推定しその誤差をフィードバック制御するかということが重要となる。この推定の妨げ要因となるのがジャイロセンサ温度ドリフトΔｗＴ（ｔ）である。温度誤差推定計算部３の目的は、可能な限りこの温度ドリフトの影響を推定し削除することである。
【００４４】
式（９）、式（１３）を積分すると、
が得られる。第１項目はセンサ固有誤差推定部４のカルマンフィルタで推定すべき項であり、この推定精度は第２項目の推定精度に左右される。
【００４５】
【００４６】
もし、この推定が完全（真値＝推定値）なら、
より、
となり、温度ドリフトの影響を完全に削除することができる。この結果ΔＶを温度誤差推定部３からセンサ固有誤差推定部４に出力し、センサ固有誤差推定部４で、ジャイロセンサバイアス誤差、加速度計バイアス誤差、速度および位置誤差、姿勢および方位角誤差等を高分解能で推定することができる。
【００４７】
以下、この温度ドリフトの影響の推定原理について式（１４）にもどり説明する。
なお、
である。
【００４８】
ここで、ΔＶｂ１，ΔＶｂ２，ΔＶＴの特性は、式（１８）と式（１９）に着目すると、バイアス項の一回積分、二回積分、そして温度ドリフト周期変動項積分であることから、各時間の１次増大傾向、時間の２次増大傾向、時間周期変動傾向のように違うことが分かる。この時間増大傾向は厳密には８４分周期（シューラー周期）を描きながら増大する。一方温度ドリフトの影響は８４分より短い短周期変動であるため、上記の時間増大傾向を有する成分と区別することが可能である。
【００４９】
この性質を利用して温度ドリフトの影響ΔＶＴ（ｔ）を次式のように、時間の多項式あるいは自己回帰モデルで近似し、ΔＶデータからモデル係数を推定する。以下は時間の多項式近似で説明する。
この係数推定はあるバッチ区間毎に行い、サンプリング時間毎に進行するバッチシーケンシャル型の最小二乗法を用いる。
【００５０】
複雑な周期変動もバッチ区間を短く取ることにより、単純な変動とみなすことができる。モデル次数とバッチ区間は温度変動周期と運用精度から決定する。また、ｃ０はバイアス項であり、バイアス誤差も推定する。
【００５１】
図２は、ジャイロセンサの温度ドリフト誤差補正について従来例と本発明を比較した図である。図２（ａ）は従来例、（ｂ）は本発明による結果を示している。横軸は時間、縦軸はジャイロセンサ誤差の大きさである。（ａ）の従来例では、温度センサや磁気方位センサを使用しているため、誤差の収束までに時間がかかり、補正の精度も悪い。また、地磁気の変化やセンサの安定性の悪さに影響され、誤差の大きさも変動する。一方（ｂ）の本発明では、誤差の収束までにかかる時間も短くなり、精度も向上する。
【００５２】
なお、本発明は水平面を機械サーボ制御で実現する従来のプラットフォーム方式にも適用可能である。また、本実施例では救難ヘリコプタ搭乗者用のヘルメットに装備されたヘッドトラッカを示したが、これ以外にも、陸上の移動体（人含む）などへの適用も考えられる。
【００５３】
また、本発明の他への応用としては、広く移動体の小型ナビゲーション装置に応用できる。たとえば、一般民生機器ではカーナビやロボットの速度／位置装置や姿勢／方位装置が考えられる。防衛関連分野では航空機や艦艇および陸上用特車の航法装置、さらにはミサイルや誘導弾用の慣性誘導装置が考えられる。
【図面の簡単な説明】
【００５４】
【図１】図１は本発明による慣性航法装置およびその誤差補正方法の一実施例を示す構成図。
【図２】図２はジャイロセンサの温度ドリフト誤差補正について従来例と本発明を比較した図。
【図３】図３は従来の慣性航法装置およびその誤差補正方法の一例を示す構成図。
【図４】図４は航空機の搭乗者が頭部に装着したヘッドトラッカの概念図。
【符号の説明】
【００５５】
１，１１，２１ センサ誤差補正計算部
２，１２，２２ 航法・姿勢方位計算部
３ 温度誤差推定計算部
４ センサ固有誤差推定部
５，１５，２５ ＭＥＭＳセンサ部
６，１６，２６ ＧＰＳセンサ部
１３，２３ 誤差推定部
１４ 基準姿勢計算部
１７，２７ 温度計
１８ 外部基準方位センサ部
【特許請求の範囲】
【請求項１】
移動体に取り付けられたジャイロセンサと加速度計の出力を利用して移動体の位置や姿勢を求める慣性航法装置において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算部と、
前記センサ誤差補正計算部により補正されたジャイロセンサおよび加速度計の出力を利用して移動体の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記移動体の位置や姿勢の計算結果を補正する航法・姿勢方位計算部と、
前記航法・姿勢方位計算部から出力された位置や姿勢の情報と移動体に取り付けられたＧＰＳセンサより得られる絶対位置および絶対速度の情報とを比較して、前記ジャイロセンサにおける温度ドリフトの影響を分離する温度誤差推定部と、
前記温度誤差推定部により分離した温度ドリフトの影響などを利用して、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算部で使用するセンサ誤差データを校正するとともに、前記計算で得られた位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正するセンサ固有誤差推定部と、
を有することを特徴とする慣性航法装置。
【請求項２】
前記温度誤差推定部は、温度ドリフトの影響を時間の多項式あるいは自己回帰モデルで近似し、低次の項を利用して温度誤差の影響を抽出することを特徴とする請求項１に記載の慣性航法装置。
【請求項３】
前記移動体は、使用者の頭部に装着され、頭部の位置や姿勢を検出するヘッドトラッカであることを特徴とする請求項１または２に記載の慣性航法装置。
【請求項４】
移動体に取り付けられたジャイロセンサと加速度計の出力を利用して移動体の位置や姿勢を求める慣性航法装置の誤差補正方法において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算ステップと、
前記センサ誤差補正計算ステップにより補正されたジャイロセンサおよび加速度計の出力を利用して移動体の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記移動体の位置や姿勢の計算結果を補正する航法・姿勢方位計算ステップと、
前記航法・姿勢方位計算ステップから出力された位置や姿勢の情報と移動体に取り付けられたＧＰＳセンサより得られる絶対位置および絶対速度の情報とを比較して、前記ジャイロセンサにおける温度ドリフトの影響を分離する温度誤差推定ステップと、
前記温度誤差推定ステップにより分離した温度ドリフトの影響などを利用して、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算ステップで使用するセンサ誤差データを校正するとともに、前記計算で得られた位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算ステップで使用する姿勢／位置誤差データを校正するセンサ固有誤差推定ステップと、
を有することを特徴とする慣性航法装置の誤差補正方法。
【請求項５】
前記温度誤差推定ステップは、温度ドリフトの影響を時間の多項式あるいは自己回帰モデルで近似し、低次の項を利用して温度誤差の影響を抽出することを特徴とする請求項４に記載の慣性航法装置の誤差補正方法。
【請求項６】
前記移動体は、使用者の頭部に装着され、頭部の位置や姿勢を検出するヘッドトラッカであることを特徴とする請求項４または５に記載の慣性航法装置の誤差補正方法。
【請求項１】
移動体に取り付けられたジャイロセンサと加速度計の出力を利用して移動体の位置や姿勢を求める慣性航法装置において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算部と、
前記センサ誤差補正計算部により補正されたジャイロセンサおよび加速度計の出力を利用して移動体の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記移動体の位置や姿勢の計算結果を補正する航法・姿勢方位計算部と、
前記航法・姿勢方位計算部から出力された位置や姿勢の情報と移動体に取り付けられたＧＰＳセンサより得られる絶対位置および絶対速度の情報とを比較して、前記ジャイロセンサにおける温度ドリフトの影響を分離する温度誤差推定部と、
前記温度誤差推定部により分離した温度ドリフトの影響などを利用して、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算部で使用するセンサ誤差データを校正するとともに、前記計算で得られた位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正するセンサ固有誤差推定部と、
を有することを特徴とする慣性航法装置。
【請求項２】
前記温度誤差推定部は、温度ドリフトの影響を時間の多項式あるいは自己回帰モデルで近似し、低次の項を利用して温度誤差の影響を抽出することを特徴とする請求項１に記載の慣性航法装置。
【請求項３】
前記移動体は、使用者の頭部に装着され、頭部の位置や姿勢を検出するヘッドトラッカであることを特徴とする請求項１または２に記載の慣性航法装置。
【請求項４】
移動体に取り付けられたジャイロセンサと加速度計の出力を利用して移動体の位置や姿勢を求める慣性航法装置の誤差補正方法において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算ステップと、
前記センサ誤差補正計算ステップにより補正されたジャイロセンサおよび加速度計の出力を利用して移動体の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記移動体の位置や姿勢の計算結果を補正する航法・姿勢方位計算ステップと、
前記航法・姿勢方位計算ステップから出力された位置や姿勢の情報と移動体に取り付けられたＧＰＳセンサより得られる絶対位置および絶対速度の情報とを比較して、前記ジャイロセンサにおける温度ドリフトの影響を分離する温度誤差推定ステップと、
前記温度誤差推定ステップにより分離した温度ドリフトの影響などを利用して、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算ステップで使用するセンサ誤差データを校正するとともに、前記計算で得られた位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算ステップで使用する姿勢／位置誤差データを校正するセンサ固有誤差推定ステップと、
を有することを特徴とする慣性航法装置の誤差補正方法。
【請求項５】
前記温度誤差推定ステップは、温度ドリフトの影響を時間の多項式あるいは自己回帰モデルで近似し、低次の項を利用して温度誤差の影響を抽出することを特徴とする請求項４に記載の慣性航法装置の誤差補正方法。
【請求項６】
前記移動体は、使用者の頭部に装着され、頭部の位置や姿勢を検出するヘッドトラッカであることを特徴とする請求項４または５に記載の慣性航法装置の誤差補正方法。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２００７−２３２４４４（Ｐ２００７−２３２４４４Ａ）
【公開日】平成１９年９月１３日（２００７．９．１３）
【国際特許分類】
【出願番号】特願２００６−５１９５９（Ｐ２００６−５１９５９）
【出願日】平成１８年２月２８日（２００６．２．２８）
【出願人】（０００００６５０７）横河電機株式会社 (4,443)
【出願人】（０００２３２３５７）横河電子機器株式会社 (109)
【Ｆターム（参考）】
【公開日】平成１９年９月１３日（２００７．９．１３）
【国際特許分類】
【出願日】平成１８年２月２８日（２００６．２．２８）
【出願人】（０００００６５０７）横河電機株式会社 (4,443)
【出願人】（０００２３２３５７）横河電子機器株式会社 (109)
【Ｆターム（参考）】
[ Back to top ]