説明

移動体測位方法、移動体測位装置

【課題】高精度な移動体測位方法を実現する。
【解決手段】移動体測位方法は、慣性センサーデータをバッファリングすることと、バッファリングされた前記慣性センサーデータを用いてINS測位演算を行うことと、現在時刻に対するGPS測位の遅延時間を計算することと、GPSデータの出力時刻とGPS測位結果とをバッファリングすることと、バッファリングされた前記GPS測位結果とINS測位結果との時刻同期をとることと、バッファリングされた前記慣性センサーデータと、前記慣性INS測位結果と、バッファリングされた前記GPS測位結果と、をカップリングし、位置・速度・姿勢の誤差を推定することと、前記位置・速度・姿勢の誤差を前記INS測位結果にフィードバックし、補正する。GPSデータと慣性センサーデータの時刻同期をとり、正確なINS測位結果の誤差補正を行うことによって、高精度な移動体測位を行うことができる。

【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体測位方法、および移動体測位装置に関する。
【背景技術】
【0002】
GPS測位データと慣性センサーデータを融合して、屋内外を問わず高精度な測位を行う場合、GPS測位データは伝送時間および測位計算時間の影響で現在時刻に対して遅延時間が発生してしまうため、GPS測位データと慣性センサーデータとの時刻同期を行う必要がある。GPS測位データと慣性センサーデータとの時刻同期をとる方法としては、GPS測位データが到達した時刻から測位演算時間と伝送時間を差し引いた時刻をGPS基準時刻とし、慣性センサーの検出結果のうちから、基準時刻に最も近い検出結果を同期させるべき検出データと特定し、特定された検出結果の検出時刻とGPS基準時刻の差分を算出し、算出した時間差分をプロパゲーションしてGPS測位データと慣性センサーデータとの時刻同期を行うものが知られている(例えば、特許文献1参照)。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開2009−222438号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
このような特許文献1による測位方法では、慣性センサーの測位結果とGPS測位結果とを同期するために、慣性センサーの検出時刻とGPSの基準時刻との差分ΔTpを算出し、外挿法(外挿補間と呼称することがある)により、移動体の速度ベクトルに時間差ΔTpを乗算して、周期iにおいて測位された移動体の位置に積算して、移動体の位置を補正し、センサーの検出結果と対応付けを行う。このような同期方法では、移動体が加減速で速度の大きさが一定に保たなかったり、回転で速度の方向が変化したりする場合に、外挿法によって補正された移動体の位置は、検出時刻における移動体の真の位置と大きくずれる可能性がある。また、速度や方位の外挿補間は困難であることから、GPS測位結果と慣性センサーの検出結果の同期がとれず、正確にカップリングすることができないという課題があった。
【課題を解決するための手段】
【0005】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【0006】
[適用例1]本適用例に係る移動体測位方法は、慣性センサーデータをバッファリングすることと、バッファリングされた前記慣性センサーデータを用いてINS測位演算を行うことと、現在時刻に対するGPS測位の遅延時間を計算することと、GPSデータの出力時刻とGPS測位結果とをバッファリングすることと、バッファリングされた前記GPS測位結果とINS測位結果との時刻同期をとることと、バッファリングされた前記慣性センサーデータと、前記INS測位結果と、バッファリングされた前記GPS測位結果と、をカップリングし、位置・速度・姿勢の誤差を推定することと、前記位置・速度・姿勢の誤差を前記INS測位結果にフィードバックし、補正することと、を含むことを特徴とする。
【0007】
本適用例によれば、慣性センサーデータと、GPSデータの出力時刻とGPS測位結果と、をバッファリング(保存)し、GPS測位結果の遅延時間を用いてGPS測位データの出力時刻を計算し、INS測位結果との時刻同期をとることによって、従来技術のように算出した時間差分をプロパゲーションしてGPS測位データと慣性センサーデータとの時刻同期を行う方法に対し、時刻ずれの影響を排除し、カップリングによる位置・速度・姿勢の推定誤差の計算精度が向上する。よって、INS測位結果の高精度な位置・速度・姿勢の補正を行うことができることから、移動体の加減速で速度の大きさが一定に保たなかったり、回転で速度の方向が変化したりする場合においても、移動体の正確な測位を行うことができる。また、小型化、低コスト化が可能ではあるが、バイアス誤差及びランダムドリフトの変動が大きなMEMS(Micro Electro Mechanical System)慣性センサーを用いる場合でも、トンネルや屋内において、移動体の高精度な制御が可能である。
【0008】
[適用例2]上記適用例に係る移動体測位方法は、慣性センサーの検出値を用いて、INS測位演算を行うことと、前記推定された位置・速度・姿勢の誤差を補正することと、をさらに含むことが好ましい。
【0009】
カルマンフィルター(KF)を用いてカップリングを行う場合、カルマフィルターからの出力は、慣性センサーのWait時間だけ遅延が発生する。そこで、慣性センサーの検出値を用いてリアルタイムでINS測位演算を行い、時刻同期されたGPSデータおよび慣性センサーデータを用いてカップリングして算出される位置・速度・姿勢の誤差を補正することで、遅延なく測位結果を出力することができる。
【0010】
[適用例3]本適用例に係る移動体制御装置は、慣性センサーデータを取得する慣性センサーと、GPSデータを受信し、GPS測位演算を行うGPS信号受信部と、前記慣性センサーデータを用いて測位演算を行うINSと、前記慣性センサーデータと、GPS測位結果と出力時刻と、をバッファリングし、時刻同期をとる時刻同期部と、バッファリングされた前記慣性データと、INS測位結果と、バッファリングされた前記GPS測位結果と、を用いてカップリングし、位置・速度・姿勢の誤差を推定するカルマンフィルターと、を備え、前記推定された位置・速度・姿勢の誤差を前記INSにフィードバックし、補正することを特徴とする。
本適用例では、INS(Inertial Navigation system)は、慣性航法を用いた測位演算部をさす。
【0011】
本適用例によれば、時刻同期部によって、慣性センサーの検出値と、前記出力時刻とGPS測位結果と、GPSの遅延時間を用いて算出したGPS測位結果の出力時刻と、INS測位結果とをバッファリングして時刻同期をとり、カルマンフィルターを用いて、位置・速度・姿勢の誤差を推定する。このことから、位置・速度・姿勢の推定誤差の計算精度が向上する。よって、INS測位結果の高精度な位置・速度・姿勢の誤差補正を行うことができる。
【0012】
[適用例4]上記適用例に係る移動体制御装置は、前記慣性センサーの検出値を用いてINS測位演算を行い、前記推定された位置・速度・姿勢の誤差を補正するリアルタイムINSと、をさらに備えることが好ましい。
【0013】
慣性センサーの検出値を用いてリアルタイムでINS測位演算を行い、時刻同期されたGPSデータおよび慣性センサーデータをカップリングすることによって、位置・速度・姿勢の誤差を正確に推定し、補正することで、遅延なく測位結果を出力することができる。
【図面の簡単な説明】
【0014】
【図1】実施形態1に係る移動体測位装置の主要な構成を示す構成説明図。
【図2】実施形態1に係る移動体測位方法の主要工程を示すフローチャート。
【図3】GPSの遅延時間が増加することがある場合のバッファー出力のタイミングを示し、(a)は、慣性センサーデータとGPSデータのタイミングを示す図、(b)は(a)のバッファー出力のタイミングと経過時間との関係を示す表。
【図4】GPSの遅延時間が減少することがある場合のバッファー出力のタイミングを示し、(a)は、慣性センサーデータとGPSデータのタイミングを示す図、(b)は(a)のバッファー出力のタイミングと経過時間との関係を示す表。
【図5】実施形態2に係る移動体測位装置の主要構成を示す構成説明図。
【発明を実施するための形態】
【0015】
以下、本発明の実施の形態を図面を参照して説明する。
(実施形態1)
【0016】
図1は、実施形態1に係る移動体測位装置の主要な構成を示す構成説明図である。移動体測位装置は、GPS(Global Positioning System)信号受信部10と、ジャイロセンサーと加速度センサーとを有する慣性センサー20と、時刻同期部30と、カルマンフィルター(KF)40と、INS50と、から構成されている。
【0017】
GPS信号受信部10では、GPS信号を受信し測位演算を行う。メジャメントの取得から測位演算を行い、測位結果を出力するまでには遅延が発生する。そこで、GPSの測位結果には、メジャメント取得時刻と、出力時の時刻と、を付与し1Hzで出力する。さらに、5Hzで時刻情報を出力する。
慣性センサー(加速度センサー、ジャイロセンサー)は、慣性センサーデータ(加速度、角速度)を検出し、100Hzで出力して時刻同期部30のバッファー部に保存する。
【0018】
時刻同期部30は、バッファー部と演算部(共に、図示せず)を有し、以下の機能を有する。
(1)GPS信号受信部10から5Hzで出力される時刻情報と、慣性センサー20から100Hzで出力される慣性センサーデータを対応づけ、慣性センサーデータに時刻情報を付与し、バッファリングする。
(2)GPS信号受信部10から1Hzで出力される測位結果の中の位置の時刻と、出力時の時刻から、GPS信号の受信から測位演算終了までの遅延時間を演算部にて計算する。
(3)計算した遅延時間から、GPSの測位結果とバッファー部内の慣性センサーデータを対応付ける。
(4)GPSの遅延時間と、あらかじめ決められた慣性センサーのWait時間から,GPS測位結果を出力するGPSのWait時間を計算し、GPSの測位結果をバッファリングする。
(5)GPSの測位結果を受け取ってから、GPSのWait時間が経過したとき,バッファー部内に保存していたGPSの測位結果を出力する。
(6)慣性センサー20から慣性センサーデータを受け取ってから、慣性センーのWait時間が経過したとき、バッファー部内に保存していた慣性センサーデータを出力する。
【0019】
INS50は、時刻同期部30(バッファー部)から出力された慣性センサーデータを用いてINS測位演算を行い、INS測位結果を100Hzで出力する。
【0020】
カルマンフィルター(KF)40は、時刻同期部30(バッファー部)から出力されたGPSの測位結果と、慣性センサーデータ、INS50の測位結果からルーズカップリングを行い、位置・速度・姿勢、慣性センサーのバイアス誤差を計算する。
これらの誤差を用いてINS50の測位結果の位置・速度・姿勢誤差を補正する。
【0021】
続いて、移動体測位方法について説明する。
図2は、移動体測位方法の主要工程を示すフローチャートである。まず、慣性センサー20で慣性センサーデータ(加速度および角速度)を検出し(ST1)、100Hzの慣性センサーデータを出力し、時刻同期部30のバッファー部内にバッファリングする(ST2)。続いて、GPS信号を受信し測位演算を行う。測位結果には、メジャメント取得時刻と、出力時の時刻を付与し1Hzで出力する。また、GPS信号受信部10は、5Hzで時刻情報を出力する。
【0022】
次に、GPSの測位検出の有無を判定し(ST3)、GPSの測位検出あり(Yes)の場合、時刻同期部30でGPSの遅延時間を計算する(ST4)。遅延時間は、5Hzで出力される時刻情報と、慣性センサー20から100Hzで出力される慣性センサーデータと、を対応づけ、慣性センサーデータに時刻情報を付与してバッファリングし、1Hzで出力される測位結果の中の位置の時刻と出力時の時刻から計算する。よって、GPSの遅延時間は、次の数式で表すことができる。
遅延時間=現在時刻−GPSの測位時間。
【0023】
次に、GPSのWait時間を計算する(ST5)。Wait時間は、あらかじめ決められた慣性センサーのWait時間と、GPSの遅延時間と、の差であって、次式で表すことができる。
GPSのWait時間=慣性センサーのWait時間−GPSの遅延時間。
【0024】
そして、現在時刻にGPSのWait時間を加えてGPSの出力時刻を計算し、測位結果と共にバッファリングする(ST6)。GPSの出力時刻は、次式で表す。
GPSの出力時刻=現在時刻+GPSのWait時間。
【0025】
次に、現在時刻と、GPSのバッファー部内に保存されているGPSの出力時刻とを比較し、現在時刻≧GPSの出力時刻となる先頭データの出力時刻を選択する(ST7)。
【0026】
なお、前述したST3の工程で、GPSの測位検出がない(No)場合には、新しいGPSデータの情報がない場合であって、上記ST7の工程に移行する。
【0027】
現在時刻≧GPSの出力時刻である場合(Yes)には、GPSの測位結果を受け取ってから、GPSのWait時間が経過したときにバッファー部内に保存していた先頭のGPSの測位結果を出力し(ST8)、出力後、バッファー部内の先頭データを削除する(ST9)。
【0028】
次に、バッファリングされている慣性センサーデータの中から、慣性センサーのWait時間前の慣性センサーデータを出力する(ST10)。つまり、慣性センサー20から慣性センサーデータを受け取ってから、慣性センサーのWait時間が経過したときにバッファー内に保存していた慣性センサーデータを出力する。
【0029】
次に、ST10において出力された慣性センサーデータを用いてINS50で測位演算を行い(ST11)、測位結果を100Hzで出力する。
【0030】
次に、バッファー部から出力された慣性センサーデータと、INSの測位結果と、バッファー部から出力されたGPSの測位結果とを、カルマンフィルター(KF)40によってカップリングし(ST12)、位置・速度・姿勢、および慣性センサーのバイアス誤差推定値を計算する。
【0031】
そして、カップリングによって推定された位置・速度・姿勢の誤差をINS50にフィードバックし(ST13)、INS50の位置・速度・姿勢の誤差を補正する(ST14)。
【0032】
続いて、上記移動体測位方法を用いた時刻同期に係る具体例について説明する。なお、GPSの遅延時間は一定ではなく変化するため、遅延時間が増加する場合と、減少する場合の各1例をあげ、図3、図4を参照して説明する。
図3は、GPSの遅延時間が増加することがある場合のバッファー出力のタイミングを示し、(a)は、慣性センサーデータとGPSデータのタイミングを示す図であり、横軸は経過時間を簡略化して表しており、慣性センサーデータのバッファー出力のタイミングを「001」、「002」、…「051」…「162」と表している。また、GPSデータのバッファー部からの出力のタイミングを「GPS001」、「GPS002」と表している。図3(b)は、図3(a)のバッファー出力のタイミングと経過時間との関係を示す表である。なお、慣性センサーデータのバッファー部からの出力は100Hzであり、例えば、「001」で表される範囲は、10msの幅を有する。また、GPSデータのバッファー部からの出力は1Hzである。なお、図3(a)では、図示右側の「001」が検出の先頭側である。
【0033】
GPSの遅延時間は、「現在時刻(510ms:「051」)−GPSの測位時刻(10ms:「001」)」とすれば、本例ではGPSデータの遅延時間は500msである。GPSのWait時間は、「慣性センサーのWait時間−GPSの遅延時間」である。ここで、慣性センサーのWait時間はあらかじめ600msに設定されているとすれば、「600ms−500ms=100ms」となる。また、GPSのバッファー部からの出力時刻は、「現在時刻+GPSのWait時間」であることから「510ms+100ms=610ms」となる。ここで、慣性センサーデータのバッファー部からの出力は100Hzであり、GPSの測位データは、バッファー部内の先頭データを1Hzで出力することから、経過時間610ms:「061」で時刻同期をとることができる。
【0034】
次に、上記の「GPS001」が出力されてから1s後にGPSデータが出力した場合(「GPS002」)について説明する。GPSの遅延時間は、「現在時刻(1510ms:「151」)−GPSの測位時刻(1000ms:「100」)」とすれば、本例ではGPSデータの遅延時間は510msであり、直前の遅延時間よりも10ms増加している。GPSのWait時間は、「慣性センサーのWait時間−GPSの遅延時間」である。ここで、慣性センサーのWait時間は、あらかじめ600msに設定されているので変化はなく、「600ms−510ms=90ms」となる。また、GPSの出力時刻は、「現在時刻+GPSのWait時間」であることから「1510ms+90ms=1600ms」となる。ここで、慣性センサーデータの出力は100Hzであり、GPSの測位結果は、バッファー部内の先頭データを1Hzで出力されることから、経過時間1600ms:「160」で時刻同期をとることができる。
【0035】
このようにして、バッファー出力された慣性センサーのWait時間前の慣性センサーデータを用いてINSで測位演算を行い、慣性センサーデータと、INSの測位結果と、バッファー出力されたGPSの測位結果を用いてカルマンフィルターによってカップリングすれば、これらのデータは時刻同期がとれているので、正確な位置・速度・姿勢の誤差が推定できる。
【0036】
図4は、図3と同様に表しており、GPSの遅延時間が減少することがある場合のバッファー出力のタイミングを示し、(a)は、慣性センサーデータとGPSデータのタイミングを示す図であり、横軸は経過時間を簡略化して表し、慣性センサーデータのバッファー出力のタイミングを「001」、「002」、…「051」…「162」と表している。また、GPSデータのバッファー部からの出力のタイミングを「GPS001」、「GPS002」と表している。図4(b)は、図4(a)のバッファー出力のタイミングと経過時間との関係を示す表である。なお、慣性センサーデータのバッファー部からの出力は100Hz、GPSデータのバッファー部からの出力は1Hzである。また、図4(a)では、図示右側の「001」が検出の先頭側である。
なお、GPSの遅延時間が500msの場合は、図3で説明したGPSデータのバッファー部からの出力「GPS001」の場合と同じなので説明を省略する。
【0037】
上記の「GPS001」が出力されてから1s後にGPSデータが出力した場合(「GPS002」)について説明する。GPSの遅延時間は、「現在時刻(1510ms:「151」)−GPSの測位時刻(1020ms:「102」)」とすれば、本例ではGPSデータの遅延時間は490msであり、直前の遅延時間よりも10ms減少している。GPSのWait時間は、「慣性センサーのWait時間−GPSの遅延時間」である。ここで、慣性センサーのWait時間はあらかじめ600msに設定されているとすれば変化はなく、「600ms−490ms=110ms」となり、直前のWait時間よりも10ms長くなる。また、GPSの出力時刻は、「現在時刻+GPSのWait時間」であることから「1510ms+110ms=1620ms」となる。ここで、慣性センサーデータのバッファー部からの出力は100Hzであり、GPSの測位データは、バッファー部内の先頭データを1Hzで出力することから、経過時間1620ms:「162」で時刻同期をとることができる。
【0038】
このようにして、バッファー出力された慣性センサーのWait時間前の慣性センサーデータを用いてINSで測位演算を行い、慣性センサーデータと、INSの測位結果と、バッファー出力されたGPSの測位結果を用いてカルマンフィルターによってカップリングすれば、これらのデータは時刻同期がとれているので、正確な位置・速度・姿勢の誤差を推定することができる。
【0039】
図3、図4を参照して説明したように、GPSの遅延時間が増減しても、GPSデータと慣性センサーデータとをバッファリングして時刻同期をとることが可能である。
【0040】
以上説明したように、慣性センサーの検出値と、GPSの出力時刻とGPSの測位結果と、をバッファリングする。そして、GPSの遅延時間と、GPSのWait時間と、GPSの出力時刻と、を用いてGPS測位データの出力時刻を計算し、慣性センサーの測位結果との時刻同期をとる。このことによって、従来技術のように算出した時間差分をプロパゲーションしてGPS測位データと慣性センサーデータとの時刻同期を行う方法に対し、時刻ずれの影響を排除し、カップリングによる位置・速度・姿勢の推定誤差の計算精度を向上させる。このことからINS測位結果の高精度な位置・速度・姿勢誤差の補正を行うことができ、移動体の加減速で速度の大きさが一定に保たなかったり、回転で速度の方向が変化したりする場合においても、移動体の正確な測位を行うことができる。また、小型化、低コスト化が可能ではあるが、バイアス誤差及びランダムドリフトの変動が大きなMEMS慣性センサーを用いる場合でも、トンネルや屋内において、移動体の高精度な制御が可能となる。
(実施形態2)
【0041】
図5は、実施形態2に係る移動体測位装置の主要構成を示す構成説明図である。移動体測位装置は、実施形態1(図1、参照)の構成にリアルタイムINS60(Real Time INS)を、さらに備えて構成している。GPS信号受信部10、慣性センサー20、時刻同期部30、カルマンフィルター(KF)40、INS50の機能は、実施形態1と同じであるので説明を省略する。
【0042】
リアルタイムINS60には、慣性センサーの検出値と、カルマンフィルター40を用いてカップリングして推定された位置・速度・姿勢の推定誤差とが入力される。慣性センサーの検出値は、ほぼリアルタイムでリアルタイムINS60に入力される。この慣性センサーの検出値を用いて、INSによる測位演算を行うと共に、位置・速度・姿勢の推定誤差を補正し、出力する。
【0043】
ところで、カルマンフィルター40からの出力は慣性センサーのWait時間だけ遅延がある。そこで、慣性センサーの検出値を用いてリアルタイムなINSによる測位演算を行い、時刻同期されたGPSデータおよび慣性センサーデータを用いてカップリングして算出される位置・速度・姿勢の誤差を補正することで、遅延なく正確なINSの測位結果を出力することができる。
【0044】
なお、本発明は前述の実施の形態に限定されるものではなく、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれるものである。
例えば、前述した実施形態、実施形態2では、GPS信号受信部からは測位結果を出力し、ルーズカップリングしているが、タイトカップリングをする場合には、GPS信号受信部からはメジャメントデータ(PR,RR)を出力する方法を採用できる。
【符号の説明】
【0045】
10…GPS信号受信部、20…慣性センサー、30…時刻同期部、40…カルマンフィルター(KF)、50…INS。

【特許請求の範囲】
【請求項1】
慣性センサーデータをバッファリングすることと、
バッファリングされた前記慣性センサーデータを用いてINS測位演算を行うことと、
現在時刻に対するGPS測位の遅延時間を計算することと、
GPSデータの出力時刻とGPS測位結果とをバッファリングすることと、
バッファリングされた前記GPS測位結果とINS測位結果との時刻同期をとることと、
バッファリングされた前記慣性センサーデータと、前記INS測位結果と、バッファリングされた前記GPS測位結果と、をカップリングし、位置・速度・姿勢の誤差を推定することと、
前記位置・速度・姿勢の誤差を前記INS測位結果にフィードバックし、補正することと、
を含むことを特徴とする移動体測位方法。
【請求項2】
請求項1に記載の移動体測位方法において、
慣性センサーの検出値を用いて、INS測位演算を行うことと、
前記推定された位置・速度・姿勢の誤差を補正することと、
をさらに含むことを特徴とする移動体測位方法。
【請求項3】
慣性センサーデータを取得する慣性センサーと、
GPSデータを受信し、GPS測位演算を行うGPS信号受信部と、
前記慣性センサーデータを用いて測位演算を行うINSと、
前記慣性センサーデータと、GPS測位結果と出力時刻と、をバッファリングし、時刻同期をとる時刻同期部と、
バッファリングされた前記慣性データと、INS測位結果と、バッファリングされた前記GPS測位結果と、を用いてカップリングし、位置・速度・姿勢の誤差を推定するカルマンフィルターと、
を備え、
前記推定された位置・速度・姿勢の誤差を前記INSにフィードバックし、補正することを特徴とする移動体測位装置。
【請求項4】
請求項3に記載の移動体測位装置において、
前記慣性センサーの検出値を用いてINS測位演算を行い、前記推定された位置・速度・姿勢の誤差を補正するリアルタイムINSと、
をさらに備えることを特徴とする移動体測位装置。

【図1】
image rotate

【図2】
image rotate

【図3】
image rotate

【図4】
image rotate

【図5】
image rotate


【公開番号】特開2012−177564(P2012−177564A)
【公開日】平成24年9月13日(2012.9.13)
【国際特許分類】
【出願番号】特願2011−39522(P2011−39522)
【出願日】平成23年2月25日(2011.2.25)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】