JP2012177564A - Mobile body positioning method, and mobile body positioning device - Google Patents
Mobile body positioning method, and mobile body positioning device Download PDFInfo
- Publication number
- JP2012177564A JP2012177564A JP2011039522A JP2011039522A JP2012177564A JP 2012177564 A JP2012177564 A JP 2012177564A JP 2011039522 A JP2011039522 A JP 2011039522A JP 2011039522 A JP2011039522 A JP 2011039522A JP 2012177564 A JP2012177564 A JP 2012177564A
- Authority
- JP
- Japan
- Prior art keywords
- gps
- time
- positioning
- inertial sensor
- ins
- 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.)
- Withdrawn
Links
Images
Landscapes
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
Description
本発明は、移動体測位方法、および移動体測位装置に関する。 The present invention relates to a moving body positioning method and a moving body positioning apparatus.
GPS測位データと慣性センサーデータを融合して、屋内外を問わず高精度な測位を行う場合、GPS測位データは伝送時間および測位計算時間の影響で現在時刻に対して遅延時間が発生してしまうため、GPS測位データと慣性センサーデータとの時刻同期を行う必要がある。GPS測位データと慣性センサーデータとの時刻同期をとる方法としては、GPS測位データが到達した時刻から測位演算時間と伝送時間を差し引いた時刻をGPS基準時刻とし、慣性センサーの検出結果のうちから、基準時刻に最も近い検出結果を同期させるべき検出データと特定し、特定された検出結果の検出時刻とGPS基準時刻の差分を算出し、算出した時間差分をプロパゲーションしてGPS測位データと慣性センサーデータとの時刻同期を行うものが知られている(例えば、特許文献1参照)。 When GPS positioning data and inertial sensor data are fused and high-precision positioning is performed regardless of whether indoors or outdoors, the GPS positioning data will have a delay time relative to the current time due to the effect of transmission time and positioning calculation time. Therefore, it is necessary to perform time synchronization between the GPS positioning data and the inertial sensor data. As a method of synchronizing the time of the GPS positioning data and inertial sensor data, the time obtained by subtracting the positioning calculation time and the transmission time from the time when the GPS positioning data arrived is set as the GPS reference time, and from the detection result of the inertial sensor, The detection result closest to the reference time is specified as detection data to be synchronized, the difference between the detection time of the specified detection result and the GPS reference time is calculated, and the calculated time difference is propagated to obtain GPS positioning data and an inertial sensor. One that performs time synchronization with data is known (for example, see Patent Document 1).
このような特許文献1による測位方法では、慣性センサーの測位結果とGPS測位結果とを同期するために、慣性センサーの検出時刻とGPSの基準時刻との差分ΔTpを算出し、外挿法(外挿補間と呼称することがある)により、移動体の速度ベクトルに時間差ΔTpを乗算して、周期iにおいて測位された移動体の位置に積算して、移動体の位置を補正し、センサーの検出結果と対応付けを行う。このような同期方法では、移動体が加減速で速度の大きさが一定に保たなかったり、回転で速度の方向が変化したりする場合に、外挿法によって補正された移動体の位置は、検出時刻における移動体の真の位置と大きくずれる可能性がある。また、速度や方位の外挿補間は困難であることから、GPS測位結果と慣性センサーの検出結果の同期がとれず、正確にカップリングすることができないという課題があった。 In such a positioning method according to Patent Document 1, in order to synchronize the positioning result of the inertial sensor and the GPS positioning result, a difference ΔTp between the detection time of the inertial sensor and the GPS reference time is calculated, and an extrapolation method (external (Sometimes referred to as interpolation), the speed vector of the moving object is multiplied by the time difference ΔTp and added to the position of the moving object measured in the period i to correct the position of the moving object and detect the sensor. Associate with results. In such a synchronization method, when the moving body is accelerated and decelerated and the magnitude of the speed is not kept constant, or the direction of the speed changes due to rotation, the position of the moving body corrected by the extrapolation method is , There is a possibility that the position of the moving body at the detection time is greatly deviated. In addition, since extrapolation of speed and direction is difficult, there is a problem that the GPS positioning result and the detection result of the inertial sensor cannot be synchronized and cannot be accurately coupled.
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。 SUMMARY An advantage of some aspects of the invention is to solve at least a part of the problems described above, and the invention can be implemented as the following forms or application examples.
[適用例1]本適用例に係る移動体測位方法は、慣性センサーデータをバッファリングすることと、バッファリングされた前記慣性センサーデータを用いてINS測位演算を行うことと、現在時刻に対するGPS測位の遅延時間を計算することと、GPSデータの出力時刻とGPS測位結果とをバッファリングすることと、バッファリングされた前記GPS測位結果とINS測位結果との時刻同期をとることと、バッファリングされた前記慣性センサーデータと、前記INS測位結果と、バッファリングされた前記GPS測位結果と、をカップリングし、位置・速度・姿勢の誤差を推定することと、前記位置・速度・姿勢の誤差を前記INS測位結果にフィードバックし、補正することと、を含むことを特徴とする。 Application Example 1 A mobile body positioning method according to this application example includes buffering inertial sensor data, performing INS positioning calculation using the buffered inertial sensor data, and GPS positioning with respect to the current time. Buffering the GPS data output time and GPS positioning result, buffering the buffered GPS positioning result and INS positioning result, and buffering The inertial sensor data, the INS positioning result, and the buffered GPS positioning result are coupled to estimate the position / velocity / attitude error, and the position / velocity / attitude error is calculated. Feedback and correction to the INS positioning result.
本適用例によれば、慣性センサーデータと、GPSデータの出力時刻とGPS測位結果と、をバッファリング(保存)し、GPS測位結果の遅延時間を用いてGPS測位データの出力時刻を計算し、INS測位結果との時刻同期をとることによって、従来技術のように算出した時間差分をプロパゲーションしてGPS測位データと慣性センサーデータとの時刻同期を行う方法に対し、時刻ずれの影響を排除し、カップリングによる位置・速度・姿勢の推定誤差の計算精度が向上する。よって、INS測位結果の高精度な位置・速度・姿勢の補正を行うことができることから、移動体の加減速で速度の大きさが一定に保たなかったり、回転で速度の方向が変化したりする場合においても、移動体の正確な測位を行うことができる。また、小型化、低コスト化が可能ではあるが、バイアス誤差及びランダムドリフトの変動が大きなMEMS(Micro Electro Mechanical System)慣性センサーを用いる場合でも、トンネルや屋内において、移動体の高精度な制御が可能である。 According to this application example, the inertial sensor data, the output time of the GPS data, and the GPS positioning result are buffered (saved), and the output time of the GPS positioning data is calculated using the delay time of the GPS positioning result. By synchronizing the time with the INS positioning result, the time difference calculated as in the prior art is propagated to eliminate the effect of time lag from the time synchronization between GPS positioning data and inertial sensor data. The accuracy of calculation of position / speed / posture estimation errors due to coupling is improved. Therefore, since the position, speed, and posture of the INS positioning result can be corrected with high accuracy, the magnitude of the speed cannot be kept constant by the acceleration / deceleration of the moving body, or the speed direction can be changed by the rotation. Even in this case, accurate positioning of the moving body can be performed. In addition, miniaturization and cost reduction are possible, but even when using a micro electro mechanical system (MEMS) inertial sensor with large fluctuations in bias error and random drift, high-precision control of moving objects can be achieved in tunnels and indoors. Is possible.
[適用例2]上記適用例に係る移動体測位方法は、慣性センサーの検出値を用いて、INS測位演算を行うことと、前記推定された位置・速度・姿勢の誤差を補正することと、をさらに含むことが好ましい。 Application Example 2 The mobile body positioning method according to the application example described above includes performing an INS positioning calculation using a detection value of the inertial sensor, correcting the estimated position / velocity / posture error, It is preferable that it is further included.
カルマンフィルター(KF)を用いてカップリングを行う場合、カルマフィルターからの出力は、慣性センサーのWait時間だけ遅延が発生する。そこで、慣性センサーの検出値を用いてリアルタイムでINS測位演算を行い、時刻同期されたGPSデータおよび慣性センサーデータを用いてカップリングして算出される位置・速度・姿勢の誤差を補正することで、遅延なく測位結果を出力することができる。 When coupling is performed using a Kalman filter (KF), the output from the karma filter is delayed by the wait time of the inertial sensor. Therefore, by performing INS positioning calculation in real time using the detection value of the inertial sensor, and correcting the position / velocity / posture error calculated by coupling using GPS data and inertial sensor data synchronized in time. The positioning result can be output without delay.
[適用例3]本適用例に係る移動体制御装置は、慣性センサーデータを取得する慣性センサーと、GPSデータを受信し、GPS測位演算を行うGPS信号受信部と、前記慣性センサーデータを用いて測位演算を行うINSと、前記慣性センサーデータと、GPS測位結果と出力時刻と、をバッファリングし、時刻同期をとる時刻同期部と、バッファリングされた前記慣性データと、INS測位結果と、バッファリングされた前記GPS測位結果と、を用いてカップリングし、位置・速度・姿勢の誤差を推定するカルマンフィルターと、を備え、前記推定された位置・速度・姿勢の誤差を前記INSにフィードバックし、補正することを特徴とする。
本適用例では、INS(Inertial Navigation system)は、慣性航法を用いた測位演算部をさす。
Application Example 3 A mobile control device according to this application example uses an inertial sensor that acquires inertial sensor data, a GPS signal reception unit that receives GPS data and performs GPS positioning calculation, and the inertial sensor data. INS that performs positioning calculation, inertial sensor data, GPS positioning result and output time are buffered to synchronize time, a buffered time synchronization unit, buffered inertial data, INS positioning result, buffer And a Kalman filter that estimates a position / velocity / posture error by coupling using the GPS positioning result thus ringed back, and feeds back the estimated position / velocity / posture error to the INS. It is characterized by correcting.
In this application example, INS (Inertial Navigation system) indicates a positioning calculation unit using inertial navigation.
本適用例によれば、時刻同期部によって、慣性センサーの検出値と、前記出力時刻とGPS測位結果と、GPSの遅延時間を用いて算出したGPS測位結果の出力時刻と、INS測位結果とをバッファリングして時刻同期をとり、カルマンフィルターを用いて、位置・速度・姿勢の誤差を推定する。このことから、位置・速度・姿勢の推定誤差の計算精度が向上する。よって、INS測位結果の高精度な位置・速度・姿勢の誤差補正を行うことができる。 According to this application example, the detection value of the inertial sensor, the output time and the GPS positioning result, the output time of the GPS positioning result calculated using the GPS delay time, and the INS positioning result are obtained by the time synchronization unit. Buffer and synchronize the time, and estimate position / velocity / attitude errors using the Kalman filter. This improves the calculation accuracy of position / velocity / attitude estimation errors. Therefore, highly accurate position / velocity / posture error correction of the INS positioning result can be performed.
[適用例4]上記適用例に係る移動体制御装置は、前記慣性センサーの検出値を用いてINS測位演算を行い、前記推定された位置・速度・姿勢の誤差を補正するリアルタイムINSと、をさらに備えることが好ましい。 Application Example 4 The mobile control device according to the application example performs real time INS that performs INS positioning calculation using the detection value of the inertial sensor and corrects the estimated position / velocity / posture error. It is preferable to further provide.
慣性センサーの検出値を用いてリアルタイムでINS測位演算を行い、時刻同期されたGPSデータおよび慣性センサーデータをカップリングすることによって、位置・速度・姿勢の誤差を正確に推定し、補正することで、遅延なく測位結果を出力することができる。 By performing the INS positioning calculation in real time using the detection value of the inertial sensor and coupling the time-synchronized GPS data and inertial sensor data, the position / velocity / posture error is accurately estimated and corrected. The positioning result can be output without delay.
以下、本発明の実施の形態を図面を参照して説明する。
(実施形態1)
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
(Embodiment 1)
図1は、実施形態1に係る移動体測位装置の主要な構成を示す構成説明図である。移動体測位装置は、GPS(Global Positioning System)信号受信部10と、ジャイロセンサーと加速度センサーとを有する慣性センサー20と、時刻同期部30と、カルマンフィルター(KF)40と、INS50と、から構成されている。
FIG. 1 is a configuration explanatory diagram illustrating a main configuration of the mobile body positioning device according to the first embodiment. The mobile body positioning device includes a GPS (Global Positioning System)
GPS信号受信部10では、GPS信号を受信し測位演算を行う。メジャメントの取得から測位演算を行い、測位結果を出力するまでには遅延が発生する。そこで、GPSの測位結果には、メジャメント取得時刻と、出力時の時刻と、を付与し1Hzで出力する。さらに、5Hzで時刻情報を出力する。
慣性センサー(加速度センサー、ジャイロセンサー)は、慣性センサーデータ(加速度、角速度)を検出し、100Hzで出力して時刻同期部30のバッファー部に保存する。
The GPS
The inertial sensor (acceleration sensor, gyro sensor) detects inertial sensor data (acceleration, angular velocity), outputs it at 100 Hz, and stores it in the buffer unit of the
時刻同期部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時間が経過したとき、バッファー部内に保存していた慣性センサーデータを出力する。
The
(1) The time information output at 5 Hz from the
(2) The calculation unit calculates a delay time from the reception of the GPS signal to the end of the positioning calculation from the time of the position in the positioning result output at 1 Hz from the GPS
(3) The GPS positioning result is associated with the inertial sensor data in the buffer unit from the calculated delay time.
(4) The GPS wait time for outputting the GPS positioning result is calculated from the GPS delay time and the predetermined inertia sensor wait time, and the GPS positioning result is buffered.
(5) When the GPS wait time has elapsed after receiving the GPS positioning result, the GPS positioning result stored in the buffer unit is output.
(6) When the inertia sensor wait time elapses after the inertia sensor data is received from the
INS50は、時刻同期部30(バッファー部)から出力された慣性センサーデータを用いてINS測位演算を行い、INS測位結果を100Hzで出力する。 The INS 50 performs INS positioning calculation using inertial sensor data output from the time synchronization unit 30 (buffer unit), and outputs the INS positioning result at 100 Hz.
カルマンフィルター(KF)40は、時刻同期部30(バッファー部)から出力されたGPSの測位結果と、慣性センサーデータ、INS50の測位結果からルーズカップリングを行い、位置・速度・姿勢、慣性センサーのバイアス誤差を計算する。
これらの誤差を用いてINS50の測位結果の位置・速度・姿勢誤差を補正する。
The Kalman filter (KF) 40 performs loose coupling from the GPS positioning result output from the time synchronization unit 30 (buffer unit), the inertial sensor data, and the positioning result of the
Using these errors, the position / velocity / posture error of the positioning result of the INS 50 is corrected.
続いて、移動体測位方法について説明する。
図2は、移動体測位方法の主要工程を示すフローチャートである。まず、慣性センサー20で慣性センサーデータ(加速度および角速度)を検出し(ST1)、100Hzの慣性センサーデータを出力し、時刻同期部30のバッファー部内にバッファリングする(ST2)。続いて、GPS信号を受信し測位演算を行う。測位結果には、メジャメント取得時刻と、出力時の時刻を付与し1Hzで出力する。また、GPS信号受信部10は、5Hzで時刻情報を出力する。
Next, the mobile body positioning method will be described.
FIG. 2 is a flowchart showing main steps of the mobile body positioning method. First, inertial sensor data (acceleration and angular velocity) is detected by the inertial sensor 20 (ST1), 100 Hz inertial sensor data is output, and buffered in the buffer unit of the time synchronization unit 30 (ST2). Subsequently, a GPS signal is received and a positioning calculation is performed. The measurement result is given a measurement acquisition time and an output time, and is output at 1 Hz. The
次に、GPSの測位検出の有無を判定し(ST3)、GPSの測位検出あり(Yes)の場合、時刻同期部30でGPSの遅延時間を計算する(ST4)。遅延時間は、5Hzで出力される時刻情報と、慣性センサー20から100Hzで出力される慣性センサーデータと、を対応づけ、慣性センサーデータに時刻情報を付与してバッファリングし、1Hzで出力される測位結果の中の位置の時刻と出力時の時刻から計算する。よって、GPSの遅延時間は、次の数式で表すことができる。
遅延時間=現在時刻−GPSの測位時間。
Next, it is determined whether or not GPS positioning is detected (ST3). If GPS positioning is detected (Yes), the
Delay time = current time−GPS positioning time.
次に、GPSのWait時間を計算する(ST5)。Wait時間は、あらかじめ決められた慣性センサーのWait時間と、GPSの遅延時間と、の差であって、次式で表すことができる。
GPSのWait時間=慣性センサーのWait時間−GPSの遅延時間。
Next, the GPS wait time is calculated (ST5). The wait time is a difference between a predetermined wait time of the inertial sensor and the GPS delay time, and can be expressed by the following equation.
GPS wait time = Inertial sensor wait time−GPS delay time.
そして、現在時刻にGPSのWait時間を加えてGPSの出力時刻を計算し、測位結果と共にバッファリングする(ST6)。GPSの出力時刻は、次式で表す。
GPSの出力時刻=現在時刻+GPSのWait時間。
Then, the GPS output time is calculated by adding the GPS wait time to the current time, and buffered together with the positioning result (ST6). The GPS output time is expressed by the following equation.
GPS output time = current time + GPS wait time.
次に、現在時刻と、GPSのバッファー部内に保存されているGPSの出力時刻とを比較し、現在時刻≧GPSの出力時刻となる先頭データの出力時刻を選択する(ST7)。 Next, the current time is compared with the GPS output time stored in the GPS buffer unit, and the output time of the top data that satisfies the current time ≧ GPS output time is selected (ST7).
なお、前述したST3の工程で、GPSの測位検出がない(No)場合には、新しいGPSデータの情報がない場合であって、上記ST7の工程に移行する。 If there is no GPS positioning detection (No) in step ST3 described above, there is no new GPS data information, and the process proceeds to step ST7.
現在時刻≧GPSの出力時刻である場合(Yes)には、GPSの測位結果を受け取ってから、GPSのWait時間が経過したときにバッファー部内に保存していた先頭のGPSの測位結果を出力し(ST8)、出力後、バッファー部内の先頭データを削除する(ST9)。 If the current time is equal to or greater than the GPS output time (Yes), the GPS positioning result stored in the buffer when the GPS wait time has elapsed since the GPS positioning result was received is output. (ST8) After the output, the top data in the buffer section is deleted (ST9).
次に、バッファリングされている慣性センサーデータの中から、慣性センサーのWait時間前の慣性センサーデータを出力する(ST10)。つまり、慣性センサー20から慣性センサーデータを受け取ってから、慣性センサーのWait時間が経過したときにバッファー内に保存していた慣性センサーデータを出力する。
Next, the inertial sensor data before the Wait time of the inertial sensor is output from the buffered inertial sensor data (ST10). That is, after the inertial sensor data is received from the
次に、ST10において出力された慣性センサーデータを用いてINS50で測位演算を行い(ST11)、測位結果を100Hzで出力する。 Next, positioning calculation is performed at INS50 using the inertial sensor data output at ST10 (ST11), and the positioning result is output at 100 Hz.
次に、バッファー部から出力された慣性センサーデータと、INSの測位結果と、バッファー部から出力されたGPSの測位結果とを、カルマンフィルター(KF)40によってカップリングし(ST12)、位置・速度・姿勢、および慣性センサーのバイアス誤差推定値を計算する。 Next, the inertial sensor data output from the buffer unit, the INS positioning result, and the GPS positioning result output from the buffer unit are coupled by the Kalman filter (KF) 40 (ST12), and the position / velocity Calculate attitude and inertial sensor bias error estimates.
そして、カップリングによって推定された位置・速度・姿勢の誤差をINS50にフィードバックし(ST13)、INS50の位置・速度・姿勢の誤差を補正する(ST14)。
Then, the position / velocity / posture error estimated by the coupling is fed back to the INS 50 (ST13), and the position / velocity / posture error of the
続いて、上記移動体測位方法を用いた時刻同期に係る具体例について説明する。なお、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」が検出の先頭側である。
Next, a specific example related to time synchronization using the mobile body positioning method will be described. Since the GPS delay time is not constant and changes, an example of each of cases where the delay time increases and decreases will be described with reference to FIGS. 3 and 4.
FIG. 3 shows the buffer output timing when the GPS delay time may increase. FIG. 3A is a diagram showing the timing of inertial sensor data and GPS data, and the horizontal axis simplifies the elapsed time. The buffer output timing of inertial sensor data is expressed as “001”, “002”,... “051”. In addition, the timing of outputting the GPS data from the buffer unit is represented as “GPS001” and “GPS002”. FIG. 3B is a table showing the relationship between the buffer output timing and the elapsed time in FIG. The output of the inertial sensor data from the buffer unit is 100 Hz. For example, the range represented by “001” has a width of 10 ms. The output of the GPS data from the buffer unit is 1 Hz. In FIG. 3A, “001” on the right side of the drawing is the head of detection.
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」で時刻同期をとることができる。 If the GPS delay time is “current time (510 ms:“ 051 ”) − GPS positioning time (10 ms:“ 001 ”)”, the delay time of GPS data is 500 ms in this example. The GPS wait time is “inertial sensor wait time−GPS delay time”. Here, if the wait time of the inertial sensor is set to 600 ms in advance, “600 ms−500 ms = 100 ms”. The output time from the GPS buffer is “510 ms + 100 ms = 610 ms” because it is “current time + GPS wait time”. Here, the output of the inertial sensor data from the buffer unit is 100 Hz, and the GPS positioning data outputs the top data in the buffer unit at 1 Hz. Therefore, time synchronization can be achieved at an elapsed time of 610 ms: “061”. it can.
次に、上記の「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」で時刻同期をとることができる。 Next, a case where GPS data is output 1 second after “GPS001” is output (“GPS002”) will be described. Assuming that the GPS delay time is “current time (1510 ms:“ 151 ”) − GPS positioning time (1000 ms:“ 100 ”)”, in this example, the delay time of GPS data is 510 ms. It is increased by 10 ms. The GPS wait time is “inertial sensor wait time−GPS delay time”. Here, the wait time of the inertial sensor is set to 600 ms in advance, so there is no change and is “600 ms−510 ms = 90 ms”. Further, since the GPS output time is “current time + GPS wait time”, it is “1510 ms + 90 ms = 1600 ms”. Here, the output of the inertial sensor data is 100 Hz, and the GPS positioning result is output at the head data in the buffer unit at 1 Hz, so that time synchronization can be achieved at an elapsed time of 1600 ms: “160”.
このようにして、バッファー出力された慣性センサーのWait時間前の慣性センサーデータを用いてINSで測位演算を行い、慣性センサーデータと、INSの測位結果と、バッファー出力されたGPSの測位結果を用いてカルマンフィルターによってカップリングすれば、これらのデータは時刻同期がとれているので、正確な位置・速度・姿勢の誤差が推定できる。 In this way, the positioning calculation is performed by INS using the inertial sensor data before the Wait time of the inertial sensor output by the buffer, and the inertial sensor data, the INS positioning result, and the GPS positioning result output by the buffer are used. If the data is coupled using the Kalman filter, these data are time-synchronized, so that accurate position / velocity / posture errors can be estimated.
図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」の場合と同じなので説明を省略する。
FIG. 4 is the same as FIG. 3 and shows the buffer output timing when the GPS delay time may decrease. FIG. 4A shows the timing of inertial sensor data and GPS data. The horizontal axis represents the elapsed time in a simplified manner, and the buffer output timing of inertial sensor data is represented as “001”, “002”,... “051”. In addition, the timing of outputting the GPS data from the buffer unit is represented as “GPS001” and “GPS002”. FIG. 4B is a table showing the relationship between the buffer output timing and the elapsed time in FIG. The output of the inertial sensor data from the buffer unit is 100 Hz, and the output of the GPS data buffer unit is 1 Hz. Further, in FIG. 4A, “001” on the right side of the drawing is the detection start side.
The GPS delay time of 500 ms is the same as the case of the output “GPS001” from the GPS data buffer unit described in FIG.
上記の「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」で時刻同期をとることができる。 A case where GPS data is output 1 s after “GPS001” is output (“GPS002”) will be described. Assuming that the GPS delay time is “current time (1510 ms:“ 151 ”) − GPS positioning time (1020 ms:“ 102 ”)”, in this example, the delay time of GPS data is 490 ms. Is reduced by 10 ms. The GPS wait time is “inertial sensor wait time−GPS delay time”. Here, if the wait time of the inertial sensor is set to 600 ms in advance, there is no change and becomes “600 ms−490 ms = 110 ms”, which is 10 ms longer than the immediately previous wait time. Further, since the GPS output time is “current time + GPS wait time”, “1510 ms + 110 ms = 1620 ms”. Here, the output of the inertial sensor data from the buffer unit is 100 Hz, and the GPS positioning data outputs the top data in the buffer unit at 1 Hz. Therefore, time synchronization can be achieved at an elapsed time of 1620 ms: “162”. it can.
このようにして、バッファー出力された慣性センサーのWait時間前の慣性センサーデータを用いてINSで測位演算を行い、慣性センサーデータと、INSの測位結果と、バッファー出力されたGPSの測位結果を用いてカルマンフィルターによってカップリングすれば、これらのデータは時刻同期がとれているので、正確な位置・速度・姿勢の誤差を推定することができる。 In this way, the positioning calculation is performed by INS using the inertial sensor data before the Wait time of the inertial sensor output by the buffer, and the inertial sensor data, the INS positioning result, and the GPS positioning result output by the buffer are used. If the data is coupled using the Kalman filter, these data are time-synchronized, so that accurate position / velocity / posture errors can be estimated.
図3、図4を参照して説明したように、GPSの遅延時間が増減しても、GPSデータと慣性センサーデータとをバッファリングして時刻同期をとることが可能である。 As described with reference to FIGS. 3 and 4, even if the GPS delay time is increased or decreased, the GPS data and the inertial sensor data can be buffered to synchronize the time.
以上説明したように、慣性センサーの検出値と、GPSの出力時刻とGPSの測位結果と、をバッファリングする。そして、GPSの遅延時間と、GPSのWait時間と、GPSの出力時刻と、を用いてGPS測位データの出力時刻を計算し、慣性センサーの測位結果との時刻同期をとる。このことによって、従来技術のように算出した時間差分をプロパゲーションしてGPS測位データと慣性センサーデータとの時刻同期を行う方法に対し、時刻ずれの影響を排除し、カップリングによる位置・速度・姿勢の推定誤差の計算精度を向上させる。このことからINS測位結果の高精度な位置・速度・姿勢誤差の補正を行うことができ、移動体の加減速で速度の大きさが一定に保たなかったり、回転で速度の方向が変化したりする場合においても、移動体の正確な測位を行うことができる。また、小型化、低コスト化が可能ではあるが、バイアス誤差及びランダムドリフトの変動が大きなMEMS慣性センサーを用いる場合でも、トンネルや屋内において、移動体の高精度な制御が可能となる。
(実施形態2)
As described above, the detection value of the inertia sensor, the GPS output time, and the GPS positioning result are buffered. Then, the output time of the GPS positioning data is calculated using the GPS delay time, the GPS wait time, and the GPS output time, and time synchronization with the positioning result of the inertial sensor is obtained. This eliminates the effects of time lags compared to the method of performing time synchronization between GPS positioning data and inertial sensor data by propagating the time difference calculated as in the prior art, and the position / velocity / Improve calculation accuracy of posture estimation error. This makes it possible to correct the INS positioning results with high accuracy in position, speed, and attitude errors. The acceleration / deceleration of the moving body does not keep the magnitude of the speed constant, or the direction of the speed changes due to rotation. Even in such a case, accurate positioning of the moving body can be performed. In addition, although miniaturization and cost reduction are possible, even when a MEMS inertial sensor with large fluctuations in bias error and random drift is used, it is possible to control a moving body with high accuracy in a tunnel or indoor.
(Embodiment 2)
図5は、実施形態2に係る移動体測位装置の主要構成を示す構成説明図である。移動体測位装置は、実施形態1(図1、参照)の構成にリアルタイムINS60(Real Time INS)を、さらに備えて構成している。GPS信号受信部10、慣性センサー20、時刻同期部30、カルマンフィルター(KF)40、INS50の機能は、実施形態1と同じであるので説明を省略する。
FIG. 5 is a configuration explanatory diagram illustrating a main configuration of the mobile body positioning device according to the second embodiment. The mobile body positioning device further includes a real time INS 60 (Real Time INS) in the configuration of the first embodiment (see FIG. 1). Since the functions of the GPS
リアルタイムINS60には、慣性センサーの検出値と、カルマンフィルター40を用いてカップリングして推定された位置・速度・姿勢の推定誤差とが入力される。慣性センサーの検出値は、ほぼリアルタイムでリアルタイムINS60に入力される。この慣性センサーの検出値を用いて、INSによる測位演算を行うと共に、位置・速度・姿勢の推定誤差を補正し、出力する。
The real-
ところで、カルマンフィルター40からの出力は慣性センサーのWait時間だけ遅延がある。そこで、慣性センサーの検出値を用いてリアルタイムなINSによる測位演算を行い、時刻同期されたGPSデータおよび慣性センサーデータを用いてカップリングして算出される位置・速度・姿勢の誤差を補正することで、遅延なく正確なINSの測位結果を出力することができる。
By the way, the output from the
なお、本発明は前述の実施の形態に限定されるものではなく、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれるものである。
例えば、前述した実施形態、実施形態2では、GPS信号受信部からは測位結果を出力し、ルーズカップリングしているが、タイトカップリングをする場合には、GPS信号受信部からはメジャメントデータ(PR,RR)を出力する方法を採用できる。
It should be noted that the present invention is not limited to the above-described embodiment, but includes modifications and improvements as long as the object of the present invention can be achieved.
For example, in the above-described embodiment and Embodiment 2, a positioning result is output from the GPS signal receiving unit and loose coupling is performed. However, in the case of tight coupling, measurement data (from the GPS signal receiving unit ( PR, RR) can be used.
10…GPS信号受信部、20…慣性センサー、30…時刻同期部、40…カルマンフィルター(KF)、50…INS。
DESCRIPTION OF
Claims (4)
バッファリングされた前記慣性センサーデータを用いてINS測位演算を行うことと、
現在時刻に対するGPS測位の遅延時間を計算することと、
GPSデータの出力時刻とGPS測位結果とをバッファリングすることと、
バッファリングされた前記GPS測位結果とINS測位結果との時刻同期をとることと、
バッファリングされた前記慣性センサーデータと、前記INS測位結果と、バッファリングされた前記GPS測位結果と、をカップリングし、位置・速度・姿勢の誤差を推定することと、
前記位置・速度・姿勢の誤差を前記INS測位結果にフィードバックし、補正することと、
を含むことを特徴とする移動体測位方法。 Buffering inertial sensor data;
Performing an INS positioning calculation using the buffered inertial sensor data;
Calculating a GPS positioning delay time relative to the current time;
Buffering the output time of GPS data and GPS positioning results;
Time synchronization between the buffered GPS positioning result and the INS positioning result;
Coupling the buffered inertial sensor data, the INS positioning results, and the buffered GPS positioning results to estimate position / velocity / attitude errors;
Feeding back and correcting the position / velocity / posture error to the INS positioning result;
A mobile body positioning method comprising:
慣性センサーの検出値を用いて、INS測位演算を行うことと、
前記推定された位置・速度・姿勢の誤差を補正することと、
をさらに含むことを特徴とする移動体測位方法。 In the moving body positioning method of Claim 1,
Performing an INS positioning calculation using the detected value of the inertial sensor;
Correcting the estimated position / velocity / posture error;
A mobile body positioning method further comprising:
GPSデータを受信し、GPS測位演算を行うGPS信号受信部と、
前記慣性センサーデータを用いて測位演算を行うINSと、
前記慣性センサーデータと、GPS測位結果と出力時刻と、をバッファリングし、時刻同期をとる時刻同期部と、
バッファリングされた前記慣性データと、INS測位結果と、バッファリングされた前記GPS測位結果と、を用いてカップリングし、位置・速度・姿勢の誤差を推定するカルマンフィルターと、
を備え、
前記推定された位置・速度・姿勢の誤差を前記INSにフィードバックし、補正することを特徴とする移動体測位装置。 An inertial sensor for acquiring inertial sensor data;
A GPS signal receiving unit that receives GPS data and performs GPS positioning calculation;
INS that performs positioning calculation using the inertial sensor data;
Buffering the inertial sensor data, the GPS positioning result, and the output time, and a time synchronization unit that performs time synchronization;
Coupling using the buffered inertial data, the INS positioning result, and the buffered GPS positioning result, and estimating a position / velocity / posture error; and
With
A moving body positioning apparatus which feeds back and corrects the estimated position / velocity / attitude error to the INS.
前記慣性センサーの検出値を用いてINS測位演算を行い、前記推定された位置・速度・姿勢の誤差を補正するリアルタイムINSと、
をさらに備えることを特徴とする移動体測位装置。 The mobile body positioning device according to claim 3,
Real-time INS that performs INS positioning calculation using the detection value of the inertial sensor and corrects the estimated position / velocity / attitude error;
The mobile body positioning device further comprising:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011039522A JP2012177564A (en) | 2011-02-25 | 2011-02-25 | Mobile body positioning method, and mobile body positioning device |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011039522A JP2012177564A (en) | 2011-02-25 | 2011-02-25 | Mobile body positioning method, and mobile body positioning device |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2012177564A true JP2012177564A (en) | 2012-09-13 |
Family
ID=46979501
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2011039522A Withdrawn JP2012177564A (en) | 2011-02-25 | 2011-02-25 | Mobile body positioning method, and mobile body positioning device |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2012177564A (en) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2016059930A1 (en) * | 2014-10-17 | 2016-04-21 | ソニー株式会社 | Device, method, and program |
KR101752724B1 (en) * | 2015-12-29 | 2017-06-30 | 국방과학연구소 | Alignment method for inertial navigation system |
JP2017129438A (en) * | 2016-01-20 | 2017-07-27 | 三菱電機株式会社 | Data processing device, data processing method, and data processing program |
CN108844533A (en) * | 2018-04-24 | 2018-11-20 | 西安交通大学 | A kind of free posture PDR localization method based on Multi-sensor Fusion and attitude algorithm |
WO2021174485A1 (en) * | 2020-03-05 | 2021-09-10 | Baidu.Com Times Technology (Beijing) Co., Ltd. | Latency compensation in inertial navigation system |
JPWO2022029878A1 (en) * | 2020-08-04 | 2022-02-10 | ||
CN114379577A (en) * | 2020-10-16 | 2022-04-22 | 北京四维图新科技股份有限公司 | Method and device for generating driving track |
-
2011
- 2011-02-25 JP JP2011039522A patent/JP2012177564A/en not_active Withdrawn
Cited By (20)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11067396B2 (en) | 2014-10-17 | 2021-07-20 | Sony Corporation | Position estimation device and position estimation method |
JP7014243B2 (en) | 2014-10-17 | 2022-02-01 | ソニーグループ株式会社 | Equipment, methods and programs |
JPWO2016059930A1 (en) * | 2014-10-17 | 2017-07-27 | ソニー株式会社 | Apparatus, method, and program |
JP2020112557A (en) * | 2014-10-17 | 2020-07-27 | ソニー株式会社 | Apparatus, method and program |
US11668569B2 (en) | 2014-10-17 | 2023-06-06 | Sony Group Corporation | Position estimation device and position estimation method |
US10203208B2 (en) | 2014-10-17 | 2019-02-12 | Sony Corporation | Position estimation device and position estimation method |
US10436591B2 (en) | 2014-10-17 | 2019-10-08 | Sony Corporation | Position estimation device and position estimation method |
WO2016059930A1 (en) * | 2014-10-17 | 2016-04-21 | ソニー株式会社 | Device, method, and program |
US20190376791A1 (en) | 2014-10-17 | 2019-12-12 | Sony Corporation | Position estimation device and position estimation method |
KR101752724B1 (en) * | 2015-12-29 | 2017-06-30 | 국방과학연구소 | Alignment method for inertial navigation system |
JP2017129438A (en) * | 2016-01-20 | 2017-07-27 | 三菱電機株式会社 | Data processing device, data processing method, and data processing program |
CN108844533B (en) * | 2018-04-24 | 2021-01-19 | 西安交通大学 | Free attitude PDR positioning method based on multi-sensor fusion and attitude calculation |
CN108844533A (en) * | 2018-04-24 | 2018-11-20 | 西安交通大学 | A kind of free posture PDR localization method based on Multi-sensor Fusion and attitude algorithm |
US11609342B2 (en) | 2020-03-05 | 2023-03-21 | Baidu Usa Llc | Latency compensation in inertial navigation system |
WO2021174485A1 (en) * | 2020-03-05 | 2021-09-10 | Baidu.Com Times Technology (Beijing) Co., Ltd. | Latency compensation in inertial navigation system |
CN113748362A (en) * | 2020-03-05 | 2021-12-03 | 百度时代网络技术(北京)有限公司 | Time delay compensation in inertial navigation systems |
JP7407947B2 (en) | 2020-08-04 | 2024-01-04 | 三菱電機株式会社 | Vehicle control device |
JPWO2022029878A1 (en) * | 2020-08-04 | 2022-02-10 | ||
WO2022029878A1 (en) * | 2020-08-04 | 2022-02-10 | 三菱電機株式会社 | Vehicle control device |
CN114379577A (en) * | 2020-10-16 | 2022-04-22 | 北京四维图新科技股份有限公司 | Method and device for generating driving track |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP2012177564A (en) | Mobile body positioning method, and mobile body positioning device | |
JP2012185111A (en) | Positioning device and positioning method | |
JP5606656B2 (en) | Positioning device | |
US9121929B2 (en) | Position output device using satellite navigation system | |
US20220342086A1 (en) | Position measurement system, positioning calculation apparatus, position measurement method and program | |
JP2012207919A (en) | Abnormal value determination device, positioning device, and program | |
JP2015004678A (en) | Vehicle positioning in high-reflection environment | |
KR20150082382A (en) | Adaptive scale and/or gravity estimation | |
JP5892845B2 (en) | Calibration quality determination apparatus and method | |
JP6008510B2 (en) | Measuring device and measuring method | |
JP2012193965A (en) | Position estimating device, and position estimating method and position estimating position program for position estimating device | |
JP2013170903A (en) | Measurement apparatus and measurement method | |
KR101741076B1 (en) | Apparatus for compensating time delay of inertial navigation system and method thereof | |
KR101402088B1 (en) | Method for estimating location based on technology convergence of satellite navigation system and a vision system | |
CN113783652A (en) | Data synchronization method and device of combined navigation system | |
JP5022747B2 (en) | Mobile body posture and orientation detection device | |
JP2014190900A5 (en) | ||
KR102250107B1 (en) | Device for vehicle speed correction and method of calculating the wheel scale factor by the same | |
JP6384194B2 (en) | Information processing apparatus, information processing method, and information processing program | |
CN110657800B (en) | Time synchronization method of position measurement integrated navigation system | |
JP4343581B2 (en) | Moving body posture detection device | |
JP2015102330A (en) | Movement information calculation device, movement information calculation method, movement information calculation program, and mobile body | |
US11828837B2 (en) | Method for improving radar measurements in a handheld device | |
JP6178776B2 (en) | Speed measuring device | |
JP5183050B2 (en) | In-vehicle navigation device and navigation method |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A300 | Withdrawal of application because of no request for examination |
Free format text: JAPANESE INTERMEDIATE CODE: A300 Effective date: 20140513 |