JP2004239643A - ハイブリッド航法装置 - Google Patents
ハイブリッド航法装置 Download PDFInfo
- Publication number
- JP2004239643A JP2004239643A JP2003026363A JP2003026363A JP2004239643A JP 2004239643 A JP2004239643 A JP 2004239643A JP 2003026363 A JP2003026363 A JP 2003026363A JP 2003026363 A JP2003026363 A JP 2003026363A JP 2004239643 A JP2004239643 A JP 2004239643A
- Authority
- JP
- Japan
- Prior art keywords
- navigation device
- error
- kalman filter
- azimuth
- angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
【課題】姿勢角および方位角についても正確に誤差推定することができるハイブリッド航法装置を構成する。
【解決手段】観測値演算部４は、ＧＰＳ受信機２から入力された速度および加速度に基づいて、姿勢角および方位角を算出する。このように求められた姿勢角および方位角と、ＩＮＳ１で測定された姿勢角および方位角とに基づいて、チルトと方位角に関する観測値を算出して、他の位置や速度の観測値とともにカルマンフィルタ３に入力する。カルマンフィルタ３はこれらの観測値に基づいて、位置や速度とともに、姿勢角や方位角の推定誤差を算出する。得られた推定誤差は加算器５に入力され、加算器５は、これら姿勢角および方位角の推定誤差を用いて、ＩＮＳ１から直接入力された姿勢角および方位角を補正し出力する。
【選択図】 図１
【解決手段】観測値演算部４は、ＧＰＳ受信機２から入力された速度および加速度に基づいて、姿勢角および方位角を算出する。このように求められた姿勢角および方位角と、ＩＮＳ１で測定された姿勢角および方位角とに基づいて、チルトと方位角に関する観測値を算出して、他の位置や速度の観測値とともにカルマンフィルタ３に入力する。カルマンフィルタ３はこれらの観測値に基づいて、位置や速度とともに、姿勢角や方位角の推定誤差を算出する。得られた推定誤差は加算器５に入力され、加算器５は、これら姿勢角および方位角の推定誤差を用いて、ＩＮＳ１から直接入力された姿勢角および方位角を補正し出力する。
【選択図】 図１
Description
【０００１】
【発明の属する技術分野】
この発明は、ＧＰＳ受信機等の衛星航法装置を用いた電波航法と慣性航法装置（ＩＮＳ）を用いた慣性航法のそれぞれより得られる測定値に基づいて、機体の位置、速度、姿勢、方位等を得るハイブリッド航法を用いたハイブリッド航法装置に関するものである。
【０００２】
【従来の技術】
従来、航空機や船舶等の航法装置として、複数種類の航法手段を備え、これらを組み合わせることで機体や船体の位置や速度の測定精度を高めるハイブリッド航法装置が各種考案されている。このようなハイブリッド航法装置には、測定された時系列データから現在の位置や速度などの状態を推定するカルマンフィルタが用いられている航法装置がある。
【０００３】
このような従来のハイブリッド航法装置について、図を参照して説明する。
なお、衛星航法装置として、ＧＰＳ受信機を例に説明する。
図３は、ＧＰＳ受信機とＩＮＳ（慣性航法装置）とを組み合わせた従来のハイブリッド航法装置の主要部を示すブロック図である。
図３に示すように、ＧＰＳ受信機２で測定された位置情報および速度情報と、ＩＮＳ１で測定された位置情報および速度情報とをカルマンフィルタ３に入力し、ＩＮＳ１から得られる、位置、速度、方位角および姿勢角等の誤差を最適推定する。そして、ハイブリッド航法装置１０は、最適推定された位置、速度、方位角、および姿勢角等の誤差に基づいてＩＮＳ１より入力される位置、速度、方位角、および姿勢角を補正して出力する。（例えば、特許文献１参照。）。
【０００４】
【特許文献１】
特開２００１−１７４２７５号公報
【０００５】
【発明が解決しようとする課題】
上述のようなハイブリッド航法装置に備えられたカルマンフィルタにおいては、ＩＮＳから入力される測定値とＧＰＳ受信機等の衛星航法装置から入力される測定値との差に基づいて、誤差推定値を算出する。
【０００６】
しかしながら、従来のハイブリッド航法装置では、衛星航法装置から入力される測定値が位置および速度のみであるため、ＩＮＳから入力される測定値が位置、速度、姿勢角、および方位角であっても、カルマンフィルタに入力可能な測定値の差としては、位置と速度のみであった。このため、位置と速度との誤差推定は高精度に行うことができても、姿勢角や方位角については高精度に誤差推定することができなかった。つまり、ハイブリッド航法装置は、ＩＮＳから出力される姿勢角および方位角を高精度に補正し、正確な姿勢角および方位角を提供することができなかった。
【０００７】
この発明の目的は、衛星航法装置とＩＮＳとから入力される測定値に基づいて、姿勢角および方位角を高精度に推定することができるハイブリッド航法装置を提供することにある。
【０００８】
【課題を解決するための手段】
この発明は、衛星航法装置により得られる測定値と、慣性航法装置により得られる測定値とから、カルマンフィルタにて慣性航法装置の測定誤差を推定して、補正するハイブリッド航法装置において、衛星航法装置により得られる位置、速度、および加速度と慣性航法装置により得られる位置、速度、姿勢角、方位角、風速および風向とを入力し、衛星航法装置により得られる速度および加速度に基づく姿勢角および方位角を算出するとともに、衛星航法装置により得れらる測定値と慣性航法装置より得られる測定値との差を算出し、観測値として出力する観測値演算手段と、観測値演算手段で出力された観測値に基づいて、前記慣性航法装置により得られる位置、速度、姿勢角、および方位角の推定誤差を出力するカルマンフィルタと、カルマンフィルタにより出力された推定誤差に基づいて、慣性航法装置から出力された位置、速度、姿勢角、および方位角を補正して出力する補正手段と、を備えたことを特徴としている。
【０００９】
この構成では、観測値演算手段で、衛星航法装置から入力された速度および加速度に基づいて姿勢角および方位角を算出する。この際、慣性航法装置から入力される風速および風向を用いることで算出する。このように衛星航法装置の測定値に基づく姿勢角および方位角と、慣性航法装置により測定された姿勢角および方位角とを用いて、姿勢角および方位角の差分値（観測値）を算出し、これらを他の観測値である位置の差分値、速度の差分値、加速度の差分値とともにカルマンフィルタに出力する。カルマンフィルタは、入力されたこれらの観測値に基づいて誤差推定を行い、補正手段である加算器に出力する。加算器は、ＩＮＳから入力された、位置、速度、加速度、姿勢角、および方位角の各測定値と、カルマンフィルタにより推定された誤差とを加算することで、ＩＮＳからの各測定値の補正を行って出力する。
【００１０】
また、この発明は、少なくとも二つの測位用アンテナを備えた衛星航法装置により得られる測定値と、慣性航法装置により得られる測定値とから、カルマンフィルタにて慣性航法装置の測定誤差を推定して、補正するハイブリッド航法装置において、少なくとも衛星航法装置により得られる姿勢角、および方位角と慣性航法装置により得られる姿勢角、および方位角とを入力し、衛星航法装置により得れらる測定値と慣性航法装置より得られる測定値との差を算出し、観測値として出力する観測値演算手段と、観測値演算手段で出力された観測値に基づいて、前記慣性航法装置により得られる姿勢角、および方位角の推定誤差を出力するカルマンフィルタと、カルマンフィルタにより出力された推定誤差に基づいて、慣性航法装置から出力された姿勢角、および方位角を補正して出力する補正手段と、を備えたことを特徴としている。
【００１１】
この構成では、観測値演算手段で、衛星航法装置により測定算出された姿勢角および方位角と、慣性航法装置により測定算出された姿勢角および方位角とを用いて、姿勢角および方位角の差分値（観測値）を算出し、カルマンフィルタに出力する。カルマンフィルタは、入力されたこれらの観測値に基づいて誤差推定を行い、補正手段である加算器に出力する。加算器は、ＩＮＳから入力された姿勢角、および方位角の各測定値と、カルマンフィルタにより推定された誤差とを加算することで、ＩＮＳからの各測定値の補正を行って出力する。
【００１２】
また、この発明は、カルマンフィルタにより算出された推定誤差を慣性航法装置にフィードバックすることを特徴としている。
【００１３】
この構成では、カルマンフィルタで推定された誤差をＩＮＳにフィードバックすることで、ＩＮＳは、フィードバックされた推定誤差に基づいて、プラットホームのアライメントを調整する。そして、観測値演算手段は、前述のような観測値の算出を行い、カルマンフィルタに出力する。カルマンフィルタはこの観測値に基づき、誤差を推定し、加算器に出力するとともに、再度ＩＮＳにも出力する。
【００１４】
また、この発明は、カルマンフィルタで、位置の推定誤差の生成過程にランジュバン方程式を用いたことを特徴としている。
【００１５】
この構成では、ＩＮＳにおける位置誤差が加速度計の誤差に殆ど影響されないことに基づいて、カルマンフィルタにおける演算で加速度に関する誤差を省略する。これにより、カルマンフィルタにおいては、加速度の誤差推定が削減され、位置、速度、姿勢角、および方位角の誤差推定のみを行うこととなる。
【００１６】
【発明の実施の形態】
本発明の実施形態に係るハイブリッド航法装置について図を参照して説明する。なお、本実施形態では、衛星航法装置としてＧＰＳ受信機を例に説明する。
図１は、本発明の実施形態に係るハイブリッド航法装置の主要部を示すブロック図である。
図１において、１はジャイロと加速度計とを備え、位置、速度、加速度、姿勢角、方位角、風速、および風向を計測して出力する、本発明の慣性航法装置に相当するＩＮＳであり、２は受信したＧＰＳ信号に基づいて位置、速度、加速度を出力するＧＰＳ受信機である。
【００１７】
また、ハイブリッド航法装置１０は、ＩＮＳ１およびＧＰＳ受信機２から入力された位置等の各測定値に基づいて、差分値である観測値を演算する観測値演算部４と、入力された観測値に基づいて、ＩＮＳ１で計測された各測定値に誤差を推定するカルマンフィルタ３と、ＩＮＳ１から出力された各測定値とカルマンフィルタ３から出力された各測定値の推定誤差とを入力して、該推定誤差に基づいて前記各測定値を補正して出力する加算器５とを備える。
【００１８】
これらＩＮＳ１、ＧＰＳ受信機２、およびハイブリッド航法装置１０が、航空機等の機体に搭載されている。
【００１９】
（１）システム方程式
本発明に係るハイブリッド航法装置で用いるシステム方程式は、次のように表すことができる。
【００２０】
【数１】
【００２１】
数１において、ｘはＩＮＳ１における誤差ベクトル、Ａはシステム行列、ｕは駆動雑音ベクトルであり、各測定値は、それぞれに直交する北方向（Ｎ方向）、東方向（Ｅ方向）、垂直下方向（Ｄ方向）からなるＮＥＤ座標系を用いる。
また、数１において、ａＮ ，ａＥ ，ａＤ はそれぞれＩＮＳで計測されたＮ軸方向、Ｅ軸方向、Ｄ軸方向の加速度、ｖＮ ，ｖＥ ，ｖＤ はそれぞれＩＮＳで計測されたＮ軸方向、Ｅ軸方向、Ｄ軸方向の速度、ΩＥ は地球の角速度、Ｒは地球の平均半径、λは機体の緯度、τｇ ，τａ は相関時定数である。
【００２２】
ここで、誤差ベクトルｘの各要素は、
【００２３】
【数２】
【００２４】
であり、駆動雑音ベクトルの各要素は、
【００２５】
【数３】
【００２６】
である。
【００２７】
なお、駆動雑音ベクトルｕおよび相関時定数τｇ ，τａ は設計パラメータであり、システムの設計により定まる。
【００２８】
（２）観測方程式
本発明に係るハイブリッド航法装置で用いる観測方程式は、次のように表すことができる。
【００２９】
【数４】
【００３０】
（４）式において、ｙはＩＮＳ１とＧＰＳ受信機２でそれぞれ測定した位置の差、姿勢角の差から計算したチルトの差、および方位角の差を観測値とする観測ベクトル、Ｃは観測行列、ｗは観測雑音ベクトルである。
【００３１】
ここで、観測ベクトルｙの各要素は、
【００３２】
【数５】
【００３３】
であり、観測雑音ベクトルｗの各要素は、
【００３４】
【数６】
【００３５】
である。
なお、観測雑音ベクトルｗは設計パラメータであり、設計により定まる。
【００３６】
（３）観測値演算部
観測演算部４では、ＧＰＳ受信機２から入力された速度ベクトルｖｇ と加速度ベクトルａｇ とに基づいて、機体軸座標系の姿勢角（ロール角φｇ 、ピッチ角θｇ ）と方位角ψｇ とを、次に示す方法で算出する。
【００３７】
前記ＧＰＳ受信機２により測定された地球軸座標系（ＮＥＤ座標系）の速度ベクトルｖｇ および加速度ベクトルａｇ は（７）式で表される。
【００３８】
【数７】
【００３９】
一方、ＩＮＳ１で測定された風速をｖｓ 、風向をｖｄ とすると、風速ベクトルｖｗ は（８）式で表される。
【００４０】
【数８】
【００４１】
安定軸座標系の速度をベクトルｖｓ とすると、
【００４２】
【数９】
【００４３】
と表され、ＧＰＳ受信機２により計測された地球軸座標系の速度ベクトルｖｇ と風速ベクトルｖｗ との差で表すことができるので、（１０）式のように表される。
【００４４】
【数１０】
【００４５】
安定軸座標系の加速度ベクトルａｓ は、
【００４６】
【数１１】
【００４７】
と表され、速度ベクトルｖｓ の時間微分であり、風速がほぼ定常であるとすると、加速度ベクトルａｓ は、（１２）式で表される。
【００４８】
【数１２】
【００４９】
次に、安定軸座標系の姿勢角であるピッチ角θｓ と方位角ψｓ は（１０）式を用いて、（１３）式および（１４）式で表すことができる。
【００５０】
【数１３】
【００５１】
【数１４】
【００５２】
一方、ロール角φｓ は（１２）式を用いて、次に示す方法で求められる。
加速度ベクトルａｓ の速度ベクトルｖｓ に対する直交成分をａｓ ｎ とすると、
【００５３】
【数１５】
【００５４】
となり、同様に、重力加速度ベクトルｇの速度ベクトルｖｓ に対する直交成分をｇｎ とすると、
【００５５】
【数１６】
【００５６】
となる。ここで、揚力加速度ベクトルをＩとし、水平基準ベクトルをｐすると、揚力加速度ベクトルＩと水平基準ベクトルｐのそれぞれは、次のように表される。
【００５７】
【数１７】
【００５８】
【数１８】
【００５９】
これら揚力加速度ベクトルＩおよび水平基準ベクトルｐを用いると、ロール角φｓ は次の式により求められる。
【００６０】
【数１９】
【００６１】
次に、安定軸座標系で求めた姿勢角θｓ ，φｓ および方位角ψｓ から、ＩＮＳで測定された姿勢角θ，φおよび方位角ψと同じ座標系にするため、機体軸座標系の姿勢角および方位角に変換する。
機体軸座標系のオイラー角を（ψｇ ，θｇ ，φｇ ）とすると、地球軸座標系から機体軸座標系への変換行列Ｒ（ψｇ ，θｇ ，φｇ ）は、次式で表すことができる。
【００６２】
【数２０】
【００６３】
地球軸座標系から安定軸座標系への変換行列はＲ（ψｓ ，θｓ ，φｓ ）であり、迎角計からの迎角をαとすると安定軸座標系から機体軸座標系への変換行列はＲ（０，α，０）であるので、変換行列Ｒ（ψｇ ，θｇ ，φｇ ），Ｒ（ψｓ ，θｓ ，φｓ ），Ｒ（０，α，０）には次式の関係が成り立つ。
【００６４】
【数２１】
【００６５】
したがって、機体軸座標系のロール角θｇ 、ピッチ角φｇ 、方位角ψｇ はそれぞれ次のように表される。
【００６６】
【数２２】
【００６７】
【数２３】
【００６８】
【数２４】
【００６９】
このようにＧＰＳ受信機１の測定値により求められた姿勢角θｇ ，φｇ および方位角ψｇ と、ＩＮＳにより測定された姿勢角θ，φおよび方位角ψとの差に基づいて、Ｎ軸回りのチルトｙ３ とＥ軸回りのチルトｙ４ とＤ軸回りの方位角ｙ５ とが次式により求められる。
【００７０】
【数２５】
【００７１】
また、観測値演算部４は、ＩＮＳ１により測定された位置とＧＰＳ受信機２により測定された位置とに基づき、Ｎ軸方向の位置の差ｙ１ とＥ軸方向の位置の差ｙ２ とを求める。このようにして、観測方程式の観測値ベクトルｙを求める。
【００７２】
上述のＧＰＳ受信機２で測定した速度および加速度から機体の姿勢角（ピッチ角、ロール角）および方位角を求める方法は、「ＲＩＣＨＡＲＤ Ｐ．ＫＯＲＮＦＩＥＬＤ， Ｒ．ＪＯＨＮ ＨＡＨＳＭＡＮ ａｎｄ ＪＯＨＮ Ｊ．ＤＥＹＳＴ：Ｓｉｎｇｌｅ−Ａｎｔｅｎｎａ ＧＰＳ−Ｂａｓｅｄ Ａｉｒｃｒａｆｔ Ａｔｔｉｔｕｄｅ Ｄｅｔｅｒｍｉｎｅｔｉｏｎ， Ｎａｖｉｇａｔｉｏｎ Ｊｏｕｒｎａｌ ｏｆ Ｔｈｅ Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｎａｖｉｇａｔｉｏｎ， Ｖｏｌ．４５， Ｎｏ．１， Ｓｐｒｉｎｇ，１９９８ 」に示されている。
【００７３】
（４）カルマンフィルタ
上述のシステム方程式と観測方程式とを離散時間系に変換すると、次式のように表される。
【００７４】
【数２６】
【００７５】
【数２７】
【００７６】
この離散時間系システム方程式と観測方程式とを用いて周知方法で次に示すカルマンフィルタの各式を導く。
【００７７】
伝播式
【００７８】
【数２８】
【００７９】
このように、推定値を意味する＾（ハット）付き記号を以下２文字で表す。
【００８０】
【数２９】
【００８１】
更新式
【００８２】
【数３０】
【００８３】
【数３１】
【００８４】
【数３２】
【００８５】
カルマンフィルタ３は、（２８）式〜（３０）式に示したカルマンフィルタの式を用いて、更新周期Ｔｓ毎に観測値演算部４から入力される各観測値に基づいて、誤差推定を行い、加算器５に出力する。ここで、カルマンフィルタ３は、上述のカルマンフィルタの式により求められたＮ軸回りのチルト誤差ｘ５ とＥ軸回りのチルト誤差ｘ６ とに基づいて、次の変換式を用いて姿勢角誤差を算出した後に、加算器５に出力する。
【００８６】
【数３３】
【００８７】
（５）加算器
加算器５はＩＮＳ１から直接入力された位置、速度、姿勢角、方位角に、カルマンフィルタ３から入力された位置誤差、速度誤差、姿勢角誤差、方位角誤差をそれぞれ加算する。すなわち、ＩＮＳ１から入力された位置、速度、姿勢角、方位角を、それぞれ位置誤差、速度誤差、姿勢角誤差、方位角誤差で補正し、表示器（図示せず）等に出力する。
【００８８】
このような構成とすることで、カルマンフィルタで姿勢角および方位角の誤差推定を正確に行うことができるので、従来のように位置と速度のみを高精度に得られるだけでなく、姿勢角や方位角についても高精度に得ることができる。
【００８９】
なお、本実施形態では、カルマンフィルタ３で推定した誤差に基づいて、加算器５で、ＩＮＳ１から出力された各測定値を補正する方法を示したが、図２に示すように、カルマンフィルタ３で推定した誤差をＩＮＳ１にフィードバックし、プラットホームのアライメントを調整する。このような構成とすることで、特にジャイロバイアス誤差が１ｄｅｇ／ｈ程度のような、低精度のＩＮＳを用いても誤差が発散することなく、高精度に誤差推定を行うことができる。これにより、低精度であるが安価なＩＮＳを使用できるので、装置全体のコストダウンを行うことができる。
【００９０】
また、本実施形態に示したカルマンフィルタにランジュバン方程式を適用することができる。これは、ＩＮＳの位置誤差はジャイロの誤差と加速度計の誤差が要因となっているが、その殆どがジャイロの誤差であるため、加速度計の誤差を省略することができるからである。これを利用し、ランジュバン方程式で位置誤差の生成過程をモデル化する。
このようにランジュバン方程式で位置誤差の生成過程をモデル化することで、システム方程式の誤差ベクトルから加速度誤差を省略することができる。これにより、この誤差ベクトルをｘ”とすると、システム行列Ａ”、駆動雑音ベクトルｕ”は次式となる。
【００９１】
【数３４】
【００９２】
【数３５】
【００９３】
この時、システム方程式は、
【００９４】
【数３６】
【００９５】
となる。
【００９６】
このシステム方程式と上述の観測方程式（（２）式）とを用いることで、位置誤差、姿勢角誤差、方位角誤差をカルマンフィルタで推定することができる。
【００９７】
このような構成とすることで、ＩＮＳのジャイロの誤差における加速度要因を省略することができるので、位置誤差および速度誤差とジャイロのバイアス誤差とを分離することができる。このため、カルマンフィルタのチューニングは容易になり、位置誤差、姿勢角誤差、方位角誤差等のカルマンフィルタによる誤差推定を容易にすることができる。
【００９８】
なお、上述の実施形態では、ＧＰＳ受信機で位置、速度、および加速度を測定して、ＩＮＳの位置、速度、姿勢角、および方位角の誤差推定に利用する方法を示したが、ＧＰＳ受信機で姿勢角、および方位角を直接算出して、ＩＮＳの姿勢角、および方位角の誤差推定に利用してもよい。
【００９９】
この場合、ＧＰＳ受信機には少なくとも二つのＧＰＳアンテナが備えられており、これらＧＰＳアンテナ間の相対測位を行い、この結果から姿勢角および方位角を算出する。そして、ＧＰＳ受信機で算出された姿勢角および方位角と、ＩＮＳで測定された姿勢角および方位角との差分を求め、カルマンフィルタで誤差推定を行う。
【０１００】
このような構成とすることで、ＧＰＳ受信機およびＩＮＳでそれぞれ直接求められた姿勢角および方位角を用いて、これらの誤差推定を行うことができる。
【０１０１】
また、ＧＰＳ受信機で測定、算出された位置、速度、加速度、姿勢角、および方位角のうちの必要な測定値と、ＩＮＳで測定した位置、速度、加速度、姿勢角、方位角、風向、および風速のうちの必要な測定値とを用いて、これらを組み合わせ、誤差推定を行ってもよい。
【０１０２】
また、上述の実施形態では、プラットホーム方式のＩＮＳについて説明したが、ストラップダウン方式のＩＮＳやＩＲＵ（慣性基準ユニット）についても上述の構成を適用することができる。
【０１０３】
【発明の効果】
この発明によれば、カルマンフィルタで位置誤差、速度誤差とともに、姿勢角誤差および方位角誤差をそれぞれの観測値に基づいて推定するため、高精度に誤差推定を行うことができる。
【０１０４】
また、衛星航法装置および慣性航法装置のそれぞれで少なくとも姿勢角および方位角を直接、測定算出し、これらを用いて各測定値の差分を算出してカルマンフィルタで誤差推定することで、高精度に誤差推定することができる。
【０１０５】
このため、これらの推定誤差を用いてＩＮＳから出力される位置、姿勢角、方位角を補正することで、機体の正確な位置、姿勢、方位を得ることができる。
【０１０６】
また、この発明によれば、カルマンフィルタで推定した位置誤差、速度誤差、姿勢角誤差、および方位角誤差をＩＮＳにフィードバックすることにより、ＧＰＳ受信機から得られる位置、速度の測定値や速度及び加速度から得られる姿勢角、方位角と、ＩＮＳで予め補正された位置、速度、加速度、姿勢角、方位角との差が徐々に小さくなるため、観測演算部からカルマンフィルタに出力される観測値を徐々に小さくすることができる。このように徐々に小さくなる観測値をカルマンフィルタに入力していくことで、誤差が収束しやすくなり、さらに高精度に誤差推定を行うことができる。
【０１０７】
また、この発明によれば、カルマンフィルタにランジュバン方程式を適用することで、加速度誤差を省略することができ、位置誤差および速度誤差とジャイロのバイアス誤差とを分離することができるので、誤差推定を容易にすることができる。
【図面の簡単な説明】
【図１】本発明の実施形態に係るハイブリッド航法装置の主要部を示すブロック図
【図２】他の構成のハイブリッド航法装置の主要部を示すブロック図
【図３】従来のハイブリッド航法装置の主要部を示すブロック図
【符号の説明】
１−ＩＮＳ
２−ＧＰＳ受信機
３−カルマンフィルタ
４−観測値演算部
５−加算器
１０−ハイブリッド航法装置
【発明の属する技術分野】
この発明は、ＧＰＳ受信機等の衛星航法装置を用いた電波航法と慣性航法装置（ＩＮＳ）を用いた慣性航法のそれぞれより得られる測定値に基づいて、機体の位置、速度、姿勢、方位等を得るハイブリッド航法を用いたハイブリッド航法装置に関するものである。
【０００２】
【従来の技術】
従来、航空機や船舶等の航法装置として、複数種類の航法手段を備え、これらを組み合わせることで機体や船体の位置や速度の測定精度を高めるハイブリッド航法装置が各種考案されている。このようなハイブリッド航法装置には、測定された時系列データから現在の位置や速度などの状態を推定するカルマンフィルタが用いられている航法装置がある。
【０００３】
このような従来のハイブリッド航法装置について、図を参照して説明する。
なお、衛星航法装置として、ＧＰＳ受信機を例に説明する。
図３は、ＧＰＳ受信機とＩＮＳ（慣性航法装置）とを組み合わせた従来のハイブリッド航法装置の主要部を示すブロック図である。
図３に示すように、ＧＰＳ受信機２で測定された位置情報および速度情報と、ＩＮＳ１で測定された位置情報および速度情報とをカルマンフィルタ３に入力し、ＩＮＳ１から得られる、位置、速度、方位角および姿勢角等の誤差を最適推定する。そして、ハイブリッド航法装置１０は、最適推定された位置、速度、方位角、および姿勢角等の誤差に基づいてＩＮＳ１より入力される位置、速度、方位角、および姿勢角を補正して出力する。（例えば、特許文献１参照。）。
【０００４】
【特許文献１】
特開２００１−１７４２７５号公報
【０００５】
【発明が解決しようとする課題】
上述のようなハイブリッド航法装置に備えられたカルマンフィルタにおいては、ＩＮＳから入力される測定値とＧＰＳ受信機等の衛星航法装置から入力される測定値との差に基づいて、誤差推定値を算出する。
【０００６】
しかしながら、従来のハイブリッド航法装置では、衛星航法装置から入力される測定値が位置および速度のみであるため、ＩＮＳから入力される測定値が位置、速度、姿勢角、および方位角であっても、カルマンフィルタに入力可能な測定値の差としては、位置と速度のみであった。このため、位置と速度との誤差推定は高精度に行うことができても、姿勢角や方位角については高精度に誤差推定することができなかった。つまり、ハイブリッド航法装置は、ＩＮＳから出力される姿勢角および方位角を高精度に補正し、正確な姿勢角および方位角を提供することができなかった。
【０００７】
この発明の目的は、衛星航法装置とＩＮＳとから入力される測定値に基づいて、姿勢角および方位角を高精度に推定することができるハイブリッド航法装置を提供することにある。
【０００８】
【課題を解決するための手段】
この発明は、衛星航法装置により得られる測定値と、慣性航法装置により得られる測定値とから、カルマンフィルタにて慣性航法装置の測定誤差を推定して、補正するハイブリッド航法装置において、衛星航法装置により得られる位置、速度、および加速度と慣性航法装置により得られる位置、速度、姿勢角、方位角、風速および風向とを入力し、衛星航法装置により得られる速度および加速度に基づく姿勢角および方位角を算出するとともに、衛星航法装置により得れらる測定値と慣性航法装置より得られる測定値との差を算出し、観測値として出力する観測値演算手段と、観測値演算手段で出力された観測値に基づいて、前記慣性航法装置により得られる位置、速度、姿勢角、および方位角の推定誤差を出力するカルマンフィルタと、カルマンフィルタにより出力された推定誤差に基づいて、慣性航法装置から出力された位置、速度、姿勢角、および方位角を補正して出力する補正手段と、を備えたことを特徴としている。
【０００９】
この構成では、観測値演算手段で、衛星航法装置から入力された速度および加速度に基づいて姿勢角および方位角を算出する。この際、慣性航法装置から入力される風速および風向を用いることで算出する。このように衛星航法装置の測定値に基づく姿勢角および方位角と、慣性航法装置により測定された姿勢角および方位角とを用いて、姿勢角および方位角の差分値（観測値）を算出し、これらを他の観測値である位置の差分値、速度の差分値、加速度の差分値とともにカルマンフィルタに出力する。カルマンフィルタは、入力されたこれらの観測値に基づいて誤差推定を行い、補正手段である加算器に出力する。加算器は、ＩＮＳから入力された、位置、速度、加速度、姿勢角、および方位角の各測定値と、カルマンフィルタにより推定された誤差とを加算することで、ＩＮＳからの各測定値の補正を行って出力する。
【００１０】
また、この発明は、少なくとも二つの測位用アンテナを備えた衛星航法装置により得られる測定値と、慣性航法装置により得られる測定値とから、カルマンフィルタにて慣性航法装置の測定誤差を推定して、補正するハイブリッド航法装置において、少なくとも衛星航法装置により得られる姿勢角、および方位角と慣性航法装置により得られる姿勢角、および方位角とを入力し、衛星航法装置により得れらる測定値と慣性航法装置より得られる測定値との差を算出し、観測値として出力する観測値演算手段と、観測値演算手段で出力された観測値に基づいて、前記慣性航法装置により得られる姿勢角、および方位角の推定誤差を出力するカルマンフィルタと、カルマンフィルタにより出力された推定誤差に基づいて、慣性航法装置から出力された姿勢角、および方位角を補正して出力する補正手段と、を備えたことを特徴としている。
【００１１】
この構成では、観測値演算手段で、衛星航法装置により測定算出された姿勢角および方位角と、慣性航法装置により測定算出された姿勢角および方位角とを用いて、姿勢角および方位角の差分値（観測値）を算出し、カルマンフィルタに出力する。カルマンフィルタは、入力されたこれらの観測値に基づいて誤差推定を行い、補正手段である加算器に出力する。加算器は、ＩＮＳから入力された姿勢角、および方位角の各測定値と、カルマンフィルタにより推定された誤差とを加算することで、ＩＮＳからの各測定値の補正を行って出力する。
【００１２】
また、この発明は、カルマンフィルタにより算出された推定誤差を慣性航法装置にフィードバックすることを特徴としている。
【００１３】
この構成では、カルマンフィルタで推定された誤差をＩＮＳにフィードバックすることで、ＩＮＳは、フィードバックされた推定誤差に基づいて、プラットホームのアライメントを調整する。そして、観測値演算手段は、前述のような観測値の算出を行い、カルマンフィルタに出力する。カルマンフィルタはこの観測値に基づき、誤差を推定し、加算器に出力するとともに、再度ＩＮＳにも出力する。
【００１４】
また、この発明は、カルマンフィルタで、位置の推定誤差の生成過程にランジュバン方程式を用いたことを特徴としている。
【００１５】
この構成では、ＩＮＳにおける位置誤差が加速度計の誤差に殆ど影響されないことに基づいて、カルマンフィルタにおける演算で加速度に関する誤差を省略する。これにより、カルマンフィルタにおいては、加速度の誤差推定が削減され、位置、速度、姿勢角、および方位角の誤差推定のみを行うこととなる。
【００１６】
【発明の実施の形態】
本発明の実施形態に係るハイブリッド航法装置について図を参照して説明する。なお、本実施形態では、衛星航法装置としてＧＰＳ受信機を例に説明する。
図１は、本発明の実施形態に係るハイブリッド航法装置の主要部を示すブロック図である。
図１において、１はジャイロと加速度計とを備え、位置、速度、加速度、姿勢角、方位角、風速、および風向を計測して出力する、本発明の慣性航法装置に相当するＩＮＳであり、２は受信したＧＰＳ信号に基づいて位置、速度、加速度を出力するＧＰＳ受信機である。
【００１７】
また、ハイブリッド航法装置１０は、ＩＮＳ１およびＧＰＳ受信機２から入力された位置等の各測定値に基づいて、差分値である観測値を演算する観測値演算部４と、入力された観測値に基づいて、ＩＮＳ１で計測された各測定値に誤差を推定するカルマンフィルタ３と、ＩＮＳ１から出力された各測定値とカルマンフィルタ３から出力された各測定値の推定誤差とを入力して、該推定誤差に基づいて前記各測定値を補正して出力する加算器５とを備える。
【００１８】
これらＩＮＳ１、ＧＰＳ受信機２、およびハイブリッド航法装置１０が、航空機等の機体に搭載されている。
【００１９】
（１）システム方程式
本発明に係るハイブリッド航法装置で用いるシステム方程式は、次のように表すことができる。
【００２０】
【数１】
数１において、ｘはＩＮＳ１における誤差ベクトル、Ａはシステム行列、ｕは駆動雑音ベクトルであり、各測定値は、それぞれに直交する北方向（Ｎ方向）、東方向（Ｅ方向）、垂直下方向（Ｄ方向）からなるＮＥＤ座標系を用いる。
また、数１において、ａＮ ，ａＥ ，ａＤ はそれぞれＩＮＳで計測されたＮ軸方向、Ｅ軸方向、Ｄ軸方向の加速度、ｖＮ ，ｖＥ ，ｖＤ はそれぞれＩＮＳで計測されたＮ軸方向、Ｅ軸方向、Ｄ軸方向の速度、ΩＥ は地球の角速度、Ｒは地球の平均半径、λは機体の緯度、τｇ ，τａ は相関時定数である。
【００２２】
ここで、誤差ベクトルｘの各要素は、
【００２３】
【数２】
であり、駆動雑音ベクトルの各要素は、
【００２５】
【数３】
である。
【００２７】
なお、駆動雑音ベクトルｕおよび相関時定数τｇ ，τａ は設計パラメータであり、システムの設計により定まる。
【００２８】
（２）観測方程式
本発明に係るハイブリッド航法装置で用いる観測方程式は、次のように表すことができる。
【００２９】
【数４】
（４）式において、ｙはＩＮＳ１とＧＰＳ受信機２でそれぞれ測定した位置の差、姿勢角の差から計算したチルトの差、および方位角の差を観測値とする観測ベクトル、Ｃは観測行列、ｗは観測雑音ベクトルである。
【００３１】
ここで、観測ベクトルｙの各要素は、
【００３２】
【数５】
であり、観測雑音ベクトルｗの各要素は、
【００３４】
【数６】
である。
なお、観測雑音ベクトルｗは設計パラメータであり、設計により定まる。
【００３６】
（３）観測値演算部
観測演算部４では、ＧＰＳ受信機２から入力された速度ベクトルｖｇ と加速度ベクトルａｇ とに基づいて、機体軸座標系の姿勢角（ロール角φｇ 、ピッチ角θｇ ）と方位角ψｇ とを、次に示す方法で算出する。
【００３７】
前記ＧＰＳ受信機２により測定された地球軸座標系（ＮＥＤ座標系）の速度ベクトルｖｇ および加速度ベクトルａｇ は（７）式で表される。
【００３８】
【数７】
一方、ＩＮＳ１で測定された風速をｖｓ 、風向をｖｄ とすると、風速ベクトルｖｗ は（８）式で表される。
【００４０】
【数８】
安定軸座標系の速度をベクトルｖｓ とすると、
【００４２】
【数９】
と表され、ＧＰＳ受信機２により計測された地球軸座標系の速度ベクトルｖｇ と風速ベクトルｖｗ との差で表すことができるので、（１０）式のように表される。
【００４４】
【数１０】
安定軸座標系の加速度ベクトルａｓ は、
【００４６】
【数１１】
と表され、速度ベクトルｖｓ の時間微分であり、風速がほぼ定常であるとすると、加速度ベクトルａｓ は、（１２）式で表される。
【００４８】
【数１２】
次に、安定軸座標系の姿勢角であるピッチ角θｓ と方位角ψｓ は（１０）式を用いて、（１３）式および（１４）式で表すことができる。
【００５０】
【数１３】
【数１４】
一方、ロール角φｓ は（１２）式を用いて、次に示す方法で求められる。
加速度ベクトルａｓ の速度ベクトルｖｓ に対する直交成分をａｓ ｎ とすると、
【００５３】
【数１５】
となり、同様に、重力加速度ベクトルｇの速度ベクトルｖｓ に対する直交成分をｇｎ とすると、
【００５５】
【数１６】
となる。ここで、揚力加速度ベクトルをＩとし、水平基準ベクトルをｐすると、揚力加速度ベクトルＩと水平基準ベクトルｐのそれぞれは、次のように表される。
【００５７】
【数１７】
【数１８】
これら揚力加速度ベクトルＩおよび水平基準ベクトルｐを用いると、ロール角φｓ は次の式により求められる。
【００６０】
【数１９】
次に、安定軸座標系で求めた姿勢角θｓ ，φｓ および方位角ψｓ から、ＩＮＳで測定された姿勢角θ，φおよび方位角ψと同じ座標系にするため、機体軸座標系の姿勢角および方位角に変換する。
機体軸座標系のオイラー角を（ψｇ ，θｇ ，φｇ ）とすると、地球軸座標系から機体軸座標系への変換行列Ｒ（ψｇ ，θｇ ，φｇ ）は、次式で表すことができる。
【００６２】
【数２０】
地球軸座標系から安定軸座標系への変換行列はＲ（ψｓ ，θｓ ，φｓ ）であり、迎角計からの迎角をαとすると安定軸座標系から機体軸座標系への変換行列はＲ（０，α，０）であるので、変換行列Ｒ（ψｇ ，θｇ ，φｇ ），Ｒ（ψｓ ，θｓ ，φｓ ），Ｒ（０，α，０）には次式の関係が成り立つ。
【００６４】
【数２１】
したがって、機体軸座標系のロール角θｇ 、ピッチ角φｇ 、方位角ψｇ はそれぞれ次のように表される。
【００６６】
【数２２】
【数２３】
【数２４】
このようにＧＰＳ受信機１の測定値により求められた姿勢角θｇ ，φｇ および方位角ψｇ と、ＩＮＳにより測定された姿勢角θ，φおよび方位角ψとの差に基づいて、Ｎ軸回りのチルトｙ３ とＥ軸回りのチルトｙ４ とＤ軸回りの方位角ｙ５ とが次式により求められる。
【００７０】
【数２５】
また、観測値演算部４は、ＩＮＳ１により測定された位置とＧＰＳ受信機２により測定された位置とに基づき、Ｎ軸方向の位置の差ｙ１ とＥ軸方向の位置の差ｙ２ とを求める。このようにして、観測方程式の観測値ベクトルｙを求める。
【００７２】
上述のＧＰＳ受信機２で測定した速度および加速度から機体の姿勢角（ピッチ角、ロール角）および方位角を求める方法は、「ＲＩＣＨＡＲＤ Ｐ．ＫＯＲＮＦＩＥＬＤ， Ｒ．ＪＯＨＮ ＨＡＨＳＭＡＮ ａｎｄ ＪＯＨＮ Ｊ．ＤＥＹＳＴ：Ｓｉｎｇｌｅ−Ａｎｔｅｎｎａ ＧＰＳ−Ｂａｓｅｄ Ａｉｒｃｒａｆｔ Ａｔｔｉｔｕｄｅ Ｄｅｔｅｒｍｉｎｅｔｉｏｎ， Ｎａｖｉｇａｔｉｏｎ Ｊｏｕｒｎａｌ ｏｆ Ｔｈｅ Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｎａｖｉｇａｔｉｏｎ， Ｖｏｌ．４５， Ｎｏ．１， Ｓｐｒｉｎｇ，１９９８ 」に示されている。
【００７３】
（４）カルマンフィルタ
上述のシステム方程式と観測方程式とを離散時間系に変換すると、次式のように表される。
【００７４】
【数２６】
【数２７】
この離散時間系システム方程式と観測方程式とを用いて周知方法で次に示すカルマンフィルタの各式を導く。
【００７７】
伝播式
【００７８】
【数２８】
このように、推定値を意味する＾（ハット）付き記号を以下２文字で表す。
【００８０】
【数２９】
更新式
【００８２】
【数３０】
【数３１】
【数３２】
カルマンフィルタ３は、（２８）式〜（３０）式に示したカルマンフィルタの式を用いて、更新周期Ｔｓ毎に観測値演算部４から入力される各観測値に基づいて、誤差推定を行い、加算器５に出力する。ここで、カルマンフィルタ３は、上述のカルマンフィルタの式により求められたＮ軸回りのチルト誤差ｘ５ とＥ軸回りのチルト誤差ｘ６ とに基づいて、次の変換式を用いて姿勢角誤差を算出した後に、加算器５に出力する。
【００８６】
【数３３】
（５）加算器
加算器５はＩＮＳ１から直接入力された位置、速度、姿勢角、方位角に、カルマンフィルタ３から入力された位置誤差、速度誤差、姿勢角誤差、方位角誤差をそれぞれ加算する。すなわち、ＩＮＳ１から入力された位置、速度、姿勢角、方位角を、それぞれ位置誤差、速度誤差、姿勢角誤差、方位角誤差で補正し、表示器（図示せず）等に出力する。
【００８８】
このような構成とすることで、カルマンフィルタで姿勢角および方位角の誤差推定を正確に行うことができるので、従来のように位置と速度のみを高精度に得られるだけでなく、姿勢角や方位角についても高精度に得ることができる。
【００８９】
なお、本実施形態では、カルマンフィルタ３で推定した誤差に基づいて、加算器５で、ＩＮＳ１から出力された各測定値を補正する方法を示したが、図２に示すように、カルマンフィルタ３で推定した誤差をＩＮＳ１にフィードバックし、プラットホームのアライメントを調整する。このような構成とすることで、特にジャイロバイアス誤差が１ｄｅｇ／ｈ程度のような、低精度のＩＮＳを用いても誤差が発散することなく、高精度に誤差推定を行うことができる。これにより、低精度であるが安価なＩＮＳを使用できるので、装置全体のコストダウンを行うことができる。
【００９０】
また、本実施形態に示したカルマンフィルタにランジュバン方程式を適用することができる。これは、ＩＮＳの位置誤差はジャイロの誤差と加速度計の誤差が要因となっているが、その殆どがジャイロの誤差であるため、加速度計の誤差を省略することができるからである。これを利用し、ランジュバン方程式で位置誤差の生成過程をモデル化する。
このようにランジュバン方程式で位置誤差の生成過程をモデル化することで、システム方程式の誤差ベクトルから加速度誤差を省略することができる。これにより、この誤差ベクトルをｘ”とすると、システム行列Ａ”、駆動雑音ベクトルｕ”は次式となる。
【００９１】
【数３４】
【数３５】
この時、システム方程式は、
【００９４】
【数３６】
となる。
【００９６】
このシステム方程式と上述の観測方程式（（２）式）とを用いることで、位置誤差、姿勢角誤差、方位角誤差をカルマンフィルタで推定することができる。
【００９７】
このような構成とすることで、ＩＮＳのジャイロの誤差における加速度要因を省略することができるので、位置誤差および速度誤差とジャイロのバイアス誤差とを分離することができる。このため、カルマンフィルタのチューニングは容易になり、位置誤差、姿勢角誤差、方位角誤差等のカルマンフィルタによる誤差推定を容易にすることができる。
【００９８】
なお、上述の実施形態では、ＧＰＳ受信機で位置、速度、および加速度を測定して、ＩＮＳの位置、速度、姿勢角、および方位角の誤差推定に利用する方法を示したが、ＧＰＳ受信機で姿勢角、および方位角を直接算出して、ＩＮＳの姿勢角、および方位角の誤差推定に利用してもよい。
【００９９】
この場合、ＧＰＳ受信機には少なくとも二つのＧＰＳアンテナが備えられており、これらＧＰＳアンテナ間の相対測位を行い、この結果から姿勢角および方位角を算出する。そして、ＧＰＳ受信機で算出された姿勢角および方位角と、ＩＮＳで測定された姿勢角および方位角との差分を求め、カルマンフィルタで誤差推定を行う。
【０１００】
このような構成とすることで、ＧＰＳ受信機およびＩＮＳでそれぞれ直接求められた姿勢角および方位角を用いて、これらの誤差推定を行うことができる。
【０１０１】
また、ＧＰＳ受信機で測定、算出された位置、速度、加速度、姿勢角、および方位角のうちの必要な測定値と、ＩＮＳで測定した位置、速度、加速度、姿勢角、方位角、風向、および風速のうちの必要な測定値とを用いて、これらを組み合わせ、誤差推定を行ってもよい。
【０１０２】
また、上述の実施形態では、プラットホーム方式のＩＮＳについて説明したが、ストラップダウン方式のＩＮＳやＩＲＵ（慣性基準ユニット）についても上述の構成を適用することができる。
【０１０３】
【発明の効果】
この発明によれば、カルマンフィルタで位置誤差、速度誤差とともに、姿勢角誤差および方位角誤差をそれぞれの観測値に基づいて推定するため、高精度に誤差推定を行うことができる。
【０１０４】
また、衛星航法装置および慣性航法装置のそれぞれで少なくとも姿勢角および方位角を直接、測定算出し、これらを用いて各測定値の差分を算出してカルマンフィルタで誤差推定することで、高精度に誤差推定することができる。
【０１０５】
このため、これらの推定誤差を用いてＩＮＳから出力される位置、姿勢角、方位角を補正することで、機体の正確な位置、姿勢、方位を得ることができる。
【０１０６】
また、この発明によれば、カルマンフィルタで推定した位置誤差、速度誤差、姿勢角誤差、および方位角誤差をＩＮＳにフィードバックすることにより、ＧＰＳ受信機から得られる位置、速度の測定値や速度及び加速度から得られる姿勢角、方位角と、ＩＮＳで予め補正された位置、速度、加速度、姿勢角、方位角との差が徐々に小さくなるため、観測演算部からカルマンフィルタに出力される観測値を徐々に小さくすることができる。このように徐々に小さくなる観測値をカルマンフィルタに入力していくことで、誤差が収束しやすくなり、さらに高精度に誤差推定を行うことができる。
【０１０７】
また、この発明によれば、カルマンフィルタにランジュバン方程式を適用することで、加速度誤差を省略することができ、位置誤差および速度誤差とジャイロのバイアス誤差とを分離することができるので、誤差推定を容易にすることができる。
【図面の簡単な説明】
【図１】本発明の実施形態に係るハイブリッド航法装置の主要部を示すブロック図
【図２】他の構成のハイブリッド航法装置の主要部を示すブロック図
【図３】従来のハイブリッド航法装置の主要部を示すブロック図
【符号の説明】
１−ＩＮＳ
２−ＧＰＳ受信機
３−カルマンフィルタ
４−観測値演算部
５−加算器
１０−ハイブリッド航法装置
Claims (4)
- 衛星航法装置により得られる測定値と、慣性航法装置により得られる測定値とから、カルマンフィルタにて慣性航法装置の測定誤差を推定して、補正するハイブリッド航法装置において、
前記衛星航法装置により得られる位置、速度、および加速度と前記慣性航法装置により得られる位置、速度、姿勢角、方位角、風速および風向とを入力し、前記衛星航法装置により得られる速度および加速度に基づく姿勢角および方位角を算出するとともに、前記衛星航法装置により得れらる測定値と、前記慣性航法装置より得られる測定値との差を算出して観測値として出力する観測値演算手段と、
該観測値演算手段で出力された前記観測値に基づいて、前記慣性航法装置により得られる位置、速度、姿勢角、方位角の推定誤差を出力するカルマンフィルタと、
該カルマンフィルタにより出力された推定誤差に基づいて、前記慣性航法装置から出力された位置、速度、姿勢角、および方位角を補正して出力する補正手段と、を備えたことを特徴とするハイブリッド航法装置。 - 少なくとも二つの測位用アンテナを備える衛星航法装置により得られる測定値と、慣性航法装置により得られる測定値とから、カルマンフィルタにて慣性航法装置の測定誤差を推定して、補正するハイブリッド航法装置において、
少なくとも、前記衛星航法装置により得られる姿勢角および方位角と前記慣性航法装置により得られる姿勢角および方位角とを入力し、前記衛星航法装置により得れらる測定値と、前記慣性航法装置より得られる測定値との差を算出して観測値として出力する観測値演算手段と、
該観測値演算手段で出力された前記観測値に基づいて、前記慣性航法装置により得られる姿勢角、および方位角の推定誤差を出力するカルマンフィルタと、
該カルマンフィルタにより出力された推定誤差に基づいて、前記慣性航法装置から出力された姿勢角、および方位角を補正して出力する補正手段と、を備えたことを特徴とするハイブリッド航法装置。 - 前記カルマンフィルタは、前記推定誤差を前記慣性航法装置にフィードバックする請求項１または請求項２に記載のハイブリッド航法装置。
- 前記カルマンフィルタは、前記位置の推定誤差の生成過程にランジュバン方程式を用いた請求項１〜請求項３のいずれかに記載のハイブリッド航法装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003026363A JP4199553B2 (ja) | 2003-02-03 | 2003-02-03 | ハイブリッド航法装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003026363A JP4199553B2 (ja) | 2003-02-03 | 2003-02-03 | ハイブリッド航法装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2004239643A true JP2004239643A (ja) | 2004-08-26 |
| JP4199553B2 JP4199553B2 (ja) | 2008-12-17 |
Family
ID=32954396
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2003026363A Expired - Fee Related JP4199553B2 (ja) | 2003-02-03 | 2003-02-03 | ハイブリッド航法装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP4199553B2 (ja) |
Cited By (27)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006138834A (ja) * | 2004-11-12 | 2006-06-01 | Mitsubishi Electric Corp | 航法装置、航法システム、航法測位方法、および車両 |
| JP2007163335A (ja) * | 2005-12-15 | 2007-06-28 | Mitsubishi Electric Corp | 姿勢標定装置、姿勢標定方法および姿勢標定プログラム |
| JP2008076389A (ja) * | 2006-09-19 | 2008-04-03 | Alpine Electronics Inc | ナビゲーション・システムおよびナビゲーション方法 |
| WO2008117492A1 (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co., Ltd. | Gps複合航法装置 |
| JP2008230579A (ja) * | 2007-03-23 | 2008-10-02 | Nec Corp | 姿勢検出装置および方法 |
| JP2008232867A (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co Ltd | 移動体の姿勢及び方位検出装置 |
| WO2008142757A1 (ja) * | 2007-05-18 | 2008-11-27 | Churyo Engineering Co., Ltd. | 単一gpsと慣性データ(加速度,角速度)によるビークル姿勢角の計測方法 |
| GB2451348A (en) * | 2007-07-25 | 2009-01-28 | Furuno Electric Co | GPS based attitude determination system having a self-diagnosis function |
| JPWO2007105294A1 (ja) * | 2006-03-13 | 2009-07-23 | 中菱エンジニアリング株式会社 | ビークル走行軌跡計測装置 |
| JP2011141221A (ja) * | 2010-01-08 | 2011-07-21 | Toyota Motor Corp | 測位装置及び測位方法 |
| JP2012237606A (ja) * | 2011-05-10 | 2012-12-06 | Seiko Epson Corp | 位置算出方法及び位置算出装置 |
| WO2013047966A1 (en) * | 2011-09-30 | 2013-04-04 | Samsung Electronics Co., Ltd. | Apparatus and method for detecting heading change in mobile terminal |
| JP2013170904A (ja) * | 2012-02-20 | 2013-09-02 | Ono Sokki Co Ltd | 計測装置及び計測方法 |
| KR20130112917A (ko) * | 2010-12-03 | 2013-10-14 | 퀄컴 인코포레이티드 | Gnss 운송수단 내비게이션을 위한 관성 센서 지원 헤딩 및 포지셔닝 |
| WO2015162873A1 (ja) * | 2014-04-25 | 2015-10-29 | 日本電気株式会社 | 位置姿勢推定装置、画像処理装置及び位置姿勢推定方法 |
| JP2017106919A (ja) * | 2015-12-07 | 2017-06-15 | アトランティック・イナーシャル・システムズ・リミテッドＡｔｌａｎｔｉｃ Ｉｎｅｒｔｉａｌ Ｓｙｓｔｅｍｓ Ｌｉｍｉｔｅｄ | 慣性航法システム |
| WO2017175628A1 (ja) | 2016-04-06 | 2017-10-12 | 株式会社クボタ | 測位検出装置及び測位検出装置を備えた作業機 |
| CN109387220A (zh) * | 2017-08-03 | 2019-02-26 | 北京自动化控制设备研究所 | 一种步行微惯性导航设备方位安装偏差校正方法 |
| WO2019146381A1 (ja) | 2018-01-23 | 2019-08-01 | 株式会社クボタ | 作業車両 |
| KR102061855B1 (ko) * | 2017-12-19 | 2020-01-02 | 주식회사 한화 | 비행 모델의 모의자세 추정 장치 및 그 방법 |
| JP2020006872A (ja) * | 2018-07-11 | 2020-01-16 | 株式会社クボタ | 作業車両 |
| WO2020050084A1 (ja) * | 2018-09-07 | 2020-03-12 | 国立研究開発法人宇宙航空研究開発機構 | 誘導制御プログラムを格納した記憶媒体 |
| JP2020169872A (ja) * | 2019-04-02 | 2020-10-15 | 株式会社豊田中央研究所 | 慣性航法装置 |
| CN112051598A (zh) * | 2020-06-24 | 2020-12-08 | 中铁第四勘察设计院集团有限公司 | 一种基于双重校正的车载gnss/ins组合导航方法 |
| CN114001730A (zh) * | 2021-09-24 | 2022-02-01 | 深圳元戎启行科技有限公司 | 融合定位方法、装置、计算机设备和存储介质 |
| CN114485641A (zh) * | 2022-01-24 | 2022-05-13 | 武汉梦芯科技有限公司 | 一种基于惯导卫导方位融合的姿态解算方法及装置 |
| JP7439162B2 (ja) | 2022-04-21 | 2024-02-27 | ヤマハ発動機株式会社 | 船舶の速度制御方法及び船舶 |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102997916B (zh) * | 2011-09-15 | 2015-08-05 | 北京自动化控制设备研究所 | 一种自主提高定位定向***惯性姿态解算精度的方法 |
-
2003
- 2003-02-03 JP JP2003026363A patent/JP4199553B2/ja not_active Expired - Fee Related
Cited By (50)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006138834A (ja) * | 2004-11-12 | 2006-06-01 | Mitsubishi Electric Corp | 航法装置、航法システム、航法測位方法、および車両 |
| JP4694870B2 (ja) * | 2004-11-12 | 2011-06-08 | 三菱電機株式会社 | 航法装置、航法システム、航法測位方法、および車両 |
| JP2007163335A (ja) * | 2005-12-15 | 2007-06-28 | Mitsubishi Electric Corp | 姿勢標定装置、姿勢標定方法および姿勢標定プログラム |
| JP4846784B2 (ja) * | 2006-03-13 | 2011-12-28 | 中菱エンジニアリング株式会社 | ビークル走行軌跡計測装置 |
| JPWO2007105294A1 (ja) * | 2006-03-13 | 2009-07-23 | 中菱エンジニアリング株式会社 | ビークル走行軌跡計測装置 |
| JP2008076389A (ja) * | 2006-09-19 | 2008-04-03 | Alpine Electronics Inc | ナビゲーション・システムおよびナビゲーション方法 |
| US20100211315A1 (en) * | 2007-03-22 | 2010-08-19 | Furuno Electric Company Limited | Gps composite navigation apparatus |
| WO2008117492A1 (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co., Ltd. | Gps複合航法装置 |
| US8649977B2 (en) | 2007-03-22 | 2014-02-11 | Furuno Electric Company Limited | GPS composite navigation apparatus |
| JP2008232869A (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co Ltd | Ｇｐｓ複合航法装置 |
| JP2008232867A (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co Ltd | 移動体の姿勢及び方位検出装置 |
| JP2008230579A (ja) * | 2007-03-23 | 2008-10-02 | Nec Corp | 姿勢検出装置および方法 |
| JP4527171B2 (ja) * | 2007-05-18 | 2010-08-18 | 中菱エンジニアリング株式会社 | 単一ｇｐｓと慣性データ（加速度，角速度）によるビークル姿勢角の計測方法 |
| JPWO2008142757A1 (ja) * | 2007-05-18 | 2010-08-05 | 中菱エンジニアリング株式会社 | 単一ｇｐｓと慣性データ（加速度，角速度）によるビークル姿勢角の計測方法 |
| WO2008142757A1 (ja) * | 2007-05-18 | 2008-11-27 | Churyo Engineering Co., Ltd. | 単一gpsと慣性データ(加速度,角速度)によるビークル姿勢角の計測方法 |
| US7987049B2 (en) | 2007-07-25 | 2011-07-26 | Furuno Electric Company, Ltd. | GPS-based attitude determination system with self-diagnosis function |
| GB2451348B (en) * | 2007-07-25 | 2011-09-14 | Furuno Electric Co | GPS-based attitude determination system with self-diagnosis function |
| GB2451348A (en) * | 2007-07-25 | 2009-01-28 | Furuno Electric Co | GPS based attitude determination system having a self-diagnosis function |
| JP2011141221A (ja) * | 2010-01-08 | 2011-07-21 | Toyota Motor Corp | 測位装置及び測位方法 |
| KR101600150B1 (ko) * | 2010-12-03 | 2016-03-04 | 퀄컴 인코포레이티드 | Gnss 운송수단 내비게이션을 위한 관성 센서 지원 헤딩 및 포지셔닝 |
| US9803983B2 (en) | 2010-12-03 | 2017-10-31 | Qualcomm Incorporated | Inertial sensor aided heading and positioning for GNSS vehicle navigation |
| KR20130112917A (ko) * | 2010-12-03 | 2013-10-14 | 퀄컴 인코포레이티드 | Gnss 운송수단 내비게이션을 위한 관성 센서 지원 헤딩 및 포지셔닝 |
| US9891054B2 (en) | 2010-12-03 | 2018-02-13 | Qualcomm Incorporated | Inertial sensor aided heading and positioning for GNSS vehicle navigation |
| JP2014507628A (ja) * | 2010-12-03 | 2014-03-27 | クゥアルコム・インコーポレイテッド | Ｇｎｓｓ車両ナビゲーションのための慣性センサにより支援された進行方向および測位 |
| US9816818B2 (en) | 2010-12-03 | 2017-11-14 | Qualcomm Incorporated | Inertial sensor aided heading and positioning for GNSS vehicle navigation |
| JP2012237606A (ja) * | 2011-05-10 | 2012-12-06 | Seiko Epson Corp | 位置算出方法及び位置算出装置 |
| WO2013047966A1 (en) * | 2011-09-30 | 2013-04-04 | Samsung Electronics Co., Ltd. | Apparatus and method for detecting heading change in mobile terminal |
| JP2013170904A (ja) * | 2012-02-20 | 2013-09-02 | Ono Sokki Co Ltd | 計測装置及び計測方法 |
| JPWO2015162873A1 (ja) * | 2014-04-25 | 2017-04-13 | 日本電気株式会社 | 位置姿勢推定装置、画像処理装置及び位置姿勢推定方法 |
| US10901095B2 (en) | 2014-04-25 | 2021-01-26 | Nec Corporation | Position and attitude estimation device, image processing device, and position and attitude estimation method |
| WO2015162873A1 (ja) * | 2014-04-25 | 2015-10-29 | 日本電気株式会社 | 位置姿勢推定装置、画像処理装置及び位置姿勢推定方法 |
| JP2017106919A (ja) * | 2015-12-07 | 2017-06-15 | アトランティック・イナーシャル・システムズ・リミテッドＡｔｌａｎｔｉｃ Ｉｎｅｒｔｉａｌ Ｓｙｓｔｅｍｓ Ｌｉｍｉｔｅｄ | 慣性航法システム |
| US11112422B2 (en) | 2015-12-07 | 2021-09-07 | Atlantic Inertial Systems Limited | Inertial navigation system |
| WO2017175628A1 (ja) | 2016-04-06 | 2017-10-12 | 株式会社クボタ | 測位検出装置及び測位検出装置を備えた作業機 |
| CN109387220B (zh) * | 2017-08-03 | 2022-07-08 | 北京自动化控制设备研究所 | 一种步行微惯性导航设备方位安装偏差校正方法 |
| CN109387220A (zh) * | 2017-08-03 | 2019-02-26 | 北京自动化控制设备研究所 | 一种步行微惯性导航设备方位安装偏差校正方法 |
| KR102061855B1 (ko) * | 2017-12-19 | 2020-01-02 | 주식회사 한화 | 비행 모델의 모의자세 추정 장치 및 그 방법 |
| WO2019146381A1 (ja) | 2018-01-23 | 2019-08-01 | 株式会社クボタ | 作業車両 |
| JP2020006872A (ja) * | 2018-07-11 | 2020-01-16 | 株式会社クボタ | 作業車両 |
| JP7290923B2 (ja) | 2018-07-11 | 2023-06-14 | 株式会社クボタ | 作業車両 |
| WO2020050084A1 (ja) * | 2018-09-07 | 2020-03-12 | 国立研究開発法人宇宙航空研究開発機構 | 誘導制御プログラムを格納した記憶媒体 |
| JP2020169872A (ja) * | 2019-04-02 | 2020-10-15 | 株式会社豊田中央研究所 | 慣性航法装置 |
| JP7036080B2 (ja) | 2019-04-02 | 2022-03-15 | 株式会社豊田中央研究所 | 慣性航法装置 |
| CN112051598B (zh) * | 2020-06-24 | 2023-09-29 | 中铁第四勘察设计院集团有限公司 | 一种基于双重校正的车载gnss/ins组合导航方法 |
| CN112051598A (zh) * | 2020-06-24 | 2020-12-08 | 中铁第四勘察设计院集团有限公司 | 一种基于双重校正的车载gnss/ins组合导航方法 |
| CN114001730A (zh) * | 2021-09-24 | 2022-02-01 | 深圳元戎启行科技有限公司 | 融合定位方法、装置、计算机设备和存储介质 |
| CN114001730B (zh) * | 2021-09-24 | 2024-03-08 | 深圳元戎启行科技有限公司 | 融合定位方法、装置、计算机设备和存储介质 |
| CN114485641A (zh) * | 2022-01-24 | 2022-05-13 | 武汉梦芯科技有限公司 | 一种基于惯导卫导方位融合的姿态解算方法及装置 |
| CN114485641B (zh) * | 2022-01-24 | 2024-03-26 | 武汉梦芯科技有限公司 | 一种基于惯导卫导方位融合的姿态解算方法及装置 |
| JP7439162B2 (ja) | 2022-04-21 | 2024-02-27 | ヤマハ発動機株式会社 | 船舶の速度制御方法及び船舶 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP4199553B2 (ja) | 2008-12-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4199553B2 (ja) | ハイブリッド航法装置 | |
| EP3408688B1 (en) | Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
| JP6094026B2 (ja) | 姿勢判定方法、位置算出方法及び姿勢判定装置 | |
| JP5328252B2 (ja) | ナビゲーションシステムの位置検出装置および位置検出方法 | |
| JP5419665B2 (ja) | 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム | |
| US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
| JP6471749B2 (ja) | 位置姿勢推定装置、画像処理装置及び位置姿勢推定方法 | |
| JP5586994B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
| JP5742450B2 (ja) | 位置算出方法及び位置算出装置 | |
| WO2017150106A1 (ja) | 車載装置、及び、推定方法 | |
| JP5602070B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
| JP2012173190A (ja) | 測位システム、測位方法 | |
| CN112798021B (zh) | 基于激光多普勒测速仪的惯导***行进间初始对准方法 | |
| US7248967B2 (en) | Autonomous velocity estimation and navigation | |
| JP5022747B2 (ja) | 移動体の姿勢及び方位検出装置 | |
| JP2016033473A (ja) | 位置算出方法及び位置算出装置 | |
| CN105606093B (zh) | 基于重力实时补偿的惯性导航方法及装置 | |
| CN108627152A (zh) | 一种微型无人机基于多传感器数据融合的导航方法 | |
| CN109084756B (zh) | 一种重力视运动参数辨识与加速度计零偏分离方法 | |
| JPWO2006067968A1 (ja) | 進行方向測定装置 | |
| JP4527171B2 (ja) | 単一ｇｐｓと慣性データ（加速度，角速度）によるビークル姿勢角の計測方法 | |
| JP4343581B2 (ja) | 移動体の姿勢検出装置 | |
| JP2012202749A (ja) | 方位測定装置 | |
| JP4884109B2 (ja) | 移動軌跡算出方法、移動軌跡算出装置及び地図データ生成方法 | |
| JP5180447B2 (ja) | キャリア位相相対測位装置及び方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20050330 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20080318 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20080519 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20080701 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20080829 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20080930 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20081003 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111010 Year of fee payment: 3 |
|
| R150 | Certificate of patent (=grant) or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121010 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131010 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20141010 Year of fee payment: 6 |
|
| LAPS | Cancellation because of no payment of annual fees |