JP2012177564A - Mobile body positioning method, and mobile body positioning device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a highly accurate mobile body positioning method.SOLUTION: The mobile body positioning method includes buffering inertia sensor data, using the buffered inertia sensor data to perform INS positioning operation, calculating a delay time of GPS positioning to the current time, buffering an output time of GPS data and a GPS positioning result, acquiring time synchronization of the buffered GPS positioning result with an INS positioning result, coupling the buffered inertia sensor data with the inertia INS positioning result and with the buffered GPS positioning result to estimate errors of a position, speed, and posture, and feeding back the errors of a position, speed, and posture to the INS positioning result to perform correction. Highly accurate mobile body positioning can be conducted by acquiring time synchronization of GPS data with inertia sensor data and performing error correction of a correct INS positioning result.

Description

  The present invention relates to a moving body positioning method and a moving body positioning apparatus.

  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).

JP 2009-222438 A

  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.

  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.

  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.

  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.

  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.

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.

  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.

  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.

  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.

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a configuration explanatory diagram illustrating a main configuration of a mobile body positioning device according to a first embodiment. 3 is a flowchart showing main steps of the mobile body positioning method according to the first embodiment. The buffer output timing when the GPS delay time may increase is shown, (a) is a diagram showing the timing of inertial sensor data and GPS data, (b) is the buffer output timing and progress of (a) A table showing the relationship with time. The buffer output timing when the GPS delay time may decrease is shown, (a) is a diagram showing the timing of inertial sensor data and GPS data, (b) is the buffer output timing and progress of (a) A table showing the relationship with time. Structure explanatory drawing which shows the main structures of the mobile body positioning apparatus which concerns on Embodiment 2. FIG.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
(Embodiment 1)

  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) signal receiving unit 10, an inertial sensor 20 having a gyro sensor and an acceleration sensor, a time synchronization unit 30, a Kalman filter (KF) 40, and an INS 50. Has been.

The GPS signal receiving unit 10 receives a GPS signal and performs a positioning calculation. There is a delay between the measurement acquisition and the positioning calculation until the positioning result is output. Therefore, a measurement acquisition time and an output time are added to the GPS positioning result and output at 1 Hz. Furthermore, time information is output at 5 Hz.
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 time synchronization unit 30.

The time synchronization unit 30 includes a buffer unit and a calculation unit (both not shown) and has the following functions.
(1) The time information output at 5 Hz from the GPS signal receiver 10 and the inertial sensor data output at 100 Hz from the inertial sensor 20 are associated with each other, and the time information is added to the inertial sensor data and buffered.
(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 signal receiving unit 10 and the time at the output.
(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 inertia sensor 20, the inertia sensor data stored in the buffer unit is output.

  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.

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 INS 50, and the position / velocity / attitude, inertial sensor Calculate the bias error.
Using these errors, the position / velocity / posture error of the positioning result of the INS 50 is corrected.

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 signal receiver 10 outputs time information at 5 Hz.

Next, it is determined whether or not GPS positioning is detected (ST3). If GPS positioning is detected (Yes), the time synchronization unit 30 calculates a GPS delay time (ST4). The delay time is associated with time information output at 5 Hz and inertial sensor data output from the inertial sensor 20 at 100 Hz, buffered by adding time information to the inertial sensor data, and output at 1 Hz. Calculate from the time of the position in the positioning result and the time of output. Therefore, the GPS delay time can be expressed by the following equation.
Delay time = current time−GPS positioning time.

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.

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.

  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).

  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.

  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).

  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 inertial sensor 20, the inertial sensor data stored in the buffer when the wait time of the inertial sensor has elapsed is output.

  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.

  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.

  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 INS 50 is corrected (ST14).

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.

  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.

  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”.

  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.

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.

  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.

  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.

  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.

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)

  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 signal receiving unit 10, the inertial sensor 20, the time synchronization unit 30, the Kalman filter (KF) 40, and the INS 50 are the same as those in the first embodiment, description thereof is omitted.

  The real-time INS 60 receives the detected value of the inertial sensor and the position / velocity / posture estimation error estimated by coupling using the Kalman filter 40. The detection value of the inertial sensor is input to the real-time INS 60 almost in real time. Using the detected value of the inertial sensor, positioning calculation by INS is performed, and the estimation error of position / velocity / attitude is corrected and output.

  By the way, the output from the Kalman filter 40 is delayed by the wait time of the inertial sensor. Therefore, positioning calculation by real-time INS is performed using the detection value of the inertial sensor, and position / velocity / posture errors calculated by coupling using GPS data and inertial sensor data synchronized in time are corrected. Thus, an accurate INS positioning result can be output without delay.

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.

  DESCRIPTION OF SYMBOLS 10 ... GPS signal receiving part, 20 ... Inertial sensor, 30 ... Time synchronization part, 40 ... Kalman filter (KF), 50 ... INS.

Claims (4)

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: 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: 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. 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:

Images