慣性航法装置およびその誤差補正方法
【課題】磁気方位センサの誤差や荒天時の風などの外乱に左右されることなく、より安定して姿勢／方位の初期データの精度を向上させることができる慣性航法装置を実現する。
【解決手段】移動体搭乗者のヘルメットに取付けられたジャイロセンサと加速度計の出力を利用して当該搭乗者頭部の位置や姿勢を求める慣性航法装置において、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算部と、センサ誤差補正計算部により補正されたジャイロセンサおよび加速度計の出力を利用して頭部の位置や姿勢を計算するとともに、頭部の位置や姿勢の計算結果を補正する航法・姿勢方位計算部と、頭部が静止状態にあるときに航法・姿勢方位計算部より出力される速度データを速度誤差として利用し、センサ誤差補正計算部で使用するセンサ誤差データを校正するとともに、姿勢／位置誤差データを校正する誤差推定計算部とを有することを特徴とする。
【解決手段】移動体搭乗者のヘルメットに取付けられたジャイロセンサと加速度計の出力を利用して当該搭乗者頭部の位置や姿勢を求める慣性航法装置において、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算部と、センサ誤差補正計算部により補正されたジャイロセンサおよび加速度計の出力を利用して頭部の位置や姿勢を計算するとともに、頭部の位置や姿勢の計算結果を補正する航法・姿勢方位計算部と、頭部が静止状態にあるときに航法・姿勢方位計算部より出力される速度データを速度誤差として利用し、センサ誤差補正計算部で使用するセンサ誤差データを校正するとともに、姿勢／位置誤差データを校正する誤差推定計算部とを有することを特徴とする。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、使用者の頭部に装着されたジャイロセンサや加速度センサの出力を利用して、使用者の頭部の向きや姿勢を算出する慣性航法装置において、定量的に求めることができないセンサ誤差を補償して姿勢／方位の初期データをより正確に決定することができる慣性航法装置およびその誤差補正方法に関するものである。
【背景技術】
【０００２】
図４は従来の慣性航法装置およびその誤差補正方法の一例を示すブロック図であり、航空機の搭乗者が頭部に装着したヘッドトラッカの構成を示したものである。ヘッドトラッカ装置とは、航空機などの搭乗者のヘルメットに装備され、ヘルメット装着者頭部の姿勢や方位角を提供する装置である。
【０００３】
図４において、１１はセンサ誤差補正計算部、１２は航法・姿勢方位計算部、１３は誤差推定計算部、１４は基準姿勢計算部、１５はＭＥＭＳ（Micro Electro Mechanical System：微小電気機械システム）を利用した加速度計およびジャイロセンサからなるＭＥＭＳセンサ部、１６は磁気方位センサで外部の地磁気の観測から方位を検出する外部基準方位センサ部である。なお、加速度計とジャイロセンサは、ロール、ピッチ、ヨーの３軸につき一つずつセンサを設ける。
【０００４】
図４に示すヘッドトラッカの動作の流れを簡単に説明する。ＭＥＭＳセンサ部１５のジャイロセンサと加速度計で角速度と加速度を検出し、センサ誤差補正計算部１１に出力する。センサ誤差補正計算部１１では、予め与えられた誤差推定値に基づいて入力された角速度と加速度の値をリアルタイムで補正し、航法・姿勢方位計算部１２に出力する。航法・姿勢方位計算部１２ではその補正後の角速度と加速度を用いて頭部の姿勢や方位を計算し、パイロットと誤差推定計算部１３に出力する。姿勢／方位計算の際には予め与えられた姿勢、方位の誤差推定値を利用する。基準姿勢計算部１４はＭＥＭＳセンサ部１５から出力される加速度のデータを直接利用して頭部の姿勢を計算し、誤差推定計算部１３に出力する。また、外部基準方位センサ部１６からは頭部の方位データが、それぞれ誤差推定計算部１３に出力される。
基準姿勢計算部１４、外部基準方位センサ部１６の出力は、誤差が発散しないため、それぞれ頭部の姿勢／方位データの計算値に含まれる誤差を測る際の基準データとして利用することができる。
誤差推定計算部１３は航法・姿勢方位計算部１２から入力される姿勢／方位データの計算値と、各種基準データを比較することによって、加速度計とジャイロセンサが有する誤差や、航法・姿勢方位計算部１２の計算した姿勢／方位データが含む誤差の大きさをリアルタイムで推定する。そして、誤差の推定値をセンサ誤差補正計算部１１と航法・姿勢方位計算部１２に出力し、新たな誤差推定値として補正計算に利用する。
【０００５】
図５は前記ヘッドトラッカの概念図である。遭難者を捜索中の救助ヘリコプタの中に、このヘリコプタを操縦するパイロットと、遭難者を目視で捜索する捜索者が搭乗している。捜索者は、ヘッドトラッカを装備したヘルメットを装着している。図５中、縦軸ＸＮは北、横軸ＹＥは東を意味する。ＸｐとＹｐはヘリコプタ機体座標のロール軸、ピッチ軸であり、ＸとＹは捜索者のヘルメットの頭部座標のロール軸、ピッチ軸である。Ψｐは機体の方位角、Ψはヘルメットの方位角である。
【０００６】
ヘッドトラッカは装着者（捜索者）の頭部の姿勢および方位を計算し、パイロットに情報を出力する。捜索者が遭難者を発見した際には、パイロットはヘッドトラッカが計算した情報に従い、マニュアルあるいは自動（オートパイロット）でヘッドトラッカの姿勢および方位の方向に自機を誘導する。すなわち、機体の方位角Ψｐとヘルメットの方位角Ψが一致するように機体を制御することにより、捜索者が遭難者に向いている方向に機体を誘導することができる。
【０００７】
図４に戻りこのヘッドトラッカの動作をさらに説明する。ＭＥＭＳセンサ部１５は、ＭＥＭＳ加速度計３個とＭＥＭＳジャイロセンサ３個で構成され、ヘッドトラッカ装着者の頭部運動３軸（ロール、ピッチ、ヨー）方向加速度と３軸周りの角速度を検出し、そのデジタルデータをＲＳ２３２Ｃ等のデータバスでセンサ誤差補正計算部１１へ出力する。
【０００８】
センサ誤差補正計算部１１は、加速度計とジャイロセンサの誤差（バイアス安定性誤差）を校正するための誤差推定データが誤差推定計算部１３から入力され、次式のような補正計算（校正計算の意で一般にはキャリブレーションと呼ばれる）を行なう。
Ａｂ，Ｗｂは３軸加速度ベクトルと角速度ベクトルであり、初期の静止時にはほぼゼロである。 Ａｂｃ、Ｗｂｃは誤差校正後の加速度および角速度であり、航法・姿勢方位計算部１２へ出力される。
【０００９】
航法・姿勢方位計算部１２は、校正後の加速度と角速度データから、ヘッドトラッカの装着者（捜索者）の頭部の姿勢および方位を計算し、パイロットに情報を出力する。また、姿勢および方位データは誤差推定計算部１３にも出力される。
【００１０】
航法・姿勢方位計算部１２は、誤差推定計算部１３から入力される姿勢および方位角データの誤差推定値のフィードバックにより、誤差の増大が抑えられる。速度および位置データの誤差についても、同様に誤差推定計算部１３から速度および位置の誤差推定値が航法・姿勢方位計算部にフィードバックされ、誤差の増大が抑えられる。
【００１１】
基準姿勢計算部１４はＭＥＭＳセンサ部１５から入力される加速度データから基準姿勢データを計算し、誤差推定計算部１３へ出力する。
【００１２】
誤差推定計算部１３は、航法・姿勢方位計算部１２で計算された姿勢および方位データと、外部基準方位センサ部１６から出力された基準方位データと、基準姿勢計算部１４から出力された基準姿勢データとを比較し、カルマンフィルタあるいは最小二乗法により、加速度計およびジャイロセンサのセンサ誤差と航法誤差、姿勢方位誤差等を推定する。誤差の推定値の精度は、これら誤差をどの程度正確に数式で表現することができるか否かによる。
【００１３】
基準データより姿勢および方位角の誤差、加速度計およびジャイロセンサ誤差の推定の原理を以下に説明する。
θ、Ψは航法・姿勢方位計算部で計算された姿勢角と方位角である。ここで＊印項は真値であり、Δ印項は誤差を示す。ΔθｇとΔΨｇはジャイロセンサ誤差と加速度計誤差による誤差である。また、添え字ｂが付されている項はジャイロセンサと加速度計のバイアス誤差による誤差で、時間とともに増大する誤差である。添え字ｍが付された項は運動（加速度、角速度）に影響される誤差で、初期の静止時は問題にならない。添え字Ｔが付された項は環境温度変化に影響される誤差である。
【００１４】
基準姿勢角と方位角は、
である。Δθｒは加速度計誤差による姿勢角誤差、ΔΨｒは磁気方位センサ誤差で場所（ｐ）にも影響される。
【００１５】
式（２）と式（３）を比較し、その差をとると、
となる。
【００１６】
ここで、Δθｇ(ｔ)、ΔΨｇ(ｔ)は、加速度計とジャイロセンサのバイアス誤差により時間とともに増大する。すなわち、航法・姿勢方位計算部での航法計算、姿勢角および方位角計算には積分処理が行なわれるため、センサのバイアス誤差があると、速度、位置、姿勢角、方位角の誤差は時間経過とともに増大する。
【００１７】
これに対し、基準姿勢計算部１４の基準姿勢データの姿勢角は直接加速度計出力と重力加速度の比で算出されるため、式（４）の基準姿勢角誤差Δθｒ(ｔ)は時間経過とともには増大するものではない。また、式（４）の基準方位角誤差ΔΨｒ(ｔ，ｐ)も時間経過とともに増大するものではない。
【００１８】
そこで、比較すべき時間インターバルを運用精度に合わせて充分大きくとると、
のように、Δθｇ、ΔΨｇを充分大きく取り出すことが出来る。なお、ａ>>ｂは、ａはｂより非常に大きいことを意味する。
【００１９】
式（２）よりΔθｇ、ΔΨｇの内訳のバイアス誤差、運動に起因する誤差、温度に起因する誤差等を数式でモデル化すれば、カルマンフィルタや最小二乗法によりΔθｇ、ΔΨｇを推定することができる。また、速度誤差および位置誤差についてもΔθｇ、ΔΨｇと密接に関係しているので、速度誤差および位置誤差の数式モデルから同様に推定することが可能である。
【００２０】
【特許文献１】特許第３４９０７０６号
【発明の開示】
【発明が解決しようとする課題】
【００２１】
しかし、方位データの計算値の誤差推定においては外部基準方位センサ部１６の磁気方位センサが有する誤差が修正限界となり、方位データの誤差推定はこの磁気方位センサに大きく左右されてしまう。
【００２２】
基準姿勢計算部では、基準姿勢データを加速度計出力と重力加速度の比より計算しているため、飛行前（移動開始前）の初期静止状態では一般に問題なく基準姿勢データを求めることができるが、天候の悪い緊急運用時では風で振られる運動の影響を大きく受ける。
【００２３】
基準方位データを得るために外部基準方位センサ部を設けるため、機器全体の構成が複雑化する上にコスト高となる。また、地磁気は場所によって変動するため方位基準の安定性が悪く、方位データの誤差補正の精度が低い。
【００２４】
本発明は、このような従来の慣性航法装置が有していた問題を解決しようとするものであり、磁気方位センサの誤差や荒天時の風などの外乱に左右されることなく、より安定して姿勢／方位の初期データの精度を向上させることができる慣性航法装置およびその誤差補正方法を実現することを目的とする。
【課題を解決するための手段】
【００２５】
上記のような目的を達成するために、本発明の請求項１では、移動体の搭乗者のヘルメットに取り付けられたジャイロセンサと加速度計の出力を利用してヘルメット装着者の頭部の位置や姿勢を求める慣性航法装置において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算部と、
前記センサ誤差補正計算部により補正されたジャイロセンサおよび加速度計の出力を利用して前記頭部の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記頭部の位置や姿勢の計算結果を補正する航法・姿勢方位計算部と、
前記頭部が静止状態にあるときに前記航法・姿勢方位計算部より出力される速度データを速度誤差として利用し、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算部で使用するセンサ誤差データを校正するとともに、前記位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正する誤差推定計算部と、
を有することを特徴とする。
【００２６】
請求項２では、請求項１に記載の慣性航法装置において、前記誤差推定計算部は、カルマンフィルタを利用して誤差の推定値を計算することを特徴とする。
【００２７】
請求項３では、請求項１に記載の慣性航法装置において、前記誤差推定計算部は、最小二乗法を利用して誤差の推定値を計算することを特徴とする。
【００２８】
請求項４では、移動体の搭乗者のヘルメットに取り付けられたジャイロセンサと加速度計の出力を利用してヘルメット装着者の頭部の位置や姿勢を求める慣性航法装置の誤差補正方法において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算ステップと、
前記センサ誤差補正計算ステップにより補正されたジャイロセンサおよび加速度計の出力を利用して前記頭部の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記頭部の位置や姿勢の計算結果を補正する航法・姿勢方位計算ステップと、
前記頭部が静止状態にあるときに前記航法・姿勢方位計算部より出力される速度データを速度誤差として利用し、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算ステップで使用するセンサ誤差データを校正するとともに、前記位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正する誤差推定計算ステップと、
を有することを特徴とする。
【００２９】
請求項５では、請求項４に記載の慣性航法装置の誤差補正方法において、前記誤差推定計算ステップは、カルマンフィルタを利用して誤差の推定値を計算することを特徴とする。
【００３０】
請求項６では、請求項４に記載の慣性航法装置の誤差補正方法において、前記誤差推定計算ステップは、最小二乗法を利用して誤差の推定値を計算することを特徴とする。
【発明の効果】
【００３１】
慣性航法装置が電源ＯＮ時の静止した状態において、初期の姿勢および方位データを正確に決定しておけば、効率の良い運用に資することができる。上記のように、慣性航法装置が静止状態にあるときに航法・姿勢方位計算部から出力される速度データを速度誤差として利用して補正をかけることにより、磁気方位センサの誤差や荒天時の風などの外乱に左右されることなく、より安定して姿勢／方位の初期データの精度を向上させることができる慣性航法装置およびその誤差補正方法を実現することができる。
【００３２】
磁気方位センサを使わないことにより、方位データの誤差推定が磁気方位センサ固有の誤差や外部の地磁気の影響に左右されない。そのため、より安定しかつ高精度なジャイロセンサや加速度計の誤差補正を実現することができ、パイロットに対してもより安定かつ高精度な姿勢／方位データを提供することができる。また、磁気方位センサが不要となるため機器全体の構成が簡易化され、コストを下げることができる。
【００３３】
加速度計の出力から計算された基準姿勢データを使わないことにより、荒天時など外乱がある場合でも安定で高精度な姿勢データを得ることができる。
【発明を実施するための最良の形態】
【００３４】
以下、図面を用いて本発明の慣性航法装置およびその誤差補正方法を説明する。
【実施例１】
【００３５】
図１は本発明による慣性航法装置およびその誤差補正方法の一実施例を示す構成図である。図中、１はセンサ誤差補正計算部、２は航法・姿勢方位計算部、３は誤差推定計算部、４はＭＥＭＳを利用した加速度計およびジャイロセンサからなるＭＥＭＳセンサ部である。
【００３６】
本発明例のセンサ誤差補正計算部１およびＭＥＭＳセンサ部４は、図４に示した従来例と同じである。以下は航法・姿勢方位計算部２と誤差推定計算部３についてその構成と動作について説明する。
【００３７】
まず航法・姿勢方位計算部２について説明する。
【００３８】
ヘルメット装着者の速度および位置データは次式で計算される。
ここで、Ｖｎは航法座標系で表現された速度ベクトルであり、北方向成分ｖＮ、東方向成分ｖＥ、鉛直方向成分ｖＤに分解される。
また、Ωｅｎは航法座標系での地球自転速度の北軸周り、東軸周り、鉛直軸周り、各成分からなる３行３列の行列である。Ωｐｎは航法座標系での地球の周りを移動する移動速度の北軸周り、東軸周り、鉛直軸周り、各成分からなる３行３列の行列である。Ｇｎは重力加速度ベクトルの航法座標表現であり、Ａｎは移動加速度ベクトルの航法座標表現で加速度計出力である。Ｒλ、ＲΛは緯度方向と経度方向の地球半径である。ｈは飛行高度である。λ、Λは緯度と経度である。
【００３９】
Ｕｖｎ、ｕλ、ｕΛは誤差推定計算部３の誤差推定計算で得られた速度誤差、位置誤差等の推定値を用いて次式で求められるフィードバック制御量である。
Ｋｖｎ、Ｋλ、ＫΛは速度誤差、緯度誤差、経度誤差を最小にするフィードバックゲインである。
【００４０】
ヘルメット装着者の姿勢および方位角計算は次式で求められる。
ここで、Ｃｎｈは３行３列のヘルメット座標ｈから航法座標ｎへの座標変換行列である。
Ψ、θ、φは頭部の方位角、ピッチ角、ロール角である。
Ｋｗｈは姿勢および方位データの誤差を最小にするフィードバックゲインである。
【００４１】
次に、誤差推定計算部３について説明する。
航法・姿勢方位計算部２から出力される速度データから、どのようにジャイロセンサ誤差や加速度計誤差、姿勢角誤差、速度誤差等を推定するかについて、以下のように簡易モデルで説明する。実際はもっと複雑であるが考え方は同じである。
以下で（ＸＮ、ＹＥ）は航法座標で北軸と東軸を、（Ｘ，Ｙ）はヘルメット装着の頭部座標のロール軸とピッチ軸を示す。加速度計とジャイロセンサはストラップダウン方式で直接頭部基準軸のＸとＹに取り付けられる。Ψは方位角である。
図２に航法座標と頭部座標の座標系を示す。
【００４２】
航法・姿勢方位計算部２の航法計算出力の北速度データ出力は次式の積分で表されるものとする。
ＡＮは北方向の加速度であり、初期状態の静止時のため０である。
δａＮは加速度誤差である。結局、初期は静止時ゆえ計算された速度出力は速度誤差そのものであり、式（９）は、
のように表現できる。ここが、本発明のポイントである。
【００４３】
ここで、北方向加速度誤差δａＮは、
で表される。
【００４４】
ここで、Δａｘ、ΔａｙはＸ加速度計とＹ加速度計のバイアス誤差で、δθＥは次式で表される東軸周りの角度誤差である。Ψ０，ｇは初期方位角と重力加速度である。
ここで、Δωｘ，ΔωｙはＸジャイロセンサとＹジャイロセンサのバイアス誤差である。
【００４５】
誤差推定計算部３の目的は、式（１０）の積分の速度データ入力より、どのようにして加速度計のバイアス誤差、ジャイロセンサのバイアス誤差、角度誤差、速度誤差等を推定するかである。以下に最小二乗法を用いて上記誤差を求める場合について説明する。なお、 カルマンフィルタを用いても同様に誤差を求めることが可能である。
【００４６】
初期方位角Ψ０は外乱が無い限り常数である。したがって、式（１２）の時間積分は、
となる。
【００４７】
式（１０）の積分は次式の出力データで与えられる。
【００４８】
式（１４）式を行列表現すると、
と書ける。ここでｈ(ｔ)は１行６列の観測行列で時間ｔの関数、Θが推定すべき未知パラメータで６行１列の行列である。
【００４９】
初期方位角Ψ０と初期緯度λ０は、次式で簡易的に最初に推定しておく。
ここで、ωｘ，ωｙはＸジャイロセンサとＹジャイロセンサの出力で、
である。Ωは地球自転速度で１５°／Ｈｒである。
【００５０】
λ０は初期緯度で、式（１７）より
で与えられる。±の決定は北半球にいるか南半球にいるかで決まる。
【００５１】
式（１５）は未知パラメータが６個であるため、これらすべての未知パラメータを求めるには６本以上の式が必要となる。そこで、時間t１,t２,t３,…,ｔｎ毎の出力から次式を準備する。
【００５２】
式（１９）に最小二乗法を適用すると、未知パラメータΘの推定は
で求まる。
【００５３】
角度誤差と速度誤差の推定は 式（１３）と式（１４）より
で求まる。
【００５４】
以上のようにして求められた加速度計バイアス誤差とジャイロセンサバイアス誤差の推定値はセンサ誤差補正計算部１に出力される。
また、角度誤差と速度誤差の推定値は航法・姿勢方位計算部２へ出力され、式（７）、式（８）の速度誤差と姿勢/方位角誤差フィードバック制御に用いられる。
【００５５】
ここでは、簡単に角度誤差と速度誤差の推定のみについて説明したが、実際は同じ考え方に基づき速度誤差、位置誤差、姿勢/方位誤差等の推定が実行される。なお、実際の本発明の推定はノイズの影響を最小にするためにカルマンフィルタが適用されるが、その場合でも誤差の数式表現が異なるのみである。
【００５６】
図３は、航法・姿勢方位計算部２で計算された方位データの誤差について従来例と本発明を比較した図である。図３（ａ）は従来例、（ｂ）は本発明による結果を示している。横軸は時間、縦軸は方位誤差の大きさである。（ａ）の従来例では、磁気方位センサを使用しているため、磁気方位センサの誤差により修正限界が決まり、ある一定のレベルよりも誤差を小さくすることができない。また、地磁気の変化などにより誤差の大きさも安定しない。一方（ｂ）の本発明では、静止時は基準速度がゼロであるということを利用して、方位誤差を限りなくゼロに近づけることができる。
【００５７】
なお、本発明ではセンサを直接運動体に取り付けるが、機械サーボ制御で実現される水平面（プラットホーム方式）に取り付けても良い。また、静止時に風等外乱が無視できない場合は、外乱除去用のフィルタを設けることにより適用可能である。この場合は本発明の速度データに外乱速度が重畳しているので、誤差推定計算部３の前段に外乱除去フィルタを設け、外乱除去後の速度データを誤差推定計算に用いる。
【００５８】
また、本発明では、ジャイロセンサや加速度計の誤差を推定値で修正し、速度／位置データなどの航法誤差や姿勢／方位角データの誤差を推定値によるフィードバック制御で修正する。このフィードバック制御は最適制御を主とするが、ＰＩＤ（Proportional Integral Differential）制御でも良い。
【図面の簡単な説明】
【００５９】
【図１】図１は本発明による慣性航法装置およびその誤差補正方法の一実施例を示す構成図。
【図２】図２は航法座標と頭部座標の座標系を示す図。
【図３】図３は方位データの誤差について従来例と本発明を比較した図。
【図４】図４は従来の慣性航法装置およびその誤差補正方法の一例を示す構成図。
【図５】図５は航空機の搭乗者が頭部に装着したヘッドトラッカの概念図。
【符号の説明】
【００６０】
１，１１ センサ誤差補正計算部
２，１２ 航法・姿勢方位計算部
３，１３ 誤差推定計算部
４，１５ ＭＥＭＳセンサ部
１４ 基準姿勢計算部
１６ 外部基準方位センサ部
【技術分野】
【０００１】
本発明は、使用者の頭部に装着されたジャイロセンサや加速度センサの出力を利用して、使用者の頭部の向きや姿勢を算出する慣性航法装置において、定量的に求めることができないセンサ誤差を補償して姿勢／方位の初期データをより正確に決定することができる慣性航法装置およびその誤差補正方法に関するものである。
【背景技術】
【０００２】
図４は従来の慣性航法装置およびその誤差補正方法の一例を示すブロック図であり、航空機の搭乗者が頭部に装着したヘッドトラッカの構成を示したものである。ヘッドトラッカ装置とは、航空機などの搭乗者のヘルメットに装備され、ヘルメット装着者頭部の姿勢や方位角を提供する装置である。
【０００３】
図４において、１１はセンサ誤差補正計算部、１２は航法・姿勢方位計算部、１３は誤差推定計算部、１４は基準姿勢計算部、１５はＭＥＭＳ（Micro Electro Mechanical System：微小電気機械システム）を利用した加速度計およびジャイロセンサからなるＭＥＭＳセンサ部、１６は磁気方位センサで外部の地磁気の観測から方位を検出する外部基準方位センサ部である。なお、加速度計とジャイロセンサは、ロール、ピッチ、ヨーの３軸につき一つずつセンサを設ける。
【０００４】
図４に示すヘッドトラッカの動作の流れを簡単に説明する。ＭＥＭＳセンサ部１５のジャイロセンサと加速度計で角速度と加速度を検出し、センサ誤差補正計算部１１に出力する。センサ誤差補正計算部１１では、予め与えられた誤差推定値に基づいて入力された角速度と加速度の値をリアルタイムで補正し、航法・姿勢方位計算部１２に出力する。航法・姿勢方位計算部１２ではその補正後の角速度と加速度を用いて頭部の姿勢や方位を計算し、パイロットと誤差推定計算部１３に出力する。姿勢／方位計算の際には予め与えられた姿勢、方位の誤差推定値を利用する。基準姿勢計算部１４はＭＥＭＳセンサ部１５から出力される加速度のデータを直接利用して頭部の姿勢を計算し、誤差推定計算部１３に出力する。また、外部基準方位センサ部１６からは頭部の方位データが、それぞれ誤差推定計算部１３に出力される。
基準姿勢計算部１４、外部基準方位センサ部１６の出力は、誤差が発散しないため、それぞれ頭部の姿勢／方位データの計算値に含まれる誤差を測る際の基準データとして利用することができる。
誤差推定計算部１３は航法・姿勢方位計算部１２から入力される姿勢／方位データの計算値と、各種基準データを比較することによって、加速度計とジャイロセンサが有する誤差や、航法・姿勢方位計算部１２の計算した姿勢／方位データが含む誤差の大きさをリアルタイムで推定する。そして、誤差の推定値をセンサ誤差補正計算部１１と航法・姿勢方位計算部１２に出力し、新たな誤差推定値として補正計算に利用する。
【０００５】
図５は前記ヘッドトラッカの概念図である。遭難者を捜索中の救助ヘリコプタの中に、このヘリコプタを操縦するパイロットと、遭難者を目視で捜索する捜索者が搭乗している。捜索者は、ヘッドトラッカを装備したヘルメットを装着している。図５中、縦軸ＸＮは北、横軸ＹＥは東を意味する。ＸｐとＹｐはヘリコプタ機体座標のロール軸、ピッチ軸であり、ＸとＹは捜索者のヘルメットの頭部座標のロール軸、ピッチ軸である。Ψｐは機体の方位角、Ψはヘルメットの方位角である。
【０００６】
ヘッドトラッカは装着者（捜索者）の頭部の姿勢および方位を計算し、パイロットに情報を出力する。捜索者が遭難者を発見した際には、パイロットはヘッドトラッカが計算した情報に従い、マニュアルあるいは自動（オートパイロット）でヘッドトラッカの姿勢および方位の方向に自機を誘導する。すなわち、機体の方位角Ψｐとヘルメットの方位角Ψが一致するように機体を制御することにより、捜索者が遭難者に向いている方向に機体を誘導することができる。
【０００７】
図４に戻りこのヘッドトラッカの動作をさらに説明する。ＭＥＭＳセンサ部１５は、ＭＥＭＳ加速度計３個とＭＥＭＳジャイロセンサ３個で構成され、ヘッドトラッカ装着者の頭部運動３軸（ロール、ピッチ、ヨー）方向加速度と３軸周りの角速度を検出し、そのデジタルデータをＲＳ２３２Ｃ等のデータバスでセンサ誤差補正計算部１１へ出力する。
【０００８】
センサ誤差補正計算部１１は、加速度計とジャイロセンサの誤差（バイアス安定性誤差）を校正するための誤差推定データが誤差推定計算部１３から入力され、次式のような補正計算（校正計算の意で一般にはキャリブレーションと呼ばれる）を行なう。
Ａｂ，Ｗｂは３軸加速度ベクトルと角速度ベクトルであり、初期の静止時にはほぼゼロである。 Ａｂｃ、Ｗｂｃは誤差校正後の加速度および角速度であり、航法・姿勢方位計算部１２へ出力される。
【０００９】
航法・姿勢方位計算部１２は、校正後の加速度と角速度データから、ヘッドトラッカの装着者（捜索者）の頭部の姿勢および方位を計算し、パイロットに情報を出力する。また、姿勢および方位データは誤差推定計算部１３にも出力される。
【００１０】
航法・姿勢方位計算部１２は、誤差推定計算部１３から入力される姿勢および方位角データの誤差推定値のフィードバックにより、誤差の増大が抑えられる。速度および位置データの誤差についても、同様に誤差推定計算部１３から速度および位置の誤差推定値が航法・姿勢方位計算部にフィードバックされ、誤差の増大が抑えられる。
【００１１】
基準姿勢計算部１４はＭＥＭＳセンサ部１５から入力される加速度データから基準姿勢データを計算し、誤差推定計算部１３へ出力する。
【００１２】
誤差推定計算部１３は、航法・姿勢方位計算部１２で計算された姿勢および方位データと、外部基準方位センサ部１６から出力された基準方位データと、基準姿勢計算部１４から出力された基準姿勢データとを比較し、カルマンフィルタあるいは最小二乗法により、加速度計およびジャイロセンサのセンサ誤差と航法誤差、姿勢方位誤差等を推定する。誤差の推定値の精度は、これら誤差をどの程度正確に数式で表現することができるか否かによる。
【００１３】
基準データより姿勢および方位角の誤差、加速度計およびジャイロセンサ誤差の推定の原理を以下に説明する。
θ、Ψは航法・姿勢方位計算部で計算された姿勢角と方位角である。ここで＊印項は真値であり、Δ印項は誤差を示す。ΔθｇとΔΨｇはジャイロセンサ誤差と加速度計誤差による誤差である。また、添え字ｂが付されている項はジャイロセンサと加速度計のバイアス誤差による誤差で、時間とともに増大する誤差である。添え字ｍが付された項は運動（加速度、角速度）に影響される誤差で、初期の静止時は問題にならない。添え字Ｔが付された項は環境温度変化に影響される誤差である。
【００１４】
基準姿勢角と方位角は、
である。Δθｒは加速度計誤差による姿勢角誤差、ΔΨｒは磁気方位センサ誤差で場所（ｐ）にも影響される。
【００１５】
式（２）と式（３）を比較し、その差をとると、
となる。
【００１６】
ここで、Δθｇ(ｔ)、ΔΨｇ(ｔ)は、加速度計とジャイロセンサのバイアス誤差により時間とともに増大する。すなわち、航法・姿勢方位計算部での航法計算、姿勢角および方位角計算には積分処理が行なわれるため、センサのバイアス誤差があると、速度、位置、姿勢角、方位角の誤差は時間経過とともに増大する。
【００１７】
これに対し、基準姿勢計算部１４の基準姿勢データの姿勢角は直接加速度計出力と重力加速度の比で算出されるため、式（４）の基準姿勢角誤差Δθｒ(ｔ)は時間経過とともには増大するものではない。また、式（４）の基準方位角誤差ΔΨｒ(ｔ，ｐ)も時間経過とともに増大するものではない。
【００１８】
そこで、比較すべき時間インターバルを運用精度に合わせて充分大きくとると、
のように、Δθｇ、ΔΨｇを充分大きく取り出すことが出来る。なお、ａ>>ｂは、ａはｂより非常に大きいことを意味する。
【００１９】
式（２）よりΔθｇ、ΔΨｇの内訳のバイアス誤差、運動に起因する誤差、温度に起因する誤差等を数式でモデル化すれば、カルマンフィルタや最小二乗法によりΔθｇ、ΔΨｇを推定することができる。また、速度誤差および位置誤差についてもΔθｇ、ΔΨｇと密接に関係しているので、速度誤差および位置誤差の数式モデルから同様に推定することが可能である。
【００２０】
【特許文献１】特許第３４９０７０６号
【発明の開示】
【発明が解決しようとする課題】
【００２１】
しかし、方位データの計算値の誤差推定においては外部基準方位センサ部１６の磁気方位センサが有する誤差が修正限界となり、方位データの誤差推定はこの磁気方位センサに大きく左右されてしまう。
【００２２】
基準姿勢計算部では、基準姿勢データを加速度計出力と重力加速度の比より計算しているため、飛行前（移動開始前）の初期静止状態では一般に問題なく基準姿勢データを求めることができるが、天候の悪い緊急運用時では風で振られる運動の影響を大きく受ける。
【００２３】
基準方位データを得るために外部基準方位センサ部を設けるため、機器全体の構成が複雑化する上にコスト高となる。また、地磁気は場所によって変動するため方位基準の安定性が悪く、方位データの誤差補正の精度が低い。
【００２４】
本発明は、このような従来の慣性航法装置が有していた問題を解決しようとするものであり、磁気方位センサの誤差や荒天時の風などの外乱に左右されることなく、より安定して姿勢／方位の初期データの精度を向上させることができる慣性航法装置およびその誤差補正方法を実現することを目的とする。
【課題を解決するための手段】
【００２５】
上記のような目的を達成するために、本発明の請求項１では、移動体の搭乗者のヘルメットに取り付けられたジャイロセンサと加速度計の出力を利用してヘルメット装着者の頭部の位置や姿勢を求める慣性航法装置において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算部と、
前記センサ誤差補正計算部により補正されたジャイロセンサおよび加速度計の出力を利用して前記頭部の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記頭部の位置や姿勢の計算結果を補正する航法・姿勢方位計算部と、
前記頭部が静止状態にあるときに前記航法・姿勢方位計算部より出力される速度データを速度誤差として利用し、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算部で使用するセンサ誤差データを校正するとともに、前記位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正する誤差推定計算部と、
を有することを特徴とする。
【００２６】
請求項２では、請求項１に記載の慣性航法装置において、前記誤差推定計算部は、カルマンフィルタを利用して誤差の推定値を計算することを特徴とする。
【００２７】
請求項３では、請求項１に記載の慣性航法装置において、前記誤差推定計算部は、最小二乗法を利用して誤差の推定値を計算することを特徴とする。
【００２８】
請求項４では、移動体の搭乗者のヘルメットに取り付けられたジャイロセンサと加速度計の出力を利用してヘルメット装着者の頭部の位置や姿勢を求める慣性航法装置の誤差補正方法において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算ステップと、
前記センサ誤差補正計算ステップにより補正されたジャイロセンサおよび加速度計の出力を利用して前記頭部の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記頭部の位置や姿勢の計算結果を補正する航法・姿勢方位計算ステップと、
前記頭部が静止状態にあるときに前記航法・姿勢方位計算部より出力される速度データを速度誤差として利用し、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算ステップで使用するセンサ誤差データを校正するとともに、前記位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正する誤差推定計算ステップと、
を有することを特徴とする。
【００２９】
請求項５では、請求項４に記載の慣性航法装置の誤差補正方法において、前記誤差推定計算ステップは、カルマンフィルタを利用して誤差の推定値を計算することを特徴とする。
【００３０】
請求項６では、請求項４に記載の慣性航法装置の誤差補正方法において、前記誤差推定計算ステップは、最小二乗法を利用して誤差の推定値を計算することを特徴とする。
【発明の効果】
【００３１】
慣性航法装置が電源ＯＮ時の静止した状態において、初期の姿勢および方位データを正確に決定しておけば、効率の良い運用に資することができる。上記のように、慣性航法装置が静止状態にあるときに航法・姿勢方位計算部から出力される速度データを速度誤差として利用して補正をかけることにより、磁気方位センサの誤差や荒天時の風などの外乱に左右されることなく、より安定して姿勢／方位の初期データの精度を向上させることができる慣性航法装置およびその誤差補正方法を実現することができる。
【００３２】
磁気方位センサを使わないことにより、方位データの誤差推定が磁気方位センサ固有の誤差や外部の地磁気の影響に左右されない。そのため、より安定しかつ高精度なジャイロセンサや加速度計の誤差補正を実現することができ、パイロットに対してもより安定かつ高精度な姿勢／方位データを提供することができる。また、磁気方位センサが不要となるため機器全体の構成が簡易化され、コストを下げることができる。
【００３３】
加速度計の出力から計算された基準姿勢データを使わないことにより、荒天時など外乱がある場合でも安定で高精度な姿勢データを得ることができる。
【発明を実施するための最良の形態】
【００３４】
以下、図面を用いて本発明の慣性航法装置およびその誤差補正方法を説明する。
【実施例１】
【００３５】
図１は本発明による慣性航法装置およびその誤差補正方法の一実施例を示す構成図である。図中、１はセンサ誤差補正計算部、２は航法・姿勢方位計算部、３は誤差推定計算部、４はＭＥＭＳを利用した加速度計およびジャイロセンサからなるＭＥＭＳセンサ部である。
【００３６】
本発明例のセンサ誤差補正計算部１およびＭＥＭＳセンサ部４は、図４に示した従来例と同じである。以下は航法・姿勢方位計算部２と誤差推定計算部３についてその構成と動作について説明する。
【００３７】
まず航法・姿勢方位計算部２について説明する。
【００３８】
ヘルメット装着者の速度および位置データは次式で計算される。
ここで、Ｖｎは航法座標系で表現された速度ベクトルであり、北方向成分ｖＮ、東方向成分ｖＥ、鉛直方向成分ｖＤに分解される。
また、Ωｅｎは航法座標系での地球自転速度の北軸周り、東軸周り、鉛直軸周り、各成分からなる３行３列の行列である。Ωｐｎは航法座標系での地球の周りを移動する移動速度の北軸周り、東軸周り、鉛直軸周り、各成分からなる３行３列の行列である。Ｇｎは重力加速度ベクトルの航法座標表現であり、Ａｎは移動加速度ベクトルの航法座標表現で加速度計出力である。Ｒλ、ＲΛは緯度方向と経度方向の地球半径である。ｈは飛行高度である。λ、Λは緯度と経度である。
【００３９】
Ｕｖｎ、ｕλ、ｕΛは誤差推定計算部３の誤差推定計算で得られた速度誤差、位置誤差等の推定値を用いて次式で求められるフィードバック制御量である。
Ｋｖｎ、Ｋλ、ＫΛは速度誤差、緯度誤差、経度誤差を最小にするフィードバックゲインである。
【００４０】
ヘルメット装着者の姿勢および方位角計算は次式で求められる。
ここで、Ｃｎｈは３行３列のヘルメット座標ｈから航法座標ｎへの座標変換行列である。
Ψ、θ、φは頭部の方位角、ピッチ角、ロール角である。
Ｋｗｈは姿勢および方位データの誤差を最小にするフィードバックゲインである。
【００４１】
次に、誤差推定計算部３について説明する。
航法・姿勢方位計算部２から出力される速度データから、どのようにジャイロセンサ誤差や加速度計誤差、姿勢角誤差、速度誤差等を推定するかについて、以下のように簡易モデルで説明する。実際はもっと複雑であるが考え方は同じである。
以下で（ＸＮ、ＹＥ）は航法座標で北軸と東軸を、（Ｘ，Ｙ）はヘルメット装着の頭部座標のロール軸とピッチ軸を示す。加速度計とジャイロセンサはストラップダウン方式で直接頭部基準軸のＸとＹに取り付けられる。Ψは方位角である。
図２に航法座標と頭部座標の座標系を示す。
【００４２】
航法・姿勢方位計算部２の航法計算出力の北速度データ出力は次式の積分で表されるものとする。
ＡＮは北方向の加速度であり、初期状態の静止時のため０である。
δａＮは加速度誤差である。結局、初期は静止時ゆえ計算された速度出力は速度誤差そのものであり、式（９）は、
のように表現できる。ここが、本発明のポイントである。
【００４３】
ここで、北方向加速度誤差δａＮは、
で表される。
【００４４】
ここで、Δａｘ、ΔａｙはＸ加速度計とＹ加速度計のバイアス誤差で、δθＥは次式で表される東軸周りの角度誤差である。Ψ０，ｇは初期方位角と重力加速度である。
ここで、Δωｘ，ΔωｙはＸジャイロセンサとＹジャイロセンサのバイアス誤差である。
【００４５】
誤差推定計算部３の目的は、式（１０）の積分の速度データ入力より、どのようにして加速度計のバイアス誤差、ジャイロセンサのバイアス誤差、角度誤差、速度誤差等を推定するかである。以下に最小二乗法を用いて上記誤差を求める場合について説明する。なお、 カルマンフィルタを用いても同様に誤差を求めることが可能である。
【００４６】
初期方位角Ψ０は外乱が無い限り常数である。したがって、式（１２）の時間積分は、
となる。
【００４７】
式（１０）の積分は次式の出力データで与えられる。
【００４８】
式（１４）式を行列表現すると、
と書ける。ここでｈ(ｔ)は１行６列の観測行列で時間ｔの関数、Θが推定すべき未知パラメータで６行１列の行列である。
【００４９】
初期方位角Ψ０と初期緯度λ０は、次式で簡易的に最初に推定しておく。
ここで、ωｘ，ωｙはＸジャイロセンサとＹジャイロセンサの出力で、
である。Ωは地球自転速度で１５°／Ｈｒである。
【００５０】
λ０は初期緯度で、式（１７）より
で与えられる。±の決定は北半球にいるか南半球にいるかで決まる。
【００５１】
式（１５）は未知パラメータが６個であるため、これらすべての未知パラメータを求めるには６本以上の式が必要となる。そこで、時間t１,t２,t３,…,ｔｎ毎の出力から次式を準備する。
【００５２】
式（１９）に最小二乗法を適用すると、未知パラメータΘの推定は
で求まる。
【００５３】
角度誤差と速度誤差の推定は 式（１３）と式（１４）より
で求まる。
【００５４】
以上のようにして求められた加速度計バイアス誤差とジャイロセンサバイアス誤差の推定値はセンサ誤差補正計算部１に出力される。
また、角度誤差と速度誤差の推定値は航法・姿勢方位計算部２へ出力され、式（７）、式（８）の速度誤差と姿勢/方位角誤差フィードバック制御に用いられる。
【００５５】
ここでは、簡単に角度誤差と速度誤差の推定のみについて説明したが、実際は同じ考え方に基づき速度誤差、位置誤差、姿勢/方位誤差等の推定が実行される。なお、実際の本発明の推定はノイズの影響を最小にするためにカルマンフィルタが適用されるが、その場合でも誤差の数式表現が異なるのみである。
【００５６】
図３は、航法・姿勢方位計算部２で計算された方位データの誤差について従来例と本発明を比較した図である。図３（ａ）は従来例、（ｂ）は本発明による結果を示している。横軸は時間、縦軸は方位誤差の大きさである。（ａ）の従来例では、磁気方位センサを使用しているため、磁気方位センサの誤差により修正限界が決まり、ある一定のレベルよりも誤差を小さくすることができない。また、地磁気の変化などにより誤差の大きさも安定しない。一方（ｂ）の本発明では、静止時は基準速度がゼロであるということを利用して、方位誤差を限りなくゼロに近づけることができる。
【００５７】
なお、本発明ではセンサを直接運動体に取り付けるが、機械サーボ制御で実現される水平面（プラットホーム方式）に取り付けても良い。また、静止時に風等外乱が無視できない場合は、外乱除去用のフィルタを設けることにより適用可能である。この場合は本発明の速度データに外乱速度が重畳しているので、誤差推定計算部３の前段に外乱除去フィルタを設け、外乱除去後の速度データを誤差推定計算に用いる。
【００５８】
また、本発明では、ジャイロセンサや加速度計の誤差を推定値で修正し、速度／位置データなどの航法誤差や姿勢／方位角データの誤差を推定値によるフィードバック制御で修正する。このフィードバック制御は最適制御を主とするが、ＰＩＤ（Proportional Integral Differential）制御でも良い。
【図面の簡単な説明】
【００５９】
【図１】図１は本発明による慣性航法装置およびその誤差補正方法の一実施例を示す構成図。
【図２】図２は航法座標と頭部座標の座標系を示す図。
【図３】図３は方位データの誤差について従来例と本発明を比較した図。
【図４】図４は従来の慣性航法装置およびその誤差補正方法の一例を示す構成図。
【図５】図５は航空機の搭乗者が頭部に装着したヘッドトラッカの概念図。
【符号の説明】
【００６０】
１，１１ センサ誤差補正計算部
２，１２ 航法・姿勢方位計算部
３，１３ 誤差推定計算部
４，１５ ＭＥＭＳセンサ部
１４ 基準姿勢計算部
１６ 外部基準方位センサ部
【特許請求の範囲】
【請求項１】
移動体の搭乗者のヘルメットに取り付けられたジャイロセンサと加速度計の出力を利用してヘルメット装着者の頭部の位置や姿勢を求める慣性航法装置において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算部と、
前記センサ誤差補正計算部により補正されたジャイロセンサおよび加速度計の出力を利用して前記頭部の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記頭部の位置や姿勢の計算結果を補正する航法・姿勢方位計算部と、
前記頭部が静止状態にあるときに前記航法・姿勢方位計算部より出力される速度データを速度誤差として利用し、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算部で使用するセンサ誤差データを校正するとともに、前記位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正する誤差推定計算部と、
を有することを特徴とする慣性航法装置。
【請求項２】
前記誤差推定計算部は、カルマンフィルタを利用して誤差の推定値を計算することを特徴とする請求項１に記載の慣性航法装置。
【請求項３】
前記誤差推定計算部は、最小二乗法を利用して誤差の推定値を計算することを特徴とする請求項１に記載の慣性航法装置。
【請求項４】
移動体の搭乗者のヘルメットに取り付けられたジャイロセンサと加速度計の出力を利用してヘルメット装着者の頭部の位置や姿勢を求める慣性航法装置の誤差補正方法において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算ステップと、
前記センサ誤差補正計算ステップにより補正されたジャイロセンサおよび加速度計の出力を利用して前記頭部の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記頭部の位置や姿勢の計算結果を補正する航法・姿勢方位計算ステップと、
前記頭部が静止状態にあるときに前記航法・姿勢方位計算部より出力される速度データを速度誤差として利用し、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算ステップで使用するセンサ誤差データを校正するとともに、前記位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正する誤差推定計算ステップと、
を有することを特徴とする慣性航法装置の誤差補正方法。
【請求項５】
前記誤差推定計算ステップは、カルマンフィルタを利用して誤差の推定値を計算することを特徴とする請求項４に記載の慣性航法装置の誤差補正方法。
【請求項６】
前記誤差推定計算ステップは、最小二乗法を利用して誤差の推定値を計算することを特徴とする請求項４に記載の慣性航法装置の誤差補正方法。
【請求項１】
移動体の搭乗者のヘルメットに取り付けられたジャイロセンサと加速度計の出力を利用してヘルメット装着者の頭部の位置や姿勢を求める慣性航法装置において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算部と、
前記センサ誤差補正計算部により補正されたジャイロセンサおよび加速度計の出力を利用して前記頭部の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記頭部の位置や姿勢の計算結果を補正する航法・姿勢方位計算部と、
前記頭部が静止状態にあるときに前記航法・姿勢方位計算部より出力される速度データを速度誤差として利用し、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算部で使用するセンサ誤差データを校正するとともに、前記位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正する誤差推定計算部と、
を有することを特徴とする慣性航法装置。
【請求項２】
前記誤差推定計算部は、カルマンフィルタを利用して誤差の推定値を計算することを特徴とする請求項１に記載の慣性航法装置。
【請求項３】
前記誤差推定計算部は、最小二乗法を利用して誤差の推定値を計算することを特徴とする請求項１に記載の慣性航法装置。
【請求項４】
移動体の搭乗者のヘルメットに取り付けられたジャイロセンサと加速度計の出力を利用してヘルメット装着者の頭部の位置や姿勢を求める慣性航法装置の誤差補正方法において、
予め推定されたセンサ誤差データを利用して、ジャイロセンサおよび加速度計の出力を補正するセンサ誤差補正計算ステップと、
前記センサ誤差補正計算ステップにより補正されたジャイロセンサおよび加速度計の出力を利用して前記頭部の位置や姿勢を計算するとともに、予め推定された姿勢／方位誤差データを利用して前記頭部の位置や姿勢の計算結果を補正する航法・姿勢方位計算ステップと、
前記頭部が静止状態にあるときに前記航法・姿勢方位計算部より出力される速度データを速度誤差として利用し、前記ジャイロセンサと加速度計の出力の誤差を推定し前記センサ誤差補正計算ステップで使用するセンサ誤差データを校正するとともに、前記位置や姿勢の出力の誤差を推定し前記航法・姿勢方位計算部で使用する姿勢／位置誤差データを校正する誤差推定計算ステップと、
を有することを特徴とする慣性航法装置の誤差補正方法。
【請求項５】
前記誤差推定計算ステップは、カルマンフィルタを利用して誤差の推定値を計算することを特徴とする請求項４に記載の慣性航法装置の誤差補正方法。
【請求項６】
前記誤差推定計算ステップは、最小二乗法を利用して誤差の推定値を計算することを特徴とする請求項４に記載の慣性航法装置の誤差補正方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２００７−２３２４４３（Ｐ２００７−２３２４４３Ａ）
【公開日】平成１９年９月１３日（２００７．９．１３）
【国際特許分類】
【出願番号】特願２００６−５１９５８（Ｐ２００６−５１９５８）
【出願日】平成１８年２月２８日（２００６．２．２８）
【出願人】（０００００６５０７）横河電機株式会社 (4,443)
【出願人】（０００２３２３５７）横河電子機器株式会社 (109)
【Ｆターム（参考）】
【公開日】平成１９年９月１３日（２００７．９．１３）
【国際特許分類】
【出願日】平成１８年２月２８日（２００６．２．２８）
【出願人】（０００００６５０７）横河電機株式会社 (4,443)
【出願人】（０００２３２３５７）横河電子機器株式会社 (109)
【Ｆターム（参考）】
[ Back to top ]