JP2004045385A - 移動体の姿勢検出装置 - Google Patents
移動体の姿勢検出装置 Download PDFInfo
- Publication number
- JP2004045385A JP2004045385A JP2003131985A JP2003131985A JP2004045385A JP 2004045385 A JP2004045385 A JP 2004045385A JP 2003131985 A JP2003131985 A JP 2003131985A JP 2003131985 A JP2003131985 A JP 2003131985A JP 2004045385 A JP2004045385 A JP 2004045385A
- Authority
- JP
- Japan
- Prior art keywords
- alignment angle
- estimated
- imu
- updated
- gps
- 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
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
【課題】初期アライメント角の大きさに関係なく、アンテナ座標系とＩＭＵ座標系との間のアライメント角を確実に推定する移動体の姿勢検出装置を構成する。
【解決手段】ＧＰＳ角速度と、ＩＭＵ角速度との差分を観測し、アライメント角推定部１０６でアライメント角と誤差要因とを推定する。推定アライメント角とセンサ誤差とはそれぞれアライメント角加算部１０７とセンサ誤差加算部１０８とで、順次累積加算、更新されていく。更新されたアライメント角は慣性データ変換部１０３にフィードバックされ、センサ誤差は慣性データ補正部１０２にフィードバックされる。このようにアライメント角推定演算フローに推定周期毎の推定値を累積加算してフィードバックすることで、更新されたアライメント角が正確なアライメント角に徐々に近づくように推定を繰り返す。
【選択図】 図２
【解決手段】ＧＰＳ角速度と、ＩＭＵ角速度との差分を観測し、アライメント角推定部１０６でアライメント角と誤差要因とを推定する。推定アライメント角とセンサ誤差とはそれぞれアライメント角加算部１０７とセンサ誤差加算部１０８とで、順次累積加算、更新されていく。更新されたアライメント角は慣性データ変換部１０３にフィードバックされ、センサ誤差は慣性データ補正部１０２にフィードバックされる。このようにアライメント角推定演算フローに推定周期毎の推定値を累積加算してフィードバックすることで、更新されたアライメント角が正確なアライメント角に徐々に近づくように推定を繰り返す。
【選択図】 図２
Description
【０００１】
【発明の属する技術分野】
この発明は、ＧＰＳ姿勢とＩＭＵ姿勢とを統合して移動体の姿勢を検出する、ＧＰＳ／ＩＭＵ統合化姿勢検出装置、特にアンテナ座標系のＧＰＳ姿勢とＩＭＵ座標系のＩＭＵ姿勢とのアライメント角を確実に推定するＧＰＳ／ＩＭＵ統合化姿勢検出装置に関するものである。
【０００２】
【従来の技術】
移動体の方位および姿勢を検出するシステムの一つとして、ＧＰＳ姿勢検出システムがある。このシステムでは、全てが一直線上に並ばない少なくとも３個のＧＰＳ受信アンテナを移動体に装着し、これら直交３軸座標系で定義されたＧＰＳ受信アンテナのそれぞれがＧＰＳ衛星からの電波信号を受信して、アンテナ間のキャリア位相差を観測する。この観測値からそれぞれのアンテナの相対位置を計測することにより、アンテナ座標系を構成し、座標系の基準座標に対する方位、姿勢を計測する。
【０００３】
しかし、このようなＧＰＳ姿勢検出システムでは、ＧＳＰ衛星からの電波信号を受信することにより姿勢検出が可能であるが、ＧＰＳ衛星からの電波信号が遮断されたり、キャリア位相サイクルスリップ等が生じた場合、キャリア位相差を観測できなくなり、姿勢検出できなくなってしまうという問題がある。
【０００４】
この問題を解決する方法として、角速度センサや加速度センサ等の慣性センサ（ＩＭＵ）を移動体に取り付け、移動体の動きを観測して、この観測データから得られる姿勢とＧＰＳ姿勢検出システムにより得られる姿勢との座標系回転量（アライメント角）を推定して統合する、ＧＰＳ／ＩＭＵ統合化技術がある。
【０００５】
この方法は、直交３軸座標系の各軸にそれぞれマウントされた慣性センサから既知の方法で得られる慣性センサ座標系（以下、「ＩＭＵ座標系」という）の姿勢と、ＧＰＳ姿勢検出システムから得られるアンテナ座標系の姿勢とのアライメント角を推定して統合することにより、安定して高精度に姿勢を得ることができる。この方法を用いることにより、ＧＰＳ衛星からの電波信号が中断しても、中断時の移動体の動きを慣性センサで観測することができるので、電波信号中断中は慣性センサによる姿勢でＧＰＳによる姿勢の欠落を補間することができ、移動体の姿勢を常時出力することができる（例えば、特許文献１参照。）。
【０００６】
【特許文献１】
特開２００２−５４９４６号
【０００７】
【発明が解決しようとする課題】
ところが、このような従来の移動体の姿勢検出装置では、次に示す解決すべき課題が存在した。
【０００８】
移動体の姿勢検出装置におけるＧＰＳ／ＩＭＵ統合技術では、アンテナおよび慣性センサを取り付ける際に、それぞれの座標系であるアンテナ座標系とＩＭＵ座標系との座標系回転量（アライメント角）を求めなければならない。
【０００９】
従来のアライメント角推定技法として、次のいずれかの方法が用いられている。
第１の方法としては、複数の慣性センサからなるＩＭＵ座標系ユニットの一基準方向と複数のＧＰＳ受信アンテナからなるアンテナ座標系の一基準方向とが一致するように、目視等で確認しながら、複数のＧＰＳ受信アンテナを取り付ける。その後、この取り付けで生じたであろう両座標系間のアライメント角を無視し、両座標系が一致したものとみなす。
【００１０】
この方法では、数度程度のアライメント誤差が頻繁に発生するため、ＧＰＳ／ＩＭＵ統合技術を用いても、姿勢精度が低く、不安定であった。
【００１１】
第２の方法としては、第１の方法に示したアライメント角設定の後、次に示すアライメント角推定方法でアライメント角を推定するものである。
【００１２】
一例として、ＩＭＵセンサとして角速度センサを用いた場合を説明する。
ＧＰＳ姿勢検出システムで得られた姿勢から角速度（以下「ＧＰＳ角速度」という。）ωｇ１を演算し、これと同じ時点で角速度センサにより角速度（以下「ＩＭＵ角速度」という。）ωｉ１を得る。これらＧＰＳ角速度ωｇ１とＩＭＵ角速度ωｉ１とを差分することにより、差分値Δｚ１ を求める。この差分値Δｚ１ に基づいてアライメント角θｉ１を推定し、このアライメント角θｉ１を用いて次の時点のＩＭＵ角速度ωｉ２を補正する。次に、このＩＭＵ角速度ωｉ２と同時点のＧＰＳ角速度ωｇ２との差分をとり、新たな差分値Δｚ２ を求め、この差分値Δｚ２ から新たなアライメント角θｉ２を推定し、これをさらに次の時点のＩＭＵ角速度の補正に用いる。このような演算を繰り返し行うことにより、アライメント角θｉ を収束させて、真のアライメント角を算出する。
【００１３】
この方法では、算出されるアライメント角が数度程度の微小角でなければ、非線形のため収束しないので、初期条件として、アライメント角が微小角であることが必要となってしまう。
【００１４】
ところで、例えば、ユーザがＧＰＳ受信アンテナを任意に現場で配置する場合には、その場でＧＰＳ座標系を設定し、これをＩＭＵ座標系に合うようにとりつけなければならない。しかし、このような作業で、微小なアライメント角となるように複数のＧＰＳ受信アンテナを設置することは非常に困難なことである。また、ＧＰＳ受信アンテナの取り付け位置からＩＭＵセンサユニットが視認できない場合には、前述の目視によるあわせ込みは不可能であるため、アライメント角を微小にすることは非常に困難である。
【００１５】
この発明の目的は、アライメント角の大きさに関係なく、アンテナ座標系とＩＭＵ座標系とのアライメント角を確実に推定することができる移動体の姿勢検出装置を提供することにある。
【００１６】
【課題を解決するための手段】
この発明は、アライメント角推定手段とアライメント角加算手段とを備えて、所定の周期毎に推定されるアライメント角を累積加算して更新しながら、アライメント角推定演算にフィードバックすることを特徴としている。
【００１７】
アライメント角推定手段は、慣性データ変換手段とアライメント角推定手段とからなり、慣性データ変換手段は、ＩＭＵ慣性センサにより計測された慣性データをＩＭＵ座標系からアンテナ座標系に変換する。
【００１８】
アライメント角推定手段は、座標変換された慣性データとＧＰＳ姿勢検出システムから演算された慣性データ（以下、「ＧＰＳ慣性データ」という。）との差分値から、所定の周期でアライメント角を推定する。
【００１９】
推定されたアライメント角は、アライメント角加算手段で、前記周期で順次累積加算され更新され、慣性データ変換手段に出力される。慣性データ変換手段では、ある時点での更新アライメント角を用いて、慣性データを座標変換する。
【００２０】
このように、本発明のアライメント角推定手段は、ある時点の差分値から推定したアライメント角を用いて、逐次、慣性データを変換し、この変換された慣性データと、変換された慣性データと同じ時点のＧＰＳ慣性データとを差分して新たなアライメント角を推定する。
【００２１】
このようなフィードバック演算を繰り返し行うことで、ある時点での推定アライメント角は、前時点で更新されたアライメント角を用いて変換された慣性データとその時点でのＧＰＳ慣性データとの差分値から推定されるので、アライメント角推定手段で推定されるアライメント値は徐々に小さく収束していく。そして、これと同時にアライメント角加算部では、更新アライメント角がアライメント角の真値に向かって徐々に収束していく。
【００２２】
このように推定と累積加算とを繰り返して得られたアライメント角を用いることにより、アンテナ座標系の姿勢とＩＭＵ座標系の姿勢とを統合して、高精度で安定した姿勢を出力する。
【００２３】
また、この発明は、アライメント角の各成分が、−８５°≦θｘ ≦８５°，−８５°≦θｙ ≦８５°，−９０°≦θｚ ≦９０°のように大きな値であるときでも、アライメント角を推定することができる。
【００２４】
このように所定の範囲にアライメント角で収まるようにＧＰＳ受信アンテナを設置し、更新されたアライメント角と推定されたアライメント角とをアライメント推定演算にフィードバックすることで、アライメント角推定演算のアルゴリズムを簡略化することができ、アライメント角の推定速度が速くなる。
【００２５】
また、この発明は、センサ誤差加算部と慣性データ補正部とを備え、慣性センサから出力された慣性データに含まれるセンサに起因するセンサ誤差を補正する。
【００２６】
アライメント角推定手段は、二つのシステムで得られた慣性データの差分値からセンサ誤差を推定して、センサ誤差加算手段に出力する。センサ誤差加算手段では、センサ誤差が、前記アライメント角と同様に、推定周期毎に順次累積加算され更新される。この更新センサ誤差は慣性データ補正部に出力され、慣性データ補正部では、更新センサ誤差を適用して、次の時点での慣性データを補正する。
【００２７】
また、この発明は、上述のアライメント角推定演算を行う前、目測等の方法でアライメント角を概略推定しておき、この推定アライメント角を用いて、アライメント推定手段で用いる変換マトリクスの初期値を設定して、アライメント角推定演算を行うことも可能である。
【００２８】
また、この発明は、アライメント角が上記のような大きい値である時も、姿勢の初期値を設定することなく、アライメント角推定演算が正しいアライメント角を推定するまで行われることで、確実にアライメント角が推定される。
【００２９】
【発明の実施の形態】
本発明の第１の実施形態に係る移動体の姿勢検出装置について、図１、図２を参照して説明する。
図１はアンテナ座標系とＩＭＵ座標系の相関関係を示した図である。
図２は姿勢検出装置のアライメント角推定フローを示したブロック図である。図１において、ＡＮＴ０，１，２はＧＰＳ受信アンテナ、Ｓｘ ，Ｓｙ ，Ｓｚ は慣性センサである角速度センサであり、ｘｇ ，ｙｇ ，ｚｇ はそれぞれアンテナ座標系、ｘｉ ，ｙｉ ，ｚｉ はＩＭＵ座標系である。また、Ｃｇ ｉ はＩＭＵ座標系からアンテナ座標系に変換する変換マトリックスである。
【００３０】
図２のおいて、１０１はＩＭＵ姿勢検出システムのＩＭＵ角速度演算部、１０２は慣性データ補正部、１０３は慣性データ変換部、１０４はＧＰＳ姿勢検出システムのＧＰＳ姿勢演算部、１０５はＧＰＳ姿勢検出システムのＧＰＳ角速度演算部、１０６はアライメント角推定部、１０７はアライメント角加算部、１０８はセンサ誤差加算部である。本発明におけるアライメント角推定手段とは、慣性データ補正部１０２、慣性データ変換部１０３、およびアライメント角推定部１０６に対応する。
【００３１】
図１に示すように、アンテナ座標系には、３個のＧＰＳ受信アンテナＡＮＴ０，ＡＮＴ１，ＡＮＴ２が全て一直線上に並ばないように配置されている。ここで、アンテナＡＮＴ０をアンテナ座標系の原点に置き、他のアンテナＡＮＴ１，２の座標を（ｘ１ ，ｙ１ ，ｚ１ ），（ｘ２ ，ｙ２ ，ｚ２ ）とする。また、ＩＭＵ座標系の各基準軸には、それぞれ角速度センサＳｘ ，Ｓｙ ，Ｓｚ が配置されている。
【００３２】
図１において、ＩＭＵ座標系に対してアンテナ座標系が所定の角度で座標回転しており、θｘ ，θｙ ，θｚ を、座標回転順序を仮にｚ，ｙ，ｘとしたときのオイラー角とすると、その変換マトリックスＣｇ ｉ は、次式で表される。
【００３３】
【数１】
【００３４】
ここで、θｘ ，θｙ ，θｚ はアライメント角であり、このアライメント角を演算して推定することにより、アンテナ座標系とＩＭＵ座標系を統合することができる。
【００３５】
次に、図２を用いて、前記アライメント角の推定方法について説明する。
本説明では、慣性センサとして角速度センサを用い、本発明の慣性データに対応するものとしてＩＭＵ角速度を用いて説明する。
【００３６】
ＩＭＵ角速度演算部１０１には、図１に示した直交３軸座標系にそれぞれ配置された３個の角速度センサが備えられており、該角速度センサでＩＭＵ座標系のＩＭＵ角速度ωｉｍを出力する。このＩＭＵ角速度ωｉｍに基づいて、ＩＭＵ姿勢角演算部（図示せず）で既知の方法を用いてＩＭＵ姿勢を検出する。
【００３７】
角速度センサには内部誤差として、バイアス誤差Δωｉ およびスケールファクター誤差ΔＫｓ が含まれている。このため、ＩＭＵ角速度ωｉｍはその真値ωｉ として、
【００３８】
【数２】
【００３９】
と表すことができる。ここで、誤差の２次以上の項は値が小さく、無視できるものとすると、
【００４０】
【数３】
【００４１】
と表すことができる。
【００４２】
アンテナ座標系に対するＩＭＵ座標系のアライメント角をΔθｉ ｇｉとすると、このアライメント角に対する変換マトリックスＣ’ｇ ｉ は次式で表すことができる。
【００４３】
【数４】
【００４４】
ここで、Ｃｇ ｉ は図１および（１）式に示した真の変換マトリックスであり、Δθｉ ｇｉはベクトル量でありその内成分は（Δθｘ ，Δθｙ ，Δθｚ ）である。また、Ｓ（Δθｉ ｇｉ）はアライメント角Δθｉ ｇｉの交代行列であり、
【００４５】
【数５】
【００４６】
で表される。
【００４７】
慣性データ変換部１０３では、（１）式を用いて、ＩＭＵ座標系のＩＭＵ角速度ωｉｍをアンテナ座標系に変換する。
【００４８】
ＩＭＵ角速度ωｉｍをアンテナ座標系に変換したＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ は、（２’）式、（３）式より、誤差の２次以上の項を無視すると、
【００４９】
【数６】
【００５０】
と表される。
【００５１】
一方、ＧＰＳ姿勢演算部１０４では、ＧＰＳ衛星からの電波信号を図１に示した、ＧＰＳ受信アンテナＡＮＴ０，ＡＮＴ１，ＡＮＴ２で受信し、ＧＰＳ姿勢演算部１０４で既知の方法にてＧＰＳ姿勢を出力する。ＧＰＳ角速度演算部１０５では、このＧＰＳ姿勢を用いてＧＰＳ角速度ωｇｍを出力する。ここで、実際に観測されるＧＰＳ角速度ωｇｍは誤差Δωｇ を含んでいるので、ＧＰＳ角速度の真値をωｇ とすると、
【００５２】
【数７】
【００５３】
と表される。
【００５４】
ここで、ＧＰＳ角速度の真値ωｇ とＩＭＵ角速度の真値ωｉ とは、
【００５５】
【数８】
【００５６】
の関係にある。
【００５７】
（５）式、（６）式、（７）式より、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ とＧＰＳ角速度ωｇｍとの差Δｚは次式で表される。
【００５８】
【数９】
【００５９】
【数１０】
【００６０】
【数１１】
【００６１】
ここで、Δθｉ ｇｉｘ ，Δθｉ ｇｉｙ ，Δθｉ ｇｉｚ はそれぞれアライメント角、Δω’ｘ ，Δω’ｙ ，Δω’ｚ はそれぞれ角速度センサにおけるｘ，ｙ，ｚ軸の角速度の推定バイアス誤差、ΔＫ’ｓｘ，ΔＫ’ｓｙ，ΔＫ’ｓｚはそれぞれ角速度センサにおけるｘ，ｙ，ｚ軸の角速度の推定スケールファクタ誤差、νはΔｚの観測誤差を表す。
【００６２】
なお、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ とＧＰＳ角速度ωｇｍは、それぞれ周期Ｔｇで同期するようにサンプリングされており、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ とＧＰＳ角速度ωｇｍとは、それぞれ角速度センサとＧＰＳ姿勢演算部とでの観測された時刻が同じものであるように時刻同期の処理が行われているものとする。
【００６３】
アライメント角推定部１０６では、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ とＧＰＳ角速度ωｇｍとの差分値Δｚを入力し、（１０）式の状態変数を推定する。
【００６４】
例えば、サンプリング周期Ｔｇを用いて、次式に示すようなカルマンフィルタで、周期Ｔｇ毎に順次、前記各状態変数を推定する。
【００６５】
【数１２】
【００６６】
ここで、Φは状態遷移マトリクスであり、
ｗｋ ＝（０，０，０，ηｘ ，ηｙ ，ηｚ ，０，０，０）Ｔ は観測雑音である。
【００６７】
カルマンフィルタは、所定の周期で、推定誤差の平均２乗誤差を最小とするように、前回の観測値から次回の推定値を演算し、この演算を繰り返し行うことにより、目的値を導き出す手法である。
【００６８】
ある時点での推定バイアス誤差Δω’ｉ をδΔω’ｉ とし、推定スケールファクタ誤差ΔＫ’ｓ をδΔＫ’ｓ とすると、センサ誤差加算部１０８では、推定バイアス誤差δΔω’ｉ と推定スケールファクタ誤差δΔＫ’ｓ とを入力し、次式のように前時点での推定バイアス誤差Δω’ｉ と推定スケールファクタ誤差ΔＫ’ｓ にそれぞれ加算される。
【００６９】
【数１３】
【００７０】
この演算は、周期ＴｇでδΔω’ｉ ，δΔＫ’ｓ が推定される毎に行われ、推定バイアス誤差Δω’ｉ と推定スケールファクタ誤差ΔＫ’ｓ とを順次累積加算し、更新していく。
【００７１】
更新された推定バイアス誤差Δω’ｉ と推定スケールファクタ誤差ΔＫ’ｓ は、慣性データ補正部１０２に出力され、慣性データ補正部１０２では、更新された推定バイアス誤差Δω’ｉ と推定スケールファクタ誤差ΔＫ’ｓ を用いて、次の時点のＩＭＵ角速度ωｉｍを補正する。
【００７２】
このように、推定バイアス誤差Δω’ｉ と推定スケールファクタ誤差ΔＫ’ｓ とをフィードバックしていくことにより、ある時点で、アライメント角推定部１０６で推定されるセンサ誤差δΔω’ｉ ，δΔＫ’ｓ は、前時点の推定値で補正されたＩＭＵ角速度と、その時点でのＧＰＳ角速度とから求められる。これにより、アライメント角推定部１０６で推定されるセンサ誤差は、更新される度に徐々に小さい値となっていき、０に近づく。一方、センサ誤差加算部１０８では、このように時系列に推定されたセンサ誤差を加算していき、徐々にセンサ誤差の真値となるように収束していく。
【００７３】
これにより、推定を繰り返す毎にセンサ誤差は真値に近づき、これを用いてＩＭＵ角速度を補正することで、ＩＭＵ角速度におけるセンサ誤差の影響を排除していく。
【００７４】
アライメント角加算部１０７は、アライメント角推定部１０６で推定された推定アライメント角Δθｉ ｇｉを周期Ｔｇ毎に累積加算していき、更新アライメント角θｉ ｇｉを生成する。
【００７５】
【数１４】
【００７６】
更新アライメント角θｉ ｇｉは、慣性データ変換部１０３に出力され、慣性データ変換部１０３はこの更新アライメント角θｉ ｇｉに基づいて、（１）式に示した変換マトリクスを順次演算して更新する。
【００７７】
このように、更新アライメント角θｉ ｇｉをフィードバックしていくことにより、ある時点での推定アライメント角Δθｉ ｇｉは、前時点での更新アライメント角θｉ ｇｉを用いて座標変換されたＩＭＵ角速度と、その時点でのＧＰＳ角速度との差分から求められる。これにより、アライメント角推定部１０６で推定される推定アライメント角Δθｉ ｇｉは徐々に小さくなり、０に近づくように収束し、更新されるアライメント角θｉ ｇｉは徐々にアライメント角の真値となるように収束していく。
【００７８】
図３に、アライメント角推定シミュレーションの結果を示す。なお、このシミュレーションに示すアライメント角とは図２に示すθｉ ｇｉを意味する。
アンテナ座標系とＩＭＵ座標系とのアライメント角は各成分をＲｏｌｌ角、Ｐｉｔｃｈ角、Ｙａｗ角として、それぞれ−３０°，５０°，１００°であり、推定の初期値は全て０°で、白色雑音が重畳された状態として推定を行った。また、推定を行うためのＩＭＵユニットの揺動条件として、Ｒｏｌｌ角、Ｐｉｔｃｈ角、Ｙａｗ角の振幅および周期を表１に示すように設定した。
【００７９】
【表１】
【００８０】
図３に示すように、推定初期は白色雑音の影響等で各アライメント角成分は振動するが、徐々に振幅が小さくなるとともに真値に収束する。
【００８１】
図４は、振揺装置に設置した角速度センサユニットとＧＰＳ受信アンテナユニットを用いて、それぞれのユニットから得られる角速度に基づいて、アライメント角を推定した実験結果を示す。本推定実験では、Ｒｏｌｌ角のみが−９０°、Ｙａｗ角、Ｐｉｔｃｈ角がそれぞれ０°となるように二つの座標系を設置し、Ｙａｗ角およびＰｉｔｃｈ角方向の動作開始時刻を１８０ｓｅｃ．、Ｒｏｌｌ角方向の動作開始時刻を５００ｓｅｃ．とした。
【００８２】
図４に示すように、アライメント角の各成分は、１°程度の誤差範囲内に収束する。
このように、上述のアライメント角推定演算を用いることで、アライメント角が大きくても、確実に推定することができる。
【００８３】
これにより、アンテナ座標系とＩＭＵ座標系とのアライメント角が高精度に推定できるため、ＧＰＳ姿勢検出システムで検出された姿勢とＩＭＵ姿勢検出システムで検出された姿勢とを高精度に統合することができる。これにより、外部環境に影響されることなく、常時高精度の姿勢を検出することができる。
【００８４】
なお、慣性データ補正部１０２、慣性データ変換部１０３、アライメント角加算部１０７、センサ誤差加算部１０８における各状態変数の初期値はすべて０とし、変換マトリクスＣｇ ｉ の初期値は単位マトリクスとする。
【００８５】
また、本実施形態では、カルマンフィルタを用いて各状態変数の推定を行っているが、各状態変数を算出するに必要な数の差分Δｚを記憶しておき、最小２乗法で算出することも可能である。ただし、この場合には、各状態変数の更新周期が、（状態変数の算出に必要な差分回数）×（サンプリング周期Ｔｇ）となる。
【００８６】
また、本実施形態では、センサ誤差およびスケールファクタ誤差を考慮して推定を行っているが、高精度の角速度センサを使用したり、アライメント角の要求精度によっては、これらの状態変数を削除してアライメント角を推定することもできる。
【００８７】
次に、第２の実施形態に係る移動体の姿勢検出装置について図５を参照して説明する。
図５は姿勢検出装置のアライメント角推定フローを示したブロック図である。
【００８８】
図５に示す回路は、図２に示した回路を同じ要素で構成されているが、慣性データ変換部１０３で、ＩＭＵ角速度ωｉｍをアンテナ座標系に変換する演算が異なるものである。慣性データ変換部１０３には更新されたアライメント角θｉ ｇｉとともに、推定されたアライメント角Δθｉ ｇｉがフィードバックされる。
【００８９】
（３）式を基づいて（８）式を用いるためには変換マトリクスＣｇ ｉ が既知である必要があるが、実際には既知ではないので変換マトリクスＣｇ ｉ を単位マトリクスで近似せざるを得ない。そのためには、変換マトリクスＣｇ ｉ の各要素となるアライメント角の各成分を、（θｘ ，θｙ ，θｚ ）とし、
【００９０】
【数１５】
【００９１】
となるようにすればよい。このような範囲でアンテナ座標系とＩＭＵ座標系とを設置することは、目視で容易に実現できる。
【００９２】
そして、このような範囲のアライメント角であっても、（８）式、（９）式に示すＣｇ ｉ を単位マトリクスに近似することができ、（８）式、（９）式、はそれぞれで次式で表すことができる。
【００９３】
【数１６】
【００９４】
【数１７】
【００９５】
この演算式において、上述の（１４）式を適用すると、真の変換マトリクスＣｇ ｉ の各要素と（３）式においてＣｇ ｉ を単位マトリクスとして計算した変換マトリクスＣ’ｇ ｉ の各要素とで符号が異なるのは、全ての要素のうち多くとも１要素となる。
【００９６】
このような変換マトリクスＣ’ｇ ｉ で変換を行った場合、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ の大きさは変化するが、極性が変化するのは、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ の３成分（ω’ｇ ｉｘ，ω’ｇ ｉｙ，ω’ｇ ｉｚ）のうち、１成分のみである。
【００９７】
ω’ｇ ｉ の極性が反転しない２成分を持つ場合のアライメント角の推定は、収束する方向で推定アライメント角Δθｉ ｇｉを算出する。このように算出された推定アライメント角Δθｉ ｇｉを順次累積加算しながら、角速度の変換にフィードバックしていけば、変換マトリクスの極性反転した１要素も再度極性反転して、推定アライメント角Δθｉ ｇｉを収束させるような極性となる。このように要素が修正されるので、変換マトリクスＣ’ｇ ｉ は真の変換マトリクスＣｇ ｉ に置き換えることができ、正確にアライメント角を推定することができる。
【００９８】
このように、初期アライメント角を（１４）式の条件を満たすように設定することで、上述のように変換マトリクスを単位行列に近似することができるので、演算アルゴリズムを簡略化し、推定時間を短縮することができる。
【００９９】
なお、アライメント角が（１４）式を満たさないような大きな場合は、図５に示すように、慣性データ変換部１０３における変換マトリクスの初期値Ｃｇ ｉ（１）を、（１４）式の条件を満たすように設定することで、推定時間を短縮することができる。
【０１００】
また、上述の実施形態では、アライメント角の推定演算は、正しいアライメント角推定値に収束するまで行われる。これにより、確実にアライメント角を推定することができる。
【０１０１】
【発明の効果】
この発明によれば、推定アライメント角を、推定を行う周期で順次累積加算、更新していき、アライメント角推定演算にフィードバックする。これにより、推定を行う周期毎に、その前時点に更新されたアライメント角で変換を行った慣性データから新たなアライメント角を推定していくので、周期毎に推定されるアライメント角は徐々に０へ収束していき、累積加算される更新アライメント角はアライメント角の真値に収束する。このようなフィードバックをアライメント角推定演算に行うことで、特に初期のアライメント角の大きさに関係なく、正確なアライメント角を確実に推定することができる。
【０１０２】
また、この発明によれば、アライメント角の初期値を所定の範囲内にすることで、アライメント角推定演算のアルゴリズムを簡略化することができ、アライメント角推定時間を短縮することができる。
【０１０３】
また、この発明によれば、センサ誤差についても推定、累積加算してフィードバックすることにより、推定アライメント角に含まれるセンサに起因する誤差を排除することができる。これにより、各周期での推定アライメント角を累積加算してなる更新アライメント角をさらに高精度に真値に近づけることができる。
【０１０４】
また、この発明によれば、アライメント角推定演算を行う前に、予め、目測等でアライメント角の初期条件を設定しておくことにより、アライメント角推定演算でのアライメント角推定を確実にするとともに、更に短時間でアライメント角を推定することができる。
【０１０５】
また、この発明によれば、アライメント角推定演算は正しいアライメント角推定値に収束するまで行われるので、確実にアライメント角を推定することができる。
【図面の簡単な説明】
【図１】アンテナ座標系とＩＭＵ座標系との関係を示した図
【図２】第１の実施形態に係るアライメント角推定演算フローを示したブロック図
【図３】アライメント角推定シミュレーション結果を示した図
【図４】実船によるアライメント角推定実験の結果を示した図
【図５】第２の実施形態に係るアライメント角推定演算フローを示したブロック図
【符号の説明】
１０１−ＩＭＵ角速度演算部
１０２−慣性データ補正部
１０３−慣性データ変換部
１０４−ＧＰＳ姿勢演算部
１０５−ＧＰＳ角速度演算部
１０６−アライメント角推定部
１０７−アライメント角加算部
１０８−センサ誤差加算部
ＡＮＴ１，ＡＮＴ２，ＡＮＴ３−ＧＰＳ受信アンテナ
Ｓｘ ，Ｓｙ ，Ｓｚ −角速度センサ
【発明の属する技術分野】
この発明は、ＧＰＳ姿勢とＩＭＵ姿勢とを統合して移動体の姿勢を検出する、ＧＰＳ／ＩＭＵ統合化姿勢検出装置、特にアンテナ座標系のＧＰＳ姿勢とＩＭＵ座標系のＩＭＵ姿勢とのアライメント角を確実に推定するＧＰＳ／ＩＭＵ統合化姿勢検出装置に関するものである。
【０００２】
【従来の技術】
移動体の方位および姿勢を検出するシステムの一つとして、ＧＰＳ姿勢検出システムがある。このシステムでは、全てが一直線上に並ばない少なくとも３個のＧＰＳ受信アンテナを移動体に装着し、これら直交３軸座標系で定義されたＧＰＳ受信アンテナのそれぞれがＧＰＳ衛星からの電波信号を受信して、アンテナ間のキャリア位相差を観測する。この観測値からそれぞれのアンテナの相対位置を計測することにより、アンテナ座標系を構成し、座標系の基準座標に対する方位、姿勢を計測する。
【０００３】
しかし、このようなＧＰＳ姿勢検出システムでは、ＧＳＰ衛星からの電波信号を受信することにより姿勢検出が可能であるが、ＧＰＳ衛星からの電波信号が遮断されたり、キャリア位相サイクルスリップ等が生じた場合、キャリア位相差を観測できなくなり、姿勢検出できなくなってしまうという問題がある。
【０００４】
この問題を解決する方法として、角速度センサや加速度センサ等の慣性センサ（ＩＭＵ）を移動体に取り付け、移動体の動きを観測して、この観測データから得られる姿勢とＧＰＳ姿勢検出システムにより得られる姿勢との座標系回転量（アライメント角）を推定して統合する、ＧＰＳ／ＩＭＵ統合化技術がある。
【０００５】
この方法は、直交３軸座標系の各軸にそれぞれマウントされた慣性センサから既知の方法で得られる慣性センサ座標系（以下、「ＩＭＵ座標系」という）の姿勢と、ＧＰＳ姿勢検出システムから得られるアンテナ座標系の姿勢とのアライメント角を推定して統合することにより、安定して高精度に姿勢を得ることができる。この方法を用いることにより、ＧＰＳ衛星からの電波信号が中断しても、中断時の移動体の動きを慣性センサで観測することができるので、電波信号中断中は慣性センサによる姿勢でＧＰＳによる姿勢の欠落を補間することができ、移動体の姿勢を常時出力することができる（例えば、特許文献１参照。）。
【０００６】
【特許文献１】
特開２００２−５４９４６号
【０００７】
【発明が解決しようとする課題】
ところが、このような従来の移動体の姿勢検出装置では、次に示す解決すべき課題が存在した。
【０００８】
移動体の姿勢検出装置におけるＧＰＳ／ＩＭＵ統合技術では、アンテナおよび慣性センサを取り付ける際に、それぞれの座標系であるアンテナ座標系とＩＭＵ座標系との座標系回転量（アライメント角）を求めなければならない。
【０００９】
従来のアライメント角推定技法として、次のいずれかの方法が用いられている。
第１の方法としては、複数の慣性センサからなるＩＭＵ座標系ユニットの一基準方向と複数のＧＰＳ受信アンテナからなるアンテナ座標系の一基準方向とが一致するように、目視等で確認しながら、複数のＧＰＳ受信アンテナを取り付ける。その後、この取り付けで生じたであろう両座標系間のアライメント角を無視し、両座標系が一致したものとみなす。
【００１０】
この方法では、数度程度のアライメント誤差が頻繁に発生するため、ＧＰＳ／ＩＭＵ統合技術を用いても、姿勢精度が低く、不安定であった。
【００１１】
第２の方法としては、第１の方法に示したアライメント角設定の後、次に示すアライメント角推定方法でアライメント角を推定するものである。
【００１２】
一例として、ＩＭＵセンサとして角速度センサを用いた場合を説明する。
ＧＰＳ姿勢検出システムで得られた姿勢から角速度（以下「ＧＰＳ角速度」という。）ωｇ１を演算し、これと同じ時点で角速度センサにより角速度（以下「ＩＭＵ角速度」という。）ωｉ１を得る。これらＧＰＳ角速度ωｇ１とＩＭＵ角速度ωｉ１とを差分することにより、差分値Δｚ１ を求める。この差分値Δｚ１ に基づいてアライメント角θｉ１を推定し、このアライメント角θｉ１を用いて次の時点のＩＭＵ角速度ωｉ２を補正する。次に、このＩＭＵ角速度ωｉ２と同時点のＧＰＳ角速度ωｇ２との差分をとり、新たな差分値Δｚ２ を求め、この差分値Δｚ２ から新たなアライメント角θｉ２を推定し、これをさらに次の時点のＩＭＵ角速度の補正に用いる。このような演算を繰り返し行うことにより、アライメント角θｉ を収束させて、真のアライメント角を算出する。
【００１３】
この方法では、算出されるアライメント角が数度程度の微小角でなければ、非線形のため収束しないので、初期条件として、アライメント角が微小角であることが必要となってしまう。
【００１４】
ところで、例えば、ユーザがＧＰＳ受信アンテナを任意に現場で配置する場合には、その場でＧＰＳ座標系を設定し、これをＩＭＵ座標系に合うようにとりつけなければならない。しかし、このような作業で、微小なアライメント角となるように複数のＧＰＳ受信アンテナを設置することは非常に困難なことである。また、ＧＰＳ受信アンテナの取り付け位置からＩＭＵセンサユニットが視認できない場合には、前述の目視によるあわせ込みは不可能であるため、アライメント角を微小にすることは非常に困難である。
【００１５】
この発明の目的は、アライメント角の大きさに関係なく、アンテナ座標系とＩＭＵ座標系とのアライメント角を確実に推定することができる移動体の姿勢検出装置を提供することにある。
【００１６】
【課題を解決するための手段】
この発明は、アライメント角推定手段とアライメント角加算手段とを備えて、所定の周期毎に推定されるアライメント角を累積加算して更新しながら、アライメント角推定演算にフィードバックすることを特徴としている。
【００１７】
アライメント角推定手段は、慣性データ変換手段とアライメント角推定手段とからなり、慣性データ変換手段は、ＩＭＵ慣性センサにより計測された慣性データをＩＭＵ座標系からアンテナ座標系に変換する。
【００１８】
アライメント角推定手段は、座標変換された慣性データとＧＰＳ姿勢検出システムから演算された慣性データ（以下、「ＧＰＳ慣性データ」という。）との差分値から、所定の周期でアライメント角を推定する。
【００１９】
推定されたアライメント角は、アライメント角加算手段で、前記周期で順次累積加算され更新され、慣性データ変換手段に出力される。慣性データ変換手段では、ある時点での更新アライメント角を用いて、慣性データを座標変換する。
【００２０】
このように、本発明のアライメント角推定手段は、ある時点の差分値から推定したアライメント角を用いて、逐次、慣性データを変換し、この変換された慣性データと、変換された慣性データと同じ時点のＧＰＳ慣性データとを差分して新たなアライメント角を推定する。
【００２１】
このようなフィードバック演算を繰り返し行うことで、ある時点での推定アライメント角は、前時点で更新されたアライメント角を用いて変換された慣性データとその時点でのＧＰＳ慣性データとの差分値から推定されるので、アライメント角推定手段で推定されるアライメント値は徐々に小さく収束していく。そして、これと同時にアライメント角加算部では、更新アライメント角がアライメント角の真値に向かって徐々に収束していく。
【００２２】
このように推定と累積加算とを繰り返して得られたアライメント角を用いることにより、アンテナ座標系の姿勢とＩＭＵ座標系の姿勢とを統合して、高精度で安定した姿勢を出力する。
【００２３】
また、この発明は、アライメント角の各成分が、−８５°≦θｘ ≦８５°，−８５°≦θｙ ≦８５°，−９０°≦θｚ ≦９０°のように大きな値であるときでも、アライメント角を推定することができる。
【００２４】
このように所定の範囲にアライメント角で収まるようにＧＰＳ受信アンテナを設置し、更新されたアライメント角と推定されたアライメント角とをアライメント推定演算にフィードバックすることで、アライメント角推定演算のアルゴリズムを簡略化することができ、アライメント角の推定速度が速くなる。
【００２５】
また、この発明は、センサ誤差加算部と慣性データ補正部とを備え、慣性センサから出力された慣性データに含まれるセンサに起因するセンサ誤差を補正する。
【００２６】
アライメント角推定手段は、二つのシステムで得られた慣性データの差分値からセンサ誤差を推定して、センサ誤差加算手段に出力する。センサ誤差加算手段では、センサ誤差が、前記アライメント角と同様に、推定周期毎に順次累積加算され更新される。この更新センサ誤差は慣性データ補正部に出力され、慣性データ補正部では、更新センサ誤差を適用して、次の時点での慣性データを補正する。
【００２７】
また、この発明は、上述のアライメント角推定演算を行う前、目測等の方法でアライメント角を概略推定しておき、この推定アライメント角を用いて、アライメント推定手段で用いる変換マトリクスの初期値を設定して、アライメント角推定演算を行うことも可能である。
【００２８】
また、この発明は、アライメント角が上記のような大きい値である時も、姿勢の初期値を設定することなく、アライメント角推定演算が正しいアライメント角を推定するまで行われることで、確実にアライメント角が推定される。
【００２９】
【発明の実施の形態】
本発明の第１の実施形態に係る移動体の姿勢検出装置について、図１、図２を参照して説明する。
図１はアンテナ座標系とＩＭＵ座標系の相関関係を示した図である。
図２は姿勢検出装置のアライメント角推定フローを示したブロック図である。図１において、ＡＮＴ０，１，２はＧＰＳ受信アンテナ、Ｓｘ ，Ｓｙ ，Ｓｚ は慣性センサである角速度センサであり、ｘｇ ，ｙｇ ，ｚｇ はそれぞれアンテナ座標系、ｘｉ ，ｙｉ ，ｚｉ はＩＭＵ座標系である。また、Ｃｇ ｉ はＩＭＵ座標系からアンテナ座標系に変換する変換マトリックスである。
【００３０】
図２のおいて、１０１はＩＭＵ姿勢検出システムのＩＭＵ角速度演算部、１０２は慣性データ補正部、１０３は慣性データ変換部、１０４はＧＰＳ姿勢検出システムのＧＰＳ姿勢演算部、１０５はＧＰＳ姿勢検出システムのＧＰＳ角速度演算部、１０６はアライメント角推定部、１０７はアライメント角加算部、１０８はセンサ誤差加算部である。本発明におけるアライメント角推定手段とは、慣性データ補正部１０２、慣性データ変換部１０３、およびアライメント角推定部１０６に対応する。
【００３１】
図１に示すように、アンテナ座標系には、３個のＧＰＳ受信アンテナＡＮＴ０，ＡＮＴ１，ＡＮＴ２が全て一直線上に並ばないように配置されている。ここで、アンテナＡＮＴ０をアンテナ座標系の原点に置き、他のアンテナＡＮＴ１，２の座標を（ｘ１ ，ｙ１ ，ｚ１ ），（ｘ２ ，ｙ２ ，ｚ２ ）とする。また、ＩＭＵ座標系の各基準軸には、それぞれ角速度センサＳｘ ，Ｓｙ ，Ｓｚ が配置されている。
【００３２】
図１において、ＩＭＵ座標系に対してアンテナ座標系が所定の角度で座標回転しており、θｘ ，θｙ ，θｚ を、座標回転順序を仮にｚ，ｙ，ｘとしたときのオイラー角とすると、その変換マトリックスＣｇ ｉ は、次式で表される。
【００３３】
【数１】
ここで、θｘ ，θｙ ，θｚ はアライメント角であり、このアライメント角を演算して推定することにより、アンテナ座標系とＩＭＵ座標系を統合することができる。
【００３５】
次に、図２を用いて、前記アライメント角の推定方法について説明する。
本説明では、慣性センサとして角速度センサを用い、本発明の慣性データに対応するものとしてＩＭＵ角速度を用いて説明する。
【００３６】
ＩＭＵ角速度演算部１０１には、図１に示した直交３軸座標系にそれぞれ配置された３個の角速度センサが備えられており、該角速度センサでＩＭＵ座標系のＩＭＵ角速度ωｉｍを出力する。このＩＭＵ角速度ωｉｍに基づいて、ＩＭＵ姿勢角演算部（図示せず）で既知の方法を用いてＩＭＵ姿勢を検出する。
【００３７】
角速度センサには内部誤差として、バイアス誤差Δωｉ およびスケールファクター誤差ΔＫｓ が含まれている。このため、ＩＭＵ角速度ωｉｍはその真値ωｉ として、
【００３８】
【数２】
と表すことができる。ここで、誤差の２次以上の項は値が小さく、無視できるものとすると、
【００４０】
【数３】
と表すことができる。
【００４２】
アンテナ座標系に対するＩＭＵ座標系のアライメント角をΔθｉ ｇｉとすると、このアライメント角に対する変換マトリックスＣ’ｇ ｉ は次式で表すことができる。
【００４３】
【数４】
ここで、Ｃｇ ｉ は図１および（１）式に示した真の変換マトリックスであり、Δθｉ ｇｉはベクトル量でありその内成分は（Δθｘ ，Δθｙ ，Δθｚ ）である。また、Ｓ（Δθｉ ｇｉ）はアライメント角Δθｉ ｇｉの交代行列であり、
【００４５】
【数５】
で表される。
【００４７】
慣性データ変換部１０３では、（１）式を用いて、ＩＭＵ座標系のＩＭＵ角速度ωｉｍをアンテナ座標系に変換する。
【００４８】
ＩＭＵ角速度ωｉｍをアンテナ座標系に変換したＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ は、（２’）式、（３）式より、誤差の２次以上の項を無視すると、
【００４９】
【数６】
と表される。
【００５１】
一方、ＧＰＳ姿勢演算部１０４では、ＧＰＳ衛星からの電波信号を図１に示した、ＧＰＳ受信アンテナＡＮＴ０，ＡＮＴ１，ＡＮＴ２で受信し、ＧＰＳ姿勢演算部１０４で既知の方法にてＧＰＳ姿勢を出力する。ＧＰＳ角速度演算部１０５では、このＧＰＳ姿勢を用いてＧＰＳ角速度ωｇｍを出力する。ここで、実際に観測されるＧＰＳ角速度ωｇｍは誤差Δωｇ を含んでいるので、ＧＰＳ角速度の真値をωｇ とすると、
【００５２】
【数７】
と表される。
【００５４】
ここで、ＧＰＳ角速度の真値ωｇ とＩＭＵ角速度の真値ωｉ とは、
【００５５】
【数８】
の関係にある。
【００５７】
（５）式、（６）式、（７）式より、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ とＧＰＳ角速度ωｇｍとの差Δｚは次式で表される。
【００５８】
【数９】
【数１０】
【数１１】
ここで、Δθｉ ｇｉｘ ，Δθｉ ｇｉｙ ，Δθｉ ｇｉｚ はそれぞれアライメント角、Δω’ｘ ，Δω’ｙ ，Δω’ｚ はそれぞれ角速度センサにおけるｘ，ｙ，ｚ軸の角速度の推定バイアス誤差、ΔＫ’ｓｘ，ΔＫ’ｓｙ，ΔＫ’ｓｚはそれぞれ角速度センサにおけるｘ，ｙ，ｚ軸の角速度の推定スケールファクタ誤差、νはΔｚの観測誤差を表す。
【００６２】
なお、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ とＧＰＳ角速度ωｇｍは、それぞれ周期Ｔｇで同期するようにサンプリングされており、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ とＧＰＳ角速度ωｇｍとは、それぞれ角速度センサとＧＰＳ姿勢演算部とでの観測された時刻が同じものであるように時刻同期の処理が行われているものとする。
【００６３】
アライメント角推定部１０６では、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ とＧＰＳ角速度ωｇｍとの差分値Δｚを入力し、（１０）式の状態変数を推定する。
【００６４】
例えば、サンプリング周期Ｔｇを用いて、次式に示すようなカルマンフィルタで、周期Ｔｇ毎に順次、前記各状態変数を推定する。
【００６５】
【数１２】
ここで、Φは状態遷移マトリクスであり、
ｗｋ ＝（０，０，０，ηｘ ，ηｙ ，ηｚ ，０，０，０）Ｔ は観測雑音である。
【００６７】
カルマンフィルタは、所定の周期で、推定誤差の平均２乗誤差を最小とするように、前回の観測値から次回の推定値を演算し、この演算を繰り返し行うことにより、目的値を導き出す手法である。
【００６８】
ある時点での推定バイアス誤差Δω’ｉ をδΔω’ｉ とし、推定スケールファクタ誤差ΔＫ’ｓ をδΔＫ’ｓ とすると、センサ誤差加算部１０８では、推定バイアス誤差δΔω’ｉ と推定スケールファクタ誤差δΔＫ’ｓ とを入力し、次式のように前時点での推定バイアス誤差Δω’ｉ と推定スケールファクタ誤差ΔＫ’ｓ にそれぞれ加算される。
【００６９】
【数１３】
この演算は、周期ＴｇでδΔω’ｉ ，δΔＫ’ｓ が推定される毎に行われ、推定バイアス誤差Δω’ｉ と推定スケールファクタ誤差ΔＫ’ｓ とを順次累積加算し、更新していく。
【００７１】
更新された推定バイアス誤差Δω’ｉ と推定スケールファクタ誤差ΔＫ’ｓ は、慣性データ補正部１０２に出力され、慣性データ補正部１０２では、更新された推定バイアス誤差Δω’ｉ と推定スケールファクタ誤差ΔＫ’ｓ を用いて、次の時点のＩＭＵ角速度ωｉｍを補正する。
【００７２】
このように、推定バイアス誤差Δω’ｉ と推定スケールファクタ誤差ΔＫ’ｓ とをフィードバックしていくことにより、ある時点で、アライメント角推定部１０６で推定されるセンサ誤差δΔω’ｉ ，δΔＫ’ｓ は、前時点の推定値で補正されたＩＭＵ角速度と、その時点でのＧＰＳ角速度とから求められる。これにより、アライメント角推定部１０６で推定されるセンサ誤差は、更新される度に徐々に小さい値となっていき、０に近づく。一方、センサ誤差加算部１０８では、このように時系列に推定されたセンサ誤差を加算していき、徐々にセンサ誤差の真値となるように収束していく。
【００７３】
これにより、推定を繰り返す毎にセンサ誤差は真値に近づき、これを用いてＩＭＵ角速度を補正することで、ＩＭＵ角速度におけるセンサ誤差の影響を排除していく。
【００７４】
アライメント角加算部１０７は、アライメント角推定部１０６で推定された推定アライメント角Δθｉ ｇｉを周期Ｔｇ毎に累積加算していき、更新アライメント角θｉ ｇｉを生成する。
【００７５】
【数１４】
更新アライメント角θｉ ｇｉは、慣性データ変換部１０３に出力され、慣性データ変換部１０３はこの更新アライメント角θｉ ｇｉに基づいて、（１）式に示した変換マトリクスを順次演算して更新する。
【００７７】
このように、更新アライメント角θｉ ｇｉをフィードバックしていくことにより、ある時点での推定アライメント角Δθｉ ｇｉは、前時点での更新アライメント角θｉ ｇｉを用いて座標変換されたＩＭＵ角速度と、その時点でのＧＰＳ角速度との差分から求められる。これにより、アライメント角推定部１０６で推定される推定アライメント角Δθｉ ｇｉは徐々に小さくなり、０に近づくように収束し、更新されるアライメント角θｉ ｇｉは徐々にアライメント角の真値となるように収束していく。
【００７８】
図３に、アライメント角推定シミュレーションの結果を示す。なお、このシミュレーションに示すアライメント角とは図２に示すθｉ ｇｉを意味する。
アンテナ座標系とＩＭＵ座標系とのアライメント角は各成分をＲｏｌｌ角、Ｐｉｔｃｈ角、Ｙａｗ角として、それぞれ−３０°，５０°，１００°であり、推定の初期値は全て０°で、白色雑音が重畳された状態として推定を行った。また、推定を行うためのＩＭＵユニットの揺動条件として、Ｒｏｌｌ角、Ｐｉｔｃｈ角、Ｙａｗ角の振幅および周期を表１に示すように設定した。
【００７９】
【表１】
図３に示すように、推定初期は白色雑音の影響等で各アライメント角成分は振動するが、徐々に振幅が小さくなるとともに真値に収束する。
【００８１】
図４は、振揺装置に設置した角速度センサユニットとＧＰＳ受信アンテナユニットを用いて、それぞれのユニットから得られる角速度に基づいて、アライメント角を推定した実験結果を示す。本推定実験では、Ｒｏｌｌ角のみが−９０°、Ｙａｗ角、Ｐｉｔｃｈ角がそれぞれ０°となるように二つの座標系を設置し、Ｙａｗ角およびＰｉｔｃｈ角方向の動作開始時刻を１８０ｓｅｃ．、Ｒｏｌｌ角方向の動作開始時刻を５００ｓｅｃ．とした。
【００８２】
図４に示すように、アライメント角の各成分は、１°程度の誤差範囲内に収束する。
このように、上述のアライメント角推定演算を用いることで、アライメント角が大きくても、確実に推定することができる。
【００８３】
これにより、アンテナ座標系とＩＭＵ座標系とのアライメント角が高精度に推定できるため、ＧＰＳ姿勢検出システムで検出された姿勢とＩＭＵ姿勢検出システムで検出された姿勢とを高精度に統合することができる。これにより、外部環境に影響されることなく、常時高精度の姿勢を検出することができる。
【００８４】
なお、慣性データ補正部１０２、慣性データ変換部１０３、アライメント角加算部１０７、センサ誤差加算部１０８における各状態変数の初期値はすべて０とし、変換マトリクスＣｇ ｉ の初期値は単位マトリクスとする。
【００８５】
また、本実施形態では、カルマンフィルタを用いて各状態変数の推定を行っているが、各状態変数を算出するに必要な数の差分Δｚを記憶しておき、最小２乗法で算出することも可能である。ただし、この場合には、各状態変数の更新周期が、（状態変数の算出に必要な差分回数）×（サンプリング周期Ｔｇ）となる。
【００８６】
また、本実施形態では、センサ誤差およびスケールファクタ誤差を考慮して推定を行っているが、高精度の角速度センサを使用したり、アライメント角の要求精度によっては、これらの状態変数を削除してアライメント角を推定することもできる。
【００８７】
次に、第２の実施形態に係る移動体の姿勢検出装置について図５を参照して説明する。
図５は姿勢検出装置のアライメント角推定フローを示したブロック図である。
【００８８】
図５に示す回路は、図２に示した回路を同じ要素で構成されているが、慣性データ変換部１０３で、ＩＭＵ角速度ωｉｍをアンテナ座標系に変換する演算が異なるものである。慣性データ変換部１０３には更新されたアライメント角θｉ ｇｉとともに、推定されたアライメント角Δθｉ ｇｉがフィードバックされる。
【００８９】
（３）式を基づいて（８）式を用いるためには変換マトリクスＣｇ ｉ が既知である必要があるが、実際には既知ではないので変換マトリクスＣｇ ｉ を単位マトリクスで近似せざるを得ない。そのためには、変換マトリクスＣｇ ｉ の各要素となるアライメント角の各成分を、（θｘ ，θｙ ，θｚ ）とし、
【００９０】
【数１５】
となるようにすればよい。このような範囲でアンテナ座標系とＩＭＵ座標系とを設置することは、目視で容易に実現できる。
【００９２】
そして、このような範囲のアライメント角であっても、（８）式、（９）式に示すＣｇ ｉ を単位マトリクスに近似することができ、（８）式、（９）式、はそれぞれで次式で表すことができる。
【００９３】
【数１６】
【数１７】
この演算式において、上述の（１４）式を適用すると、真の変換マトリクスＣｇ ｉ の各要素と（３）式においてＣｇ ｉ を単位マトリクスとして計算した変換マトリクスＣ’ｇ ｉ の各要素とで符号が異なるのは、全ての要素のうち多くとも１要素となる。
【００９６】
このような変換マトリクスＣ’ｇ ｉ で変換を行った場合、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ の大きさは変化するが、極性が変化するのは、ＩＭＵ／ＧＰＳ角速度ω’ｇ ｉ の３成分（ω’ｇ ｉｘ，ω’ｇ ｉｙ，ω’ｇ ｉｚ）のうち、１成分のみである。
【００９７】
ω’ｇ ｉ の極性が反転しない２成分を持つ場合のアライメント角の推定は、収束する方向で推定アライメント角Δθｉ ｇｉを算出する。このように算出された推定アライメント角Δθｉ ｇｉを順次累積加算しながら、角速度の変換にフィードバックしていけば、変換マトリクスの極性反転した１要素も再度極性反転して、推定アライメント角Δθｉ ｇｉを収束させるような極性となる。このように要素が修正されるので、変換マトリクスＣ’ｇ ｉ は真の変換マトリクスＣｇ ｉ に置き換えることができ、正確にアライメント角を推定することができる。
【００９８】
このように、初期アライメント角を（１４）式の条件を満たすように設定することで、上述のように変換マトリクスを単位行列に近似することができるので、演算アルゴリズムを簡略化し、推定時間を短縮することができる。
【００９９】
なお、アライメント角が（１４）式を満たさないような大きな場合は、図５に示すように、慣性データ変換部１０３における変換マトリクスの初期値Ｃｇ ｉ（１）を、（１４）式の条件を満たすように設定することで、推定時間を短縮することができる。
【０１００】
また、上述の実施形態では、アライメント角の推定演算は、正しいアライメント角推定値に収束するまで行われる。これにより、確実にアライメント角を推定することができる。
【０１０１】
【発明の効果】
この発明によれば、推定アライメント角を、推定を行う周期で順次累積加算、更新していき、アライメント角推定演算にフィードバックする。これにより、推定を行う周期毎に、その前時点に更新されたアライメント角で変換を行った慣性データから新たなアライメント角を推定していくので、周期毎に推定されるアライメント角は徐々に０へ収束していき、累積加算される更新アライメント角はアライメント角の真値に収束する。このようなフィードバックをアライメント角推定演算に行うことで、特に初期のアライメント角の大きさに関係なく、正確なアライメント角を確実に推定することができる。
【０１０２】
また、この発明によれば、アライメント角の初期値を所定の範囲内にすることで、アライメント角推定演算のアルゴリズムを簡略化することができ、アライメント角推定時間を短縮することができる。
【０１０３】
また、この発明によれば、センサ誤差についても推定、累積加算してフィードバックすることにより、推定アライメント角に含まれるセンサに起因する誤差を排除することができる。これにより、各周期での推定アライメント角を累積加算してなる更新アライメント角をさらに高精度に真値に近づけることができる。
【０１０４】
また、この発明によれば、アライメント角推定演算を行う前に、予め、目測等でアライメント角の初期条件を設定しておくことにより、アライメント角推定演算でのアライメント角推定を確実にするとともに、更に短時間でアライメント角を推定することができる。
【０１０５】
また、この発明によれば、アライメント角推定演算は正しいアライメント角推定値に収束するまで行われるので、確実にアライメント角を推定することができる。
【図面の簡単な説明】
【図１】アンテナ座標系とＩＭＵ座標系との関係を示した図
【図２】第１の実施形態に係るアライメント角推定演算フローを示したブロック図
【図３】アライメント角推定シミュレーション結果を示した図
【図４】実船によるアライメント角推定実験の結果を示した図
【図５】第２の実施形態に係るアライメント角推定演算フローを示したブロック図
【符号の説明】
１０１−ＩＭＵ角速度演算部
１０２−慣性データ補正部
１０３−慣性データ変換部
１０４−ＧＰＳ姿勢演算部
１０５−ＧＰＳ角速度演算部
１０６−アライメント角推定部
１０７−アライメント角加算部
１０８−センサ誤差加算部
ＡＮＴ１，ＡＮＴ２，ＡＮＴ３−ＧＰＳ受信アンテナ
Ｓｘ ，Ｓｙ ，Ｓｚ −角速度センサ
Claims (5)
- 移動体の姿勢をアンテナ座標系で検出するＧＰＳ姿勢検出システムと、慣性センサを用いてＩＭＵ座標系で検出するＩＭＵ姿勢検出システムとからなり、二つの座標系間のアライメント角を順次推定演算することにより、各座標系で検出された前記姿勢を統合することで、前記移動体の姿勢を出力する移動体の姿勢検出装置において、
前記ＧＰＳ姿勢検出システムから演算される慣性データと、前記ＩＭＵ姿勢検出システムで観測される慣性データとの差分に基づいて、次の演算に用いる前記アライメント角を推定するアライメント角推定手段と、
該推定アライメント角を順次累積加算して更新することにより更新アライメント角を生成し、該更新アライメント角を前記アライメント角推定手段に出力するアライメント角加算手段とを備え、
前記更新アライメント角を、順次、前記アライメント角の推定演算にフィードバックすることを特徴とする移動体の姿勢検出装置。 - 前記アライメント角の各成分が
−８５°≦θｘ ≦８５°，
−８５°≦θｙ ≦８５°，
−９０°≦θｚ ≦９０°
であり、
前記アライメント角推定演算に、前記更新されたアライメント角とともに前記推定されたアライメント角を順次フィードバックすることを特徴とする請求項１に記載の移動体姿勢検出装置。 - 前記アライメント角推定手段は、前記推定アライメント角から、前記慣性センサに起因するセンサ誤差を推定し、
前記推定されたセンサ誤差を順次累積加算して更新することにより更新センサ誤差を生成し、該更新センサ誤差を前記アライメント角推定手段に出力するセンサ誤差加算手段を備え、
前記更新センサ誤差を順次アライメント角の推定演算にフィードバックすることを特徴とする請求項１または請求項２に記載の移動体の姿勢検出装置。 - 前記ＧＰＳ姿勢検出システムのＧＰＳ受信アンテナおよび前記ＩＭＵ姿勢検出システムの慣性センサを設置する際に、仮アライメント角を設定する手段を有し、該仮アライメント角を用いて、前記アライメント角推定手段の初期条件を設定することを特徴とする請求項１〜３のいずれかに記載の移動体の姿勢検出装置。
- 前記アライメント角推定演算は、前記アライメント角が確定するまで行われる請求項１〜４のいずれかに記載の移動体の姿勢検出装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003131985A JP4343581B2 (ja) | 2002-05-16 | 2003-05-09 | 移動体の姿勢検出装置 |
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002141576 | 2002-05-16 | ||
| JP2003131985A JP4343581B2 (ja) | 2002-05-16 | 2003-05-09 | 移動体の姿勢検出装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2004045385A true JP2004045385A (ja) | 2004-02-12 |
| JP4343581B2 JP4343581B2 (ja) | 2009-10-14 |
Family
ID=31719422
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2003131985A Expired - Fee Related JP4343581B2 (ja) | 2002-05-16 | 2003-05-09 | 移動体の姿勢検出装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP4343581B2 (ja) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2006067968A1 (ja) * | 2004-12-20 | 2006-06-29 | Matsushita Electric Industrial Co., Ltd. | 進行方向測定装置 |
| JP2007071868A (ja) * | 2005-08-08 | 2007-03-22 | Furuno Electric Co Ltd | キャリア位相相対測位装置及び方法 |
| JP2011075471A (ja) * | 2009-10-01 | 2011-04-14 | Topcon Corp | 測定方法及び測定装置 |
| WO2012066821A1 (ja) * | 2010-11-18 | 2012-05-24 | 古野電気株式会社 | 角速度検出装置、角速度検出方法、移動状態検出装置およびナビゲーション装置 |
| WO2021006243A1 (ja) * | 2019-07-10 | 2021-01-14 | 株式会社デンソー | アンテナ間相対位置推定方法、アンテナ間相対位置推定装置及びアンテナ間相対位置推定プログラム |
| CN112947772A (zh) * | 2021-01-28 | 2021-06-11 | 深圳市瑞立视多媒体科技有限公司 | 双光球交互笔的旋转姿态对齐方法、装置和计算机设备 |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108273258A (zh) * | 2018-01-24 | 2018-07-13 | 纳恩博（北京）科技有限公司 | 电动行驶设备 |
| US20210208232A1 (en) | 2018-05-18 | 2021-07-08 | Ensco, Inc. | Position and orientation tracking system, apparatus and method |
-
2003
- 2003-05-09 JP JP2003131985A patent/JP4343581B2/ja not_active Expired - Fee Related
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2006067968A1 (ja) * | 2004-12-20 | 2006-06-29 | Matsushita Electric Industrial Co., Ltd. | 進行方向測定装置 |
| JP2007071868A (ja) * | 2005-08-08 | 2007-03-22 | Furuno Electric Co Ltd | キャリア位相相対測位装置及び方法 |
| JP2011075471A (ja) * | 2009-10-01 | 2011-04-14 | Topcon Corp | 測定方法及び測定装置 |
| WO2012066821A1 (ja) * | 2010-11-18 | 2012-05-24 | 古野電気株式会社 | 角速度検出装置、角速度検出方法、移動状態検出装置およびナビゲーション装置 |
| JP5643334B2 (ja) * | 2010-11-18 | 2014-12-17 | 古野電気株式会社 | 角速度検出装置、角速度検出方法、移動状態検出装置およびナビゲーション装置 |
| WO2021006243A1 (ja) * | 2019-07-10 | 2021-01-14 | 株式会社デンソー | アンテナ間相対位置推定方法、アンテナ間相対位置推定装置及びアンテナ間相対位置推定プログラム |
| JP2021015002A (ja) * | 2019-07-10 | 2021-02-12 | 株式会社豊田中央研究所 | アンテナ間相対位置推定方法及びアンテナ間相対位置推定プログラム |
| JP7140443B2 (ja) | 2019-07-10 | 2022-09-21 | 株式会社豊田中央研究所 | アンテナ間相対位置推定方法及びアンテナ間相対位置推定プログラム |
| CN112947772A (zh) * | 2021-01-28 | 2021-06-11 | 深圳市瑞立视多媒体科技有限公司 | 双光球交互笔的旋转姿态对齐方法、装置和计算机设备 |
| CN112947772B (zh) * | 2021-01-28 | 2024-03-08 | 深圳市瑞立视多媒体科技有限公司 | 双光球交互笔的旋转姿态对齐方法、装置和计算机设备 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP4343581B2 (ja) | 2009-10-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US7076342B2 (en) | Attitude sensing apparatus for determining the attitude of a mobile unit | |
| US9791575B2 (en) | GNSS and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
| Bachmann et al. | Orientation tracking for humans and robots using inertial sensors | |
| JP6094026B2 (ja) | 姿勢判定方法、位置算出方法及び姿勢判定装置 | |
| JP4199553B2 (ja) | ハイブリッド航法装置 | |
| WO2019071916A1 (zh) | 天线波束姿态控制方法和系统 | |
| US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
| US9939273B2 (en) | Attitude estimating device, attitude estimating method, and storage medium | |
| US20060038718A1 (en) | Azimuth/attitude detecting sensor | |
| JP2009098125A (ja) | 動的に較正されるセンサデータと、ナビゲーションシステム内の繰り返し拡張カルマンフィルタとを使用する、ジャイロコンパスの整合用のシステム及び方法 | |
| JP2010112854A (ja) | 歩行者用ナビゲーション装置および歩行者用ナビゲーション装置における移動方向検出方法 | |
| EP2717014A1 (en) | Position calculation method and position calculation device | |
| JP2009505062A (ja) | リアルタイムバイアス推定器に基づく慣性機器のための自己較正 | |
| JP2008216062A (ja) | 移動体姿勢計測装置 | |
| JP2012173190A (ja) | 測位システム、測位方法 | |
| JP2014185955A (ja) | 移動状況情報算出方法及び移動状況情報算出装置 | |
| JP2012154769A (ja) | 加速度検出方法、位置算出方法及び加速度検出装置 | |
| CN110044377B (zh) | 一种基于Vicon的IMU离线标定方法 | |
| JP5022747B2 (ja) | 移動体の姿勢及び方位検出装置 | |
| JP4343581B2 (ja) | 移動体の姿勢検出装置 | |
| JP2014190900A (ja) | 位置算出方法及び位置算出装置 | |
| CN112985384A (zh) | 一种抗干扰磁航向角优化系统 | |
| CN114459466A (zh) | 一种基于模糊控制的mems多传感器数据融合处理方法 | |
| CN110375773B (zh) | Mems惯导系统姿态初始化方法 | |
| JP2015094631A (ja) | 位置算出装置及び位置算出方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20060413 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20090317 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20090514 |
|
| RD02 | Notification of acceptance of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7422 Effective date: 20090514 |
|
| 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: 20090609 |
|
| 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: 20090709 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120717 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: 20130717 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140717 Year of fee payment: 5 |
|
| LAPS | Cancellation because of no payment of annual fees |