JP4343581B2 - 移動体の姿勢検出装置 - Google Patents
移動体の姿勢検出装置 Download PDFInfo
- Publication number
- JP4343581B2 JP4343581B2 JP2003131985A JP2003131985A JP4343581B2 JP 4343581 B2 JP4343581 B2 JP 4343581B2 JP 2003131985 A JP2003131985 A JP 2003131985A JP 2003131985 A JP2003131985 A JP 2003131985A JP 4343581 B2 JP4343581 B2 JP 4343581B2
- Authority
- JP
- Japan
- Prior art keywords
- alignment angle
- estimated
- imu
- gps
- sensor
- 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.)
- Expired - Fee Related
Links
Images
Landscapes
- Position Fixing By Use Of Radio Waves (AREA)
Description
【発明の属する技術分野】
この発明は、GPS姿勢とIMU姿勢とを統合して移動体の姿勢を検出する、GPS/IMU統合化姿勢検出装置、特にアンテナ座標系のGPS姿勢とIMU座標系のIMU姿勢とのアライメント角を確実に推定するGPS/IMU統合化姿勢検出装置に関するものである。
【0002】
【従来の技術】
移動体の方位および姿勢を検出するシステムの一つとして、GPS姿勢検出システムがある。このシステムでは、全てが一直線上に並ばない少なくとも3個のGPS受信アンテナを移動体に装着し、これら直交3軸座標系で定義されたGPS受信アンテナのそれぞれがGPS衛星からの電波信号を受信して、アンテナ間のキャリア位相差を観測する。この観測値からそれぞれのアンテナの相対位置を計測することにより、アンテナ座標系を構成し、座標系の基準座標に対する方位、姿勢を計測する。
【0003】
しかし、このようなGPS姿勢検出システムでは、GSP衛星からの電波信号を受信することにより姿勢検出が可能であるが、GPS衛星からの電波信号が遮断されたり、キャリア位相サイクルスリップ等が生じた場合、キャリア位相差を観測できなくなり、姿勢検出できなくなってしまうという問題がある。
【0004】
この問題を解決する方法として、角速度センサや加速度センサ等の慣性センサ(IMU)を移動体に取り付け、移動体の動きを観測して、この観測データから得られる姿勢とGPS姿勢検出システムにより得られる姿勢との座標系回転量(アライメント角)を推定して統合する、GPS/IMU統合化技術がある。
【0005】
この方法は、直交3軸座標系の各軸にそれぞれマウントされた慣性センサから既知の方法で得られる慣性センサ座標系(以下、「IMU座標系」という)の姿勢と、GPS姿勢検出システムから得られるアンテナ座標系の姿勢とのアライメント角を推定して統合することにより、安定して高精度に姿勢を得ることができる。この方法を用いることにより、GPS衛星からの電波信号が中断しても、中断時の移動体の動きを慣性センサで観測することができるので、電波信号中断中は慣性センサによる姿勢でGPSによる姿勢の欠落を補間することができ、移動体の姿勢を常時出力することができる(例えば、特許文献1参照。)。
【0006】
【特許文献1】
特開2002−54946号
【0007】
【発明が解決しようとする課題】
ところが、このような従来の移動体の姿勢検出装置では、次に示す解決すべき課題が存在した。
【0008】
移動体の姿勢検出装置におけるGPS/IMU統合技術では、アンテナおよび慣性センサを取り付ける際に、それぞれの座標系であるアンテナ座標系とIMU座標系との座標系回転量(アライメント角)を求めなければならない。
【0009】
従来のアライメント角推定技法として、次のいずれかの方法が用いられている。
第1の方法としては、複数の慣性センサからなるIMU座標系ユニットの一基準方向と複数のGPS受信アンテナからなるアンテナ座標系の一基準方向とが一致するように、目視等で確認しながら、複数のGPS受信アンテナを取り付ける。その後、この取り付けで生じたであろう両座標系間のアライメント角を無視し、両座標系が一致したものとみなす。
【0010】
この方法では、数度程度のアライメント誤差が頻繁に発生するため、GPS/IMU統合技術を用いても、姿勢精度が低く、不安定であった。
【0011】
第2の方法としては、第1の方法に示したアライメント角設定の後、次に示すアライメント角推定方法でアライメント角を推定するものである。
【0012】
一例として、IMUセンサとして角速度センサを用いた場合を説明する。
GPS姿勢検出システムで得られた姿勢から角速度(以下「GPS角速度」という。)ωg1を演算し、これと同じ時点で角速度センサにより角速度(以下「IMU角速度」という。)ωi1を得る。これらGPS角速度ωg1とIMU角速度ωi1とを差分することにより、差分値Δz1 を求める。この差分値Δz1 に基づいてアライメント角θi1を推定し、このアライメント角θi1を用いて次の時点のIMU角速度ωi2を補正する。次に、このIMU角速度ωi2と同時点のGPS角速度ωg2との差分をとり、新たな差分値Δz2 を求め、この差分値Δz2 から新たなアライメント角θi2を推定し、これをさらに次の時点のIMU角速度の補正に用いる。このような演算を繰り返し行うことにより、アライメント角θi を収束させて、真のアライメント角を算出する。
【0013】
この方法では、算出されるアライメント角が数度程度の微小角でなければ、非線形のため収束しないので、初期条件として、アライメント角が微小角であることが必要となってしまう。
【0014】
ところで、例えば、ユーザがGPS受信アンテナを任意に現場で配置する場合には、その場でGPS座標系を設定し、これをIMU座標系に合うようにとりつけなければならない。しかし、このような作業で、微小なアライメント角となるように複数のGPS受信アンテナを設置することは非常に困難なことである。また、GPS受信アンテナの取り付け位置からIMUセンサユニットが視認できない場合には、前述の目視によるあわせ込みは不可能であるため、アライメント角を微小にすることは非常に困難である。
【0015】
この発明の目的は、アライメント角の大きさに関係なく、アンテナ座標系とIMU座標系とのアライメント角を確実に推定することができる移動体の姿勢検出装置を提供することにある。
【0016】
【課題を解決するための手段】
この発明は、アライメント角推定手段とアライメント角加算手段とを備えて、所定の周期毎に推定されるアライメント角を累積加算して更新しながら、アライメント角推定演算にフィードバックすることを特徴としている。
【0017】
アライメント角推定手段は、慣性データ変換手段とアライメント角推定手段とからなり、慣性データ変換手段は、IMU慣性センサにより計測された慣性データをIMU座標系からアンテナ座標系に変換する。
【0018】
アライメント角推定手段は、座標変換された慣性データとGPS姿勢検出システムから演算された慣性データ(以下、「GPS慣性データ」という。)との差分値から、所定の周期でアライメント角を推定する。
【0019】
推定されたアライメント角は、アライメント角加算手段で、前記周期で順次累積加算され更新され、慣性データ変換手段に出力される。慣性データ変換手段では、ある時点での更新アライメント角を用いて、慣性データを座標変換する。
【0020】
このように、本発明のアライメント角推定手段は、ある時点の差分値から推定したアライメント角を用いて、逐次、慣性データを変換し、この変換された慣性データと、変換された慣性データと同じ時点のGPS慣性データとを差分して新たなアライメント角を推定する。
【0021】
このようなフィードバック演算を繰り返し行うことで、ある時点での推定アライメント角は、前時点で更新されたアライメント角を用いて変換された慣性データとその時点でのGPS慣性データとの差分値から推定されるので、アライメント角推定手段で推定されるアライメント値は徐々に小さく収束していく。そして、これと同時にアライメント角加算部では、更新アライメント角がアライメント角の真値に向かって徐々に収束していく。
【0022】
このように推定と累積加算とを繰り返して得られたアライメント角を用いることにより、アンテナ座標系の姿勢とIMU座標系の姿勢とを統合して、高精度で安定した姿勢を出力する。
また、この発明は、センサ誤差加算部を備え、慣性センサから出力された慣性データに含まれるセンサに起因するセンサ誤差を補正する。アライメント角推定手段は、二つのシステムで得られた慣性データの差分値からセンサ誤差を推定して、センサ誤差加算手段に出力する。センサ誤差加算手段では、センサ誤差が、前記アライメント角と同様に、推定周期毎に順次累積加算され更新される。この更新センサ誤差は慣性データ補正部に出力され、慣性データ補正部では、更新センサ誤差を適用して、次の時点での慣性データを補正する。
【0023】
また、この発明は、アライメント角の各成分が、−85°≦θx ≦85°,−85°≦θy ≦85°,−90°≦θz ≦90°のように大きな値であるときでも、アライメント角を推定することができる。
【0024】
このように所定の範囲にアライメント角で収まるようにGPS受信アンテナを設置し、更新されたアライメント角と推定されたアライメント角とをアライメント推定演算にフィードバックすることで、アライメント角推定演算のアルゴリズムを簡略化することができ、アライメント角の推定速度が速くなる。
【0025】
また、この発明は、センサ誤差加算部と慣性データ補正部とを備え、慣性センサから出力された慣性データに含まれるセンサに起因するセンサ誤差を補正する。
【0026】
アライメント角推定手段は、二つのシステムで得られた慣性データの差分値からセンサ誤差を推定して、センサ誤差加算手段に出力する。センサ誤差加算手段では、センサ誤差が、前記アライメント角と同様に、推定周期毎に順次累積加算され更新される。この更新センサ誤差は慣性データ補正部に出力され、慣性データ補正部では、更新センサ誤差を適用して、次の時点での慣性データを補正する。
【0027】
また、この発明は、上述のアライメント角推定演算を行う前、目測等の方法でアライメント角を概略推定しておき、この推定アライメント角を用いて、アライメント推定手段で用いる変換マトリクスの初期値を設定して、アライメント角推定演算を行うことも可能である。
【0028】
また、この発明は、アライメント角が上記のような大きい値である時も、姿勢の初期値を設定することなく、アライメント角推定演算が正しいアライメント角を推定するまで行われることで、確実にアライメント角が推定される。
【0029】
【発明の実施の形態】
本発明の第1の実施形態に係る移動体の姿勢検出装置について、図1、図2を参照して説明する。
図1はアンテナ座標系とIMU座標系の相関関係を示した図である。
図2は姿勢検出装置のアライメント角推定フローを示したブロック図である。図1において、ANT0,1,2はGPS受信アンテナ、Sx ,Sy ,Sz は慣性センサである角速度センサであり、xg ,yg ,zg はそれぞれアンテナ座標系、xi ,yi ,zi はIMU座標系である。また、Cg i はIMU座標系からアンテナ座標系に変換する変換マトリックスである。
【0030】
図2のおいて、101はIMU姿勢検出システムのIMU角速度演算部、102は慣性データ補正部、103は慣性データ変換部、104はGPS姿勢検出システムのGPS姿勢演算部、105はGPS姿勢検出システムのGPS角速度演算部、106はアライメント角推定部、107はアライメント角加算部、108はセンサ誤差加算部である。本発明におけるアライメント角推定手段とは、慣性データ補正部102、慣性データ変換部103、およびアライメント角推定部106に対応する。
【0031】
図1に示すように、アンテナ座標系には、3個のGPS受信アンテナANT0,ANT1,ANT2が全て一直線上に並ばないように配置されている。ここで、アンテナANT0をアンテナ座標系の原点に置き、他のアンテナANT1,2の座標を(x1 ,y1 ,z1 ),(x2 ,y2 ,z2 )とする。また、IMU座標系の各基準軸には、それぞれ角速度センサSx ,Sy ,Sz が配置されている。
【0032】
図1において、IMU座標系に対してアンテナ座標系が所定の角度で座標回転しており、θx ,θy ,θz を、座標回転順序を仮にz,y,xとしたときのオイラー角とすると、その変換マトリックスCg i は、次式で表される。
【0033】
【数1】
【0034】
ここで、θx ,θy ,θz はアライメント角であり、このアライメント角を演算して推定することにより、アンテナ座標系とIMU座標系を統合することができる。
【0035】
次に、図2を用いて、前記アライメント角の推定方法について説明する。
本説明では、慣性センサとして角速度センサを用い、本発明の慣性データに対応するものとしてIMU角速度を用いて説明する。
【0036】
IMU角速度演算部101には、図1に示した直交3軸座標系にそれぞれ配置された3個の角速度センサが備えられており、該角速度センサでIMU座標系のIMU角速度ωimを出力する。このIMU角速度ωimに基づいて、IMU姿勢角演算部(図示せず)で既知の方法を用いてIMU姿勢を検出する。
【0037】
角速度センサには内部誤差として、バイアス誤差Δωi およびスケールファクター誤差ΔKs が含まれている。このため、IMU角速度ωimはその真値ωi として、
【0038】
【数2】
【0039】
と表すことができる。ここで、誤差の2次以上の項は値が小さく、無視できるものとすると、
【0040】
【数3】
【0041】
と表すことができる。
【0042】
アンテナ座標系に対するIMU座標系のアライメント角をΔθi giとすると、このアライメント角に対する変換マトリックスC’g i は次式で表すことができる。
【0043】
【数4】
【0044】
ここで、Cg i は図1および(1)式に示した真の変換マトリックスであり、Δθi giはベクトル量でありその内成分は(Δθx ,Δθy ,Δθz )である。また、S(Δθi gi)はアライメント角Δθi giの交代行列であり、
【0045】
【数5】
【0046】
で表される。
【0047】
慣性データ変換部103では、(1)式を用いて、IMU座標系のIMU角速度ωimをアンテナ座標系に変換する。
【0048】
IMU角速度ωimをアンテナ座標系に変換したIMU/GPS角速度ω’g i は、(2’)式、(3)式より、誤差の2次以上の項を無視すると、
【0049】
【数6】
【0050】
と表される。
【0051】
一方、GPS姿勢演算部104では、GPS衛星からの電波信号を図1に示した、GPS受信アンテナANT0,ANT1,ANT2で受信し、GPS姿勢演算部104で既知の方法にてGPS姿勢を出力する。GPS角速度演算部105では、このGPS姿勢を用いてGPS角速度ωgmを出力する。ここで、実際に観測されるGPS角速度ωgmは誤差Δωg を含んでいるので、GPS角速度の真値をωg とすると、
【0052】
【数7】
【0053】
と表される。
【0054】
ここで、GPS角速度の真値ωg とIMU角速度の真値ωi とは、
【0055】
【数8】
【0056】
の関係にある。
【0057】
(5)式、(6)式、(7)式より、IMU/GPS角速度ω’g i とGPS角速度ωgmとの差Δzは次式で表される。
【0058】
【数9】
【0059】
【数10】
【0060】
【数11】
【0061】
ここで、Δθi gix ,Δθi giy ,Δθi giz はそれぞれアライメント角、Δω’x ,Δω’y ,Δω’z はそれぞれ角速度センサにおけるx,y,z軸の角速度の推定バイアス誤差、ΔK’sx,ΔK’sy,ΔK’szはそれぞれ角速度センサにおけるx,y,z軸の角速度の推定スケールファクタ誤差、νはΔzの観測誤差を表す。
【0062】
なお、IMU/GPS角速度ω’g i とGPS角速度ωgmは、それぞれ周期Tgで同期するようにサンプリングされており、IMU/GPS角速度ω’g i とGPS角速度ωgmとは、それぞれ角速度センサとGPS姿勢演算部とでの観測された時刻が同じものであるように時刻同期の処理が行われているものとする。
【0063】
アライメント角推定部106では、IMU/GPS角速度ω’g i とGPS角速度ωgmとの差分値Δzを入力し、(10)式の状態変数を推定する。
【0064】
例えば、サンプリング周期Tgを用いて、次式に示すようなカルマンフィルタで、周期Tg毎に順次、前記各状態変数を推定する。
【0065】
【数12】
【0066】
ここで、Φは状態遷移マトリクスであり、
wk =(0,0,0,ηx ,ηy ,ηz ,0,0,0)T は観測雑音である。
【0067】
カルマンフィルタは、所定の周期で、推定誤差の平均2乗誤差を最小とするように、前回の観測値から次回の推定値を演算し、この演算を繰り返し行うことにより、目的値を導き出す手法である。
【0068】
ある時点での推定バイアス誤差Δω’i をδΔω’i とし、推定スケールファクタ誤差ΔK’s をδΔK’s とすると、センサ誤差加算部108では、推定バイアス誤差δΔω’i と推定スケールファクタ誤差δΔK’s とを入力し、次式のように前時点での推定バイアス誤差Δω’i と推定スケールファクタ誤差ΔK’s にそれぞれ加算される。
【0069】
【数13】
【0070】
この演算は、周期TgでδΔω’i ,δΔK’s が推定される毎に行われ、推定バイアス誤差Δω’i と推定スケールファクタ誤差ΔK’s とを順次累積加算し、更新していく。
【0071】
更新された推定バイアス誤差Δω’i と推定スケールファクタ誤差ΔK’s は、慣性データ補正部102に出力され、慣性データ補正部102では、更新された推定バイアス誤差Δω’i と推定スケールファクタ誤差ΔK’s を用いて、次の時点のIMU角速度ωimを補正する。
【0072】
このように、推定バイアス誤差Δω’i と推定スケールファクタ誤差ΔK’s とをフィードバックしていくことにより、ある時点で、アライメント角推定部106で推定されるセンサ誤差δΔω’i ,δΔK’s は、前時点の推定値で補正されたIMU角速度と、その時点でのGPS角速度とから求められる。これにより、アライメント角推定部106で推定されるセンサ誤差は、更新される度に徐々に小さい値となっていき、0に近づく。一方、センサ誤差加算部108では、このように時系列に推定されたセンサ誤差を加算していき、徐々にセンサ誤差の真値となるように収束していく。
【0073】
これにより、推定を繰り返す毎にセンサ誤差は真値に近づき、これを用いてIMU角速度を補正することで、IMU角速度におけるセンサ誤差の影響を排除していく。
【0074】
アライメント角加算部107は、アライメント角推定部106で推定された推定アライメント角Δθi giを周期Tg毎に累積加算していき、更新アライメント角θi giを生成する。
【0075】
【数14】
【0076】
更新アライメント角θi giは、慣性データ変換部103に出力され、慣性データ変換部103はこの更新アライメント角θi giに基づいて、(1)式に示した変換マトリクスを順次演算して更新する。
【0077】
このように、更新アライメント角θi giをフィードバックしていくことにより、ある時点での推定アライメント角Δθi giは、前時点での更新アライメント角θi giを用いて座標変換されたIMU角速度と、その時点でのGPS角速度との差分から求められる。これにより、アライメント角推定部106で推定される推定アライメント角Δθi giは徐々に小さくなり、0に近づくように収束し、更新されるアライメント角θi giは徐々にアライメント角の真値となるように収束していく。
【0078】
図3に、アライメント角推定シミュレーションの結果を示す。なお、このシミュレーションに示すアライメント角とは図2に示すθi giを意味する。
アンテナ座標系とIMU座標系とのアライメント角は各成分をRoll角、Pitch角、Yaw角として、それぞれ−30°,50°,100°であり、推定の初期値は全て0°で、白色雑音が重畳された状態として推定を行った。また、推定を行うためのIMUユニットの揺動条件として、Roll角、Pitch角、Yaw角の振幅および周期を表1に示すように設定した。
【0079】
【表1】
【0080】
図3に示すように、推定初期は白色雑音の影響等で各アライメント角成分は振動するが、徐々に振幅が小さくなるとともに真値に収束する。
【0081】
図4は、振揺装置に設置した角速度センサユニットとGPS受信アンテナユニットを用いて、それぞれのユニットから得られる角速度に基づいて、アライメント角を推定した実験結果を示す。本推定実験では、Roll角のみが−90°、Yaw角、Pitch角がそれぞれ0°となるように二つの座標系を設置し、Yaw角およびPitch角方向の動作開始時刻を180sec.、Roll角方向の動作開始時刻を500sec.とした。
【0082】
図4に示すように、アライメント角の各成分は、1°程度の誤差範囲内に収束する。
このように、上述のアライメント角推定演算を用いることで、アライメント角が大きくても、確実に推定することができる。
【0083】
これにより、アンテナ座標系とIMU座標系とのアライメント角が高精度に推定できるため、GPS姿勢検出システムで検出された姿勢とIMU姿勢検出システムで検出された姿勢とを高精度に統合することができる。これにより、外部環境に影響されることなく、常時高精度の姿勢を検出することができる。
【0084】
なお、慣性データ補正部102、慣性データ変換部103、アライメント角加算部107、センサ誤差加算部108における各状態変数の初期値はすべて0とし、変換マトリクスCg i の初期値は単位マトリクスとする。
【0085】
また、本実施形態では、カルマンフィルタを用いて各状態変数の推定を行っているが、各状態変数を算出するに必要な数の差分Δzを記憶しておき、最小2乗法で算出することも可能である。ただし、この場合には、各状態変数の更新周期が、(状態変数の算出に必要な差分回数)×(サンプリング周期Tg)となる。
【0086】
また、本実施形態では、センサ誤差およびスケールファクタ誤差を考慮して推定を行っているが、高精度の角速度センサを使用したり、アライメント角の要求精度によっては、これらの状態変数を削除してアライメント角を推定することもできる。
【0087】
次に、第2の実施形態に係る移動体の姿勢検出装置について図5を参照して説明する。
図5は姿勢検出装置のアライメント角推定フローを示したブロック図である。
【0088】
図5に示す回路は、図2に示した回路を同じ要素で構成されているが、慣性データ変換部103で、IMU角速度ωimをアンテナ座標系に変換する演算が異なるものである。慣性データ変換部103には更新されたアライメント角θi giとともに、推定されたアライメント角Δθi giがフィードバックされる。
【0089】
(3)式を基づいて(8)式を用いるためには変換マトリクスCg i が既知である必要があるが、実際には既知ではないので変換マトリクスCg i を単位マトリクスで近似せざるを得ない。そのためには、変換マトリクスCg i の各要素となるアライメント角の各成分を、(θx ,θy ,θz )とし、
【0090】
【数15】
【0091】
となるようにすればよい。このような範囲でアンテナ座標系とIMU座標系とを設置することは、目視で容易に実現できる。
【0092】
そして、このような範囲のアライメント角であっても、(8)式、(9)式に示すCg i を単位マトリクスに近似することができ、(8)式、(9)式、はそれぞれで次式で表すことができる。
【0093】
【数16】
【0094】
【数17】
【0095】
この演算式において、上述の(14)式を適用すると、真の変換マトリクスCg i の各要素と(3)式においてCg i を単位マトリクスとして計算した変換マトリクスC’g i の各要素とで符号が異なるのは、全ての要素のうち多くとも1要素となる。
【0096】
このような変換マトリクスC’g i で変換を行った場合、IMU/GPS角速度ω’g i の大きさは変化するが、極性が変化するのは、IMU/GPS角速度ω’g i の3成分(ω’g ix,ω’g iy,ω’g iz)のうち、1成分のみである。
【0097】
ω’g i の極性が反転しない2成分を持つ場合のアライメント角の推定は、収束する方向で推定アライメント角Δθi giを算出する。このように算出された推定アライメント角Δθi giを順次累積加算しながら、角速度の変換にフィードバックしていけば、変換マトリクスの極性反転した1要素も再度極性反転して、推定アライメント角Δθi giを収束させるような極性となる。このように要素が修正されるので、変換マトリクスC’g i は真の変換マトリクスCg i に置き換えることができ、正確にアライメント角を推定することができる。
【0098】
このように、初期アライメント角を(14)式の条件を満たすように設定することで、上述のように変換マトリクスを単位行列に近似することができるので、演算アルゴリズムを簡略化し、推定時間を短縮することができる。
【0099】
なお、アライメント角が(14)式を満たさないような大きな場合は、図5に示すように、慣性データ変換部103における変換マトリクスの初期値Cg i(1)を、(14)式の条件を満たすように設定することで、推定時間を短縮することができる。
【0100】
また、上述の実施形態では、アライメント角の推定演算は、正しいアライメント角推定値に収束するまで行われる。これにより、確実にアライメント角を推定することができる。
【0101】
【発明の効果】
この発明によれば、推定アライメント角を、推定を行う周期で順次累積加算、更新していき、アライメント角推定演算にフィードバックする。これにより、推定を行う周期毎に、その前時点に更新されたアライメント角で変換を行った慣性データから新たなアライメント角を推定していくので、周期毎に推定されるアライメント角は徐々に0へ収束していき、累積加算される更新アライメント角はアライメント角の真値に収束する。このようなフィードバックをアライメント角推定演算に行うことで、特に初期のアライメント角の大きさに関係なく、正確なアライメント角を確実に推定することができる。
【0102】
また、この発明によれば、アライメント角の初期値を所定の範囲内にすることで、アライメント角推定演算のアルゴリズムを簡略化することができ、アライメント角推定時間を短縮することができる。
【0103】
また、この発明によれば、センサ誤差についても推定、累積加算してフィードバックすることにより、推定アライメント角に含まれるセンサに起因する誤差を排除することができる。これにより、各周期での推定アライメント角を累積加算してなる更新アライメント角をさらに高精度に真値に近づけることができる。
【0104】
また、この発明によれば、アライメント角推定演算を行う前に、予め、目測等でアライメント角の初期条件を設定しておくことにより、アライメント角推定演算でのアライメント角推定を確実にするとともに、更に短時間でアライメント角を推定することができる。
【0105】
また、この発明によれば、アライメント角推定演算は正しいアライメント角推定値に収束するまで行われるので、確実にアライメント角を推定することができる。
【図面の簡単な説明】
【図1】アンテナ座標系とIMU座標系との関係を示した図
【図2】第1の実施形態に係るアライメント角推定演算フローを示したブロック図
【図3】アライメント角推定シミュレーション結果を示した図
【図4】実船によるアライメント角推定実験の結果を示した図
【図5】第2の実施形態に係るアライメント角推定演算フローを示したブロック図
【符号の説明】
101−IMU角速度演算部
102−慣性データ補正部
103−慣性データ変換部
104−GPS姿勢演算部
105−GPS角速度演算部
106−アライメント角推定部
107−アライメント角加算部
108−センサ誤差加算部
ANT1,ANT2,ANT3−GPS受信アンテナ
Sx ,Sy ,Sz −角速度センサ
Claims (4)
- 移動体の姿勢をアンテナ座標系で検出するGPS姿勢検出システムと、慣性センサを用いてIMU座標系で検出するIMU姿勢検出システムとからなり、二つの座標系間のアライメント角を順次推定演算することにより、各座標系で検出された前記姿勢を統合することで、前記移動体の姿勢を出力する移動体の姿勢検出装置において、
前記GPS姿勢検出システムから演算される慣性データと、前記IMU姿勢検出システムで観測される慣性データとの差分に基づいて、次の演算に用いる前記アライメント角を推定するとともに、該推定アライメント角から前記慣性センサに起因するセンサ誤差を推定するアライメント角推定手段と、
該推定アライメント角を順次累積加算して更新することにより更新アライメント角を生成し、該更新アライメント角を前記アライメント角推定手段に出力するアライメント角加算手段と、
前記推定されたセンサ誤差を順次累積加算して更新することにより更新センサ誤差を生成し、該更新センサ誤差を前記アライメント角推定手段に出力するセンサ誤差加算手段と、を備え、
順次生成される前記更新アライメント角と前記更新センサ誤差とを、前記アライメント角の推定演算に用いることを特徴とする移動体の姿勢検出装置。 - 前記アライメント角の各成分が
−85°≦θx ≦85°,
−85°≦θy ≦85°,
−90°≦θz ≦90°
であり、
前記アライメント角推定演算に、前記更新されたアライメント角とともに前記推定されたアライメント角を順次フィードバックすることを特徴とする請求項1に記載の移動体姿勢検出装置。 - 前記GPS姿勢検出システムのGPS受信アンテナおよび前記IMU姿勢検出システムの慣性センサを設置する際に、仮アライメント角を設定する手段を有し、該仮アライメント角を用いて、前記アライメント角推定手段の初期条件を設定することを特徴とする請求項1または請求項2に記載の移動体の姿勢検出装置。
- 前記アライメント角推定演算は、前記アライメント角が確定するまで行われる請求項1〜3のいずれかに記載の移動体の姿勢検出装置。
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 JP2004045385A (ja) | 2004-02-12 |
JP4343581B2 true 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 (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 |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1837675A4 (en) * | 2004-12-20 | 2008-06-18 | Matsushita Electric Ind Co Ltd | DEVICE FOR MEASURING ADVANCE STEERING |
JP5180447B2 (ja) * | 2005-08-08 | 2013-04-10 | 古野電気株式会社 | キャリア位相相対測位装置及び方法 |
JP5337656B2 (ja) * | 2009-10-01 | 2013-11-06 | 株式会社トプコン | 測定方法及び測定装置 |
US20130238277A1 (en) * | 2010-11-18 | 2013-09-12 | Furuno Electric Co., Ltd. | Angular velocity detecting device, angular velocity detecting method, movement state detecting device and navigation device |
JP7140443B2 (ja) * | 2019-07-10 | 2022-09-21 | 株式会社豊田中央研究所 | アンテナ間相対位置推定方法及びアンテナ間相対位置推定プログラム |
CN112947772B (zh) * | 2021-01-28 | 2024-03-08 | 深圳市瑞立视多媒体科技有限公司 | 双光球交互笔的旋转姿态对齐方法、装置和计算机设备 |
-
2003
- 2003-05-09 JP JP2003131985A patent/JP4343581B2/ja not_active Expired - Fee Related
Cited By (3)
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 |
US11656316B2 (en) | 2018-05-18 | 2023-05-23 | Ensco, Inc. | Position and orientation tracking system, apparatus and method |
Also Published As
Publication number | Publication date |
---|---|
JP2004045385A (ja) | 2004-02-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US7076342B2 (en) | Attitude sensing apparatus for determining the attitude of a mobile unit | |
JP6094026B2 (ja) | 姿勢判定方法、位置算出方法及び姿勢判定装置 | |
US9791575B2 (en) | GNSS and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
JP4199553B2 (ja) | ハイブリッド航法装置 | |
Bachmann et al. | Orientation tracking for humans and robots using inertial sensors | |
US10508970B2 (en) | System for precision measurement of structure and method therefor | |
WO2019071916A1 (zh) | 天线波束姿态控制方法和系统 | |
CN108680942B (zh) | 一种惯性/多天线gnss组合导航方法及装置 | |
CN107270893B (zh) | 面向不动产测量的杆臂、时间不同步误差估计与补偿方法 | |
US7230567B2 (en) | Azimuth/attitude detecting sensor | |
CN100516775C (zh) | 一种捷联惯性导航系统初始姿态确定方法 | |
EP3865914A1 (en) | Gnss/imu surveying and mapping system and method | |
JP3850796B2 (ja) | スレーブ慣性測定システムの姿勢アライメント | |
EP2951530A1 (en) | Inertial device, method, and program | |
JP2012173190A (ja) | 測位システム、測位方法 | |
JP2010112854A (ja) | 歩行者用ナビゲーション装置および歩行者用ナビゲーション装置における移動方向検出方法 | |
JP2012154769A (ja) | 加速度検出方法、位置算出方法及び加速度検出装置 | |
JP2008216062A (ja) | 移動体姿勢計測装置 | |
JP2016033473A (ja) | 位置算出方法及び位置算出装置 | |
JP2019120587A (ja) | 測位システム及び測位方法 | |
JP5022747B2 (ja) | 移動体の姿勢及び方位検出装置 | |
JP4343581B2 (ja) | 移動体の姿勢検出装置 | |
Wenk et al. | Posture from motion | |
JP2014190900A (ja) | 位置算出方法及び位置算出装置 | |
WO2019167517A1 (ja) | 航法装置、vslam補正方法、空間情報推定方法、vslam補正プログラム、および、空間情報推定プログラム |
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 |