JP2011185899A - Position locating device, position locating method of position locating device and position locating program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To correct data of an inertial measuring device from the beginning of navigation in high accuracy and locate the position in high accuracy. <P>SOLUTION: The own position locating device 100 conducts navigation processing two times. In the first and second navigation processings, an IMU processing part 140 corrects inertial data based on the IMU error estimation value calculated by the Kalman filter 150, and calculates the position, attitude and velocity by inertial navigation based on the corrected inertial data. The Kalman filter 150 calculates the IMU error estimation value based on a residual calculated by a GPS processing part 120 or an ODO processing part 130. In the second navigation processing, the IMU processing part 140 uses the IMU error estimation value that is calculated by the Kalman filter 150 in the first navigation processing, as the initial value of the IMU error estimation value. The IMU processing part 140 outputs the position, attitude and velocity calculated in the second navigation processing, as a navigation result. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a position locating device, a position locating method, and a position locating program for locating a position using data of, for example, a GPS (Global Positioning System) or an inertial measurement device.

  A navigation device using GPS is generally configured to use the advantages of each sensor in combination with an inertial measurement unit (IMU) and odometry (ODOmeter) because of its characteristics. However, there are bias and scale factor errors in inertial measurement devices, and scale factor errors and offset errors in odometry. These errors can be estimated and corrected if there is sufficient time for using the GPS. However, since the error amount is unknown at the start of measurement, the correction is not normally performed (correction amount 0). Therefore, when a situation where GPS cannot be used before the error can be sufficiently estimated and corrected, an error in autonomous navigation (IMU / ODO) using an inertial navigation device and odometry becomes significant.

Japanese Patent No. 3875714 JP 2006-138834 A JP 2008-249639 A

Hiroyuki Yamaguchi, Katsuhiro Asano, Yasushi Amano, “Development of vehicle body side slip angle estimation method”, Nippon Mechanics 會 Proceedings C, 67 (659), pp. 2291-2298, 2001

  An object of the present invention is, for example, to be able to determine the position with high accuracy by correcting the data of an inertial measurement device and odometry from the beginning of navigation.

The position locating device of the present invention is
Navigation data for storing a plurality of GPS data acquired at different times by a GPS (Global Positioning System) receiver included in the mobile body and a plurality of inertia data acquired at different times by an inertial measurement device included in the mobile body. A storage unit;
An error estimation unit that calculates an error estimation initial value as an estimation value of the measurement error of the inertial measurement device based on a plurality of GPS data and a plurality of inertial data stored in the navigation data storage unit;
An inertial data correction unit that corrects initial inertial data acquired first among the plurality of inertial data based on an error estimation initial value calculated by the error estimation unit;
An inertial navigation unit that calculates an orientation position of the start time as the position of the moving body at the start time at which the start inertia data was acquired based on the start inertia data corrected by the inertia data correction unit.

  According to the present invention, for example, the position of the start time can be determined with high accuracy by correcting the start inertia data of the inertial measurement device.

FIG. 3 is a functional configuration diagram of the self-positioning apparatus 100 according to the first embodiment. FIG. 2 is a configuration diagram of a vehicle 200 in the first embodiment. 5 is a flowchart showing a position locating method of self-position locating apparatus 100 in the first embodiment. 6 is a graph showing a time change of an error estimated value (and variance) calculated in the first navigation processing (S100) in the position locating method according to the first embodiment. 6 is a graph showing a change over time of an error estimated value (and variance) calculated in a second navigation process (S200) in the position locating method according to the first embodiment. The flowchart which shows the 1st navigation process (S100) and the 2nd navigation process (S200) of the position location method in Embodiment 1. FIG. 5 is a flowchart showing GPS / INS combined navigation processing (S300) in the first embodiment. 5 is a flowchart showing ODO / INS combined navigation processing (S400) in the first embodiment. FIG. 3 is a functional configuration diagram of a GPS processing unit 120 of the self-positioning apparatus 100 according to the first embodiment. 6 is a flowchart showing double phase difference residual calculation processing (S310) in the first embodiment. 2 is a functional configuration diagram of an ODO processing unit 130 and an IMU processing unit 140 of the self-positioning apparatus 100 according to Embodiment 1. FIG. FIG. 6 is a relationship diagram between the vehicle coordinate system xyz and the offset error os of the odometry 230 in the first embodiment. FIG. 5 is a diagram showing a side slip angle β in the first embodiment. 6 is a graph _{showing a} cornering power S _slip and an offset error os [1] in the first embodiment. 3 is a graph showing a scale factor error of odometry 230 in the first embodiment. 5 is a flowchart showing a skid characteristic learning process (S500) in the first embodiment. 5 is a flowchart showing a speed residual calculation process (S410) in the first embodiment. FIG. 3 is a diagram illustrating an example of hardware resources of the self-location apparatus 100 according to the first embodiment. The function block diagram of the self-positioning apparatus 100 in Embodiment 2. FIG.

Embodiment 1 FIG.
A mode in which the position of a vehicle (an example of a moving body) is determined using GPS, an inertial measurement device, and odometry will be described.

FIG. 1 is a functional configuration diagram of the self-positioning apparatus 100 according to the first embodiment.
A functional configuration of the self-positioning apparatus 100 according to Embodiment 1 will be described below with reference to FIG.

  The self-location locating device 100 (an example of a location locating device) includes a skid characteristic learning unit 110, a GPS processing unit 120, an ODO processing unit 130, an IMU processing unit 140, a Kalman filter 150, and a navigation data storage unit 190.

The navigation data storage unit 190 stores a GPS data group 191, an inertial data group 193, and an odometry data group 192.
The GPS data group 191 is a plurality of GPS data acquired at different times by a GPS receiver included in the vehicle.
The inertial data group 193 is a plurality of inertial data acquired at different times by an inertial measurement device provided in the vehicle.
The odometry data group 192 is a plurality of odometry data acquired at different times depending on the odometry provided in the vehicle.

FIG. 2 is a configuration diagram of the vehicle 200 in the first embodiment.
The configuration of the vehicle 200 that acquires the GPS data group 191, the inertia data group 193, and the odometry data group 192 will be described below with reference to FIG.

A top plate 201 is installed in the vehicle 200, and three GPS receivers and an inertial measurement device 220 are attached to the top plate 201.
The vehicle 200 includes a self-positioning device 100 and an odometry 230.

Of the three GPS receivers on the top plate 201, one is a master GPS receiver (master GPS 210), and the remaining two are slave GPS receivers (slave GPS 211, slave GPS 212).
Each GPS receiver receives a GPS carrier wave (positioning signal) transmitted from a GPS satellite, acquires an observation value from the reception result, and outputs the acquired observation value as GPS data.
The GPS data output by the GPS receiver includes the phase of the received carrier wave (carrier wave phase), the distance between the GPS receiver and the GPS satellite (pseudo distance), the coordinate value of the GPS receiver obtained by independent positioning, and the carrier wave. Includes navigation messages that can be obtained.

The inertial measurement device 220 (IMU) includes a gyro sensor and an acceleration sensor.
The gyro sensor periodically measures and outputs angular velocities in the three axial directions xyz of the vehicle 200. Hereinafter, the angular velocity in the triaxial direction xyz is referred to as “angular velocity vector”.
The acceleration sensor periodically measures and outputs the acceleration of the vehicle 200 in the triaxial direction xyz. Hereinafter, the acceleration in the triaxial direction xyz is referred to as an “acceleration vector”.
The inertial measurement device 220 outputs an angular velocity vector and an acceleration vector as inertial data.

The odometry 230 (ODO) periodically measures vehicle speed pulses (an example of speed data) representing the speed of the vehicle 200, and outputs the measured vehicle speed pulses as odometry data.
The vehicle speed pulse represents the number of rotations of the tire per unit time. By multiplying the vehicle speed pulse by the length of the circumference of the tire, a scalar amount of the speed of the vehicle 200 (hereinafter referred to as a speed scalar) is obtained.

The self-positioning device 100 calculates the position, posture, and speed of the vehicle 200 at each time based on the GPS data, inertia data, and odometry data.
For example, the self-positioning device 100 calculates a three-dimensional coordinate value (latitude, longitude, altitude) as the position of the vehicle 200.
The self-position locating apparatus 100 calculates a three-dimensional posture angle (roll angle, pitch angle, yaw angle) as the posture of the vehicle 200.
Further, the self-position locating apparatus 100 calculates a speed vector (speed vector amount) as the speed of the vehicle 200.

Hereinafter, the front-rear direction (vertical direction, length direction) of the vehicle 200 is “x-axis”, the left-right direction (horizontal direction, width direction) of the vehicle 200 is “y-axis”, and the vertical direction (height direction) of the vehicle 200 is Let it be “z-axis”.
Further, the angle around the x axis is “roll angle φ (rotation angle)”, the angle around the y axis is “pitch angle θ (elevation angle)”, and the angle around the z axis is “yaw angle ψ (azimuth angle)”. .
Also, a coordinate system (origin O) represented by the xyz axis is referred to as a “vehicle coordinate system”.

In the navigation data storage unit 190 (see FIG. 1) of the self-positioning device 100, a plurality of GPS data acquired by each of the three GPS receivers installed in the vehicle 200 is stored in advance as a GPS data group 191. In the navigation data storage unit 190, a plurality of GPS data acquired by a GPS receiver (electronic reference point) (hereinafter referred to as a reference station GPS) whose position is fixed and whose coordinate value is known is also a GPS data group 191. Stored in advance.
In the navigation data storage unit 190, a plurality of inertia data measured by the inertia measuring device 220 installed in the vehicle 200 is stored in advance as an inertia data group 193.
In the navigation data storage unit 190, a plurality of odometry data measured by the odometry 230 installed in the vehicle 200 is stored in advance as an odometry data group 192.

  Each data included in the GPS data group 191, the inertial data group 193, and the odometry data group 192 is assumed to be associated with an acquisition time (measurement time, detection time).

  Returning to FIG. 1, the description of the configuration of the self-positioning device 100 will be continued.

  The Kalman filter 150 (an example of an error estimation unit) calculates an error estimation initial value (an IMU error estimation initial value to be described later) as an estimation value of the measurement error of the inertial measurement device 220 based on the GPS data group 191 and the inertial data group 193. To do.

The IMU processing unit 140 (an example of an inertial data correction unit and an inertial navigation unit) corrects the first inertial data acquired first in the inertial data group 193 based on the error estimation initial value calculated by the Kalman filter 150.
The IMU processing unit 140 calculates the orientation position of the start time as the position of the vehicle 200 at the start time at which the start inertia data is acquired based on the corrected start inertia data.

  The GPS processing unit 120 (an example of the carrier phase residual calculation unit) is configured to generate a residual carrier phase residual (translation system, which will be described later) based on the carrier phase included in the GPS data and the reference position calculated by the IMU processing unit 140. (Attitude system double phase difference residual) is calculated.

  For example, the error estimation initial value is output as follows.

The IMU processing unit 140 detects the vehicle 200 at the start time at which the start inertia data is acquired based on the start inertia data first acquired from the inertia data group 193 before the error estimation initial value is calculated by the Kalman filter 150. The reference position of the start time is calculated as the position.
The IMU processing unit 140 corrects the inertia data based on the estimated error value of the time calculated by the Kalman filter 150 for each of the remaining inertia data excluding the head inertia data in the inertia data group 193. Based on the corrected inertia data, the IMU processing unit 140 calculates a reference position at the time as the position of the vehicle 200 at the time at which the inertia data was acquired.
The GPS processing unit 120 uses the reference position of the time calculated by the IMU processing unit 140 and the carrier wave phase included in the GPS data acquired at the time as the residual of the carrier wave phase at the time. Calculate the carrier phase residual.
The Kalman filter 150 uses the carrier phase residual at the time calculated by the GPS processing unit 120 to calculate an error estimated value at the time as an estimated value of the measurement error of the inertial measurement device 220 with respect to the time.
The Kalman filter 150 outputs an error estimation initial value based on the error estimation value of the acquisition time of the last GPS data acquired last in the GPS data group 191.

  In addition, after the orientation position of the start time is calculated, the following operation is performed.

The GPS processing unit 120 calculates a carrier phase residual at the start time based on the orientation position at the start time calculated by the IMU processing unit 140 and the carrier phase included in the start GPS data acquired at the start time.
The GPS processing unit 120 is included in the GPS data acquired at the time and the orientation position of the time calculated by the IMU processing unit 140 for each remaining GPS data excluding the top GPS data in the GPS data group 191. Based on the carrier phase, the carrier phase residual at that time is calculated.
The Kalman filter 150 calculates an error estimation value at the time based on the carrier phase residual at the time calculated by the GPS processing unit 120.
The IMU processing unit 140 corrects the next inertia data acquired next to the inertia data acquired at the time based on the error estimation value at the time calculated by the Kalman filter 150. The IMU processing unit 140 calculates an orientation position at the next time when the next inertia data is acquired based on the corrected next inertia data.

The IMU processing unit 140 calculates the speed of the vehicle 200 at the time based on the corrected inertia data.
The skid characteristic learning unit 110 estimates an error as an estimation value of the measurement error of the odometry 230 based on the odometry data acquired at the time in the odometry data group 192 and the speed at the time calculated by the IMU processing unit 140. An initial value (an ODO error estimation initial value described later) is calculated.
Based on the odometry data acquired at the time, the speed at the time calculated by the IMU processing unit 140, and the error estimation initial value calculated by the skid characteristic learning unit 110, the ODO processing unit 130 Calculate the residual.
The Kalman filter 150 calculates an error estimated value (IMU error estimated value described later) at the time based on the velocity residual at the time calculated by the ODO processing unit 130.

  FIG. 3 is a flowchart showing a position locating method of self-position locating apparatus 100 in the first embodiment. A position locating method of self-position locating apparatus 100 in the first embodiment will be described below based on FIG.

In the first navigation process (S100), the self-positioning device 100 uses the navigation data such as the GPS data group 191, the inertial data group 193, and the odometry data group 192 to determine the position, posture, and speed of the vehicle 200 as a result of the reference navigation. Calculate as
Then, the self-positioning apparatus 100 calculates an IMU error estimation initial value and an ODO error estimation initial value based on the reference navigation result.
The IMU error estimation initial value is an estimated value of the measurement error of the inertial measurement device 220 (hereinafter referred to as an IMU error estimated value).
The ODO error estimation initial value is an estimated value of the measurement error of the odometry 230 (hereinafter referred to as an ODO error estimated value).

In the second navigation processing (S200), the self-positioning device 100 determines the vehicle 200 based on the GPS data group 191, the inertial data group 193, the odometry data group 192, the IMU error estimation initial value, and the ODO error estimation initial value. The position, posture and speed are calculated as the result of the orientation navigation.
The self position locating device 100 outputs a position navigation result as the position, posture and speed of the vehicle 200.

FIG. 4 is a graph showing a time change of the error estimated value (and variance) calculated in the first navigation processing (S100) in the position locating method of the first embodiment.
The upper graph shows the time change of the error estimated value, and the lower graph shows the time change of the variance of the error estimated value.

The inertial data acquired by the inertial measurement device 220 (gyro sensor, acceleration sensor) includes measurement errors such as bias and scale factor errors. It is known that the scale factor error of the inertial measurement device 220 changes every time the power is turned on.
The odometry data acquired by the odometry 230 includes measurement errors such as a scale factor error, an offset error, and a parameter related to the offset error (for example, cornering power described later). It is known that parameters relating to the scale factor error, offset error, and offset error of the odometry 230 change according to a change in the center of gravity position of the vehicle 200, a change in tire air pressure, a road surface condition, and the like.
Since these measurement errors are unknown, the navigation results calculated based on the inertial data include errors for the measurement errors.

However, these measurement errors can be estimated by comparing a specific value obtained from inertial data with a specific value obtained from GPS data.
The estimated value of the measurement error (error estimated value) approaches a constant value by repeating the estimation (upper part of FIG. 4) and sufficiently converges (lower part of FIG. 4). The accuracy of a sufficiently converged error estimate is sufficiently high.

  The self-localization device 100 uses the error estimation value sufficiently converged by the first navigation process (S100) as the error estimation initial value in the second navigation process (S200).

  FIG. 5 is a graph showing a time change of the error estimated value (and variance) calculated in the second navigation process (S200) in the position locating method of the first embodiment.

  By using the error estimation initial value in the second navigation process (S200), the error estimation value shows a substantially constant value from the beginning (upper part of FIG. 5) and converges immediately (lower part of FIG. 5).

  The self-positioning device 100 can quickly converge the error estimation value by using the error estimation initial value in the second navigation processing (S200), and obtain the navigation result with high accuracy.

FIG. 6 is a flowchart showing the first navigation process (S100) and the second navigation process (S200) of the position location method in the first embodiment.
The first navigation process (S100) and the second navigation process (S200) will be described below with reference to FIG.

  The first navigation process (S100) is indicated by S110 to S121 and S300 to S500, and the second navigation process (S200) is indicated by S201 to S221 and S300 to S500.

The self-positioning device 100 performs the same navigation process in the first navigation process (S100) and the second navigation process (S200).
However, since the IMU / ODO error estimation initial value is specified by the first navigation process (S100), the processes using the IMU / ODO error estimation initial value (S201, S211) are only performed in the second navigation process (S200). Done.

  First, the first navigation process (S100) will be described.

In S <b> 110, the IMU processing unit 140 acquires inertial data acquired first in the inertial data group 193 (hereinafter referred to as head inertial data).
The IMU processing unit 140 performs inertial navigation (INS) using the head inertia data, and calculates the position, posture, and speed of the vehicle 200 at the time when the head inertia data was acquired (hereinafter referred to as the head time). To do.
It progresses to S120 after S110.

In S120, the IMU processing unit 140 determines whether or not there is next inertia data (whether or not inertia data that has not been processed remains).
When there is next inertia data (YES), the process proceeds to S121.
When there is no next inertia data (NO), the first navigation processing (S100) is ended.

  In S121, the GPS processing unit 120 determines whether or not the time corresponding to the position, posture, and speed of the vehicle 200 calculated by the IMU processing unit 140 is GPS visible.

  When the GPS is visible, it means the time when the three GPS receivers installed in the vehicle 200 and the GPS receiver (reference station GPS) provided at a predetermined location can receive a carrier wave from a predetermined number or more of GPS satellites.

The GPS processing unit 120 acquires GPS data at the corresponding time of each GPS receiver from the GPS data group 191, and when the acquired GPS data includes a predetermined number of carrier phases or more, the GPS processing time is visible. judge.
The GPS processing unit 120 determines that the GPS is invisible when each of the acquired GPS data does not include a predetermined number of carrier phases or more.

When the GPS is visible (YES), the process proceeds to the GPS / INS combined navigation process (S300).
When the GPS is invisible (NO), the process proceeds to the ODO / INS combined navigation process (S400).

In the GPS / INS combined navigation processing (S300), the IMU processing unit 140 calculates the position, posture, and speed of the vehicle 200 based on the GPS data and the inertia data.
The Kalman filter 150 calculates an IMU error estimated value based on the position, posture, and speed of the vehicle 200.
Details of the GPS / INS combined navigation processing (S300) will be described later.
After the GPS / INS combined navigation process (S300), the process proceeds to the skid characteristic learning process (S500).

In the skid characteristic learning process (S500), the skid characteristic learning unit 110 performs ODO based on the speed of the vehicle 200 calculated in the GPS / INS combined navigation process (S300) and the vehicle speed pulse included in the odometry data at the time. An error estimate is calculated.
After the skid characteristic learning process (S500), the process returns to S120.

In the ODO / INS combined navigation processing (S400), the IMU processing unit 140 calculates the position, posture, and speed of the vehicle 200 based on the odometry data and the inertia data.
The Kalman filter 150 calculates an IMU error estimated value based on the position, posture, and speed of the vehicle 200.
Details of the ODO / INS combined navigation processing (S400) will be described later.
After the ODO / INS combined navigation process (S400), the process returns to S120.

The self-localization device 100 uses the IMU error estimated value calculated last in the GPS / INS combined navigation processing (S300) of the first navigation processing (S100) as the IMU error estimation initial value.
The self-positioning apparatus 100 uses the ODO error estimated value calculated last in the skid characteristic learning process (S500) of the first navigation process (S100) as the ODO error estimated initial value.

If the IMU error estimated value and the ODO error estimated value are converged in S120, the self location apparatus 100 may end the first navigation process (S100) even if there is the next inertial data.
For example, the IMU processing unit 140 acquires a predetermined number of IMU error estimated values (or ODO error estimated values) calculated so far in order from the newest, and the difference between the acquired maximum value and minimum value is less than the predetermined value. If there is, it is determined that the IMU error estimated value has converged, and the first navigation processing (S100) is terminated.

The self-positioning apparatus 100 may not use the IMU / ODO error estimated value calculated last in the first navigation process as the IMU / ODO error estimated initial value.
For example, the self-localization device 100 acquires a predetermined number of IMU error estimated values (or ODO error estimated values) calculated so far, including the last IMU error estimated value, in a new order, and acquires each IMU error estimated value. May be used as the initial value of the IMU error estimation.

  Next, the second navigation process (S200) will be described.

In S201, the IMU processing unit 140 adds the IMU error estimation initial value calculated in the first navigation processing (S100) to the value of the leading inertia data as an error correction amount. By adding an error correction amount to the value of the leading inertia data, the leading inertia data can be corrected.
It progresses to S210 after S201.

In S210, the IMU processing unit 140 calculates the position, posture, and speed of the vehicle 200 at the start time based on the corrected start inertia data (similar to S110).
It progresses to S211 after S210.

In S211, the skid characteristic learning unit 110 initializes the ODO error estimation initial value calculated in the first navigation process (S200).
After S211, the process proceeds to S220.

In S220, if there is next inertia data (YES), the process proceeds to S221.
If there is no next inertia data (NO), the second navigation process (S200) ends.

If the GPS is visible in S221 (YES), the process proceeds to the GPS / INS combined navigation process (S300).
If the GPS is not visible (NO), the process proceeds to the ODO / INS combined navigation process (S400).

In the GPS / INS combined navigation processing (S300), the IMU processing unit 140 calculates the position, posture, and speed of the vehicle 200 based on the GPS data and the inertia data.
The Kalman filter 150 calculates an IMU error estimated value based on the position, posture, and speed of the vehicle 200.
After the GPS / INS combined navigation process (S300), the process proceeds to the skid characteristic learning process (S500).

In the skid characteristic learning process (S500), the skid characteristic learning unit 110 calculates an ODO error based on the speed of the vehicle 200 calculated in the GPS / INS combined navigation process (S300) and the vehicle speed pulse included in the odometry data at the time. Calculate an estimate.
After the skid characteristic learning process (S500), the process returns to S220.

In the ODO / INS combined navigation processing (S400), the IMU processing unit 140 calculates the position, posture, and speed of the vehicle 200 based on the odometry data and the inertia data.
The Kalman filter 150 calculates an IMU error estimated value based on the position, posture, and speed of the vehicle 200.
After the ODO / INS combined navigation process (S400), the process returns to S220.

  The self-location device 100 determines the position, posture, and position of the vehicle 200 at each time calculated by the GPS / INS combined navigation processing (S300) and the ODO / INS combined navigation processing (S400) of the second navigation processing (S200). The speed is output as a navigation result (landing value).

FIG. 7 is a flowchart showing GPS / INS combined navigation processing (S300) in the first embodiment.
The GPS / INS combined navigation processing (S300) in the first embodiment will be described below based on FIG.

In the double phase difference residual calculation process (S310), the GPS processing unit 120 calculates the time based on the position and orientation of the vehicle 200 calculated immediately before and the carrier phase included in the GPS data at the time. The double phase difference residual is calculated.
Details of the double phase difference residual calculation process (S310) will be described later.
After the double phase difference residual calculation process (S310), the process proceeds to S320.

  In S320, the Kalman filter 150 receives the double phase difference residual at the time as an input and calculates an IMU error estimated value at the time.

An overview of the Kalman filter 150 will be described.
The Kalman filter 150 calculates an error correction amount (IMU error estimated value or navigation error estimated value to be described later) using the modeled state equation and observation equation.
The residual (double phase difference residual or velocity residual) input to the Kalman filter 150 is used as a variable of the observation equation.

The Kalman filter 150 calculates a Kalman gain vector K using the observation noise matrix R, the error covariance matrix P, and the observation matrix H.
An observation noise matrix R and an observation matrix H are prepared for each type of residual.
The error covariance matrix P represents covariance values of state quantities (position, posture, speed, IMU error estimated value, navigation error estimated value, etc.).
The observation matrix H represents the relationship between the state quantity and the observation quantity (double phase difference residual, velocity residual, etc.).

The calculation formula (1) for the Kalman gain vector K is shown below. In Expression (1), “H ^T ” represents a transposed matrix of the observation matrix H.

  A plurality of values Kdz obtained by multiplying the residual dz by the Kalman gain vector K are error correction amounts.

  It progresses to S330 after S320.

In S330, the IMU processing unit 140 adds the estimated IMU error value at that time to the next inertial data value as an error correction amount. The inertial data can be corrected by adding the error correction amount to the value of the inertial data.
It progresses to S340 after S330.

In S340, the IMU processing unit 140 performs inertial navigation using the corrected inertial data, and calculates the position, posture, and speed of the vehicle 200 at the time (next time) when the inertial data is acquired.
After S340, the GPS / INS combined navigation processing (S300) ends.

FIG. 8 is a flowchart showing the ODO / INS combined navigation processing (S400) in the first embodiment.
The ODO / INS combined navigation processing (S400) in the first embodiment will be described below based on FIG.

In the speed residual calculation process (S410), the ODO processing unit 130 calculates the speed residual at the time based on the speed of the vehicle 200 calculated immediately before and the ODO error estimated value at the time.
Details of the speed residual calculation process (S410) will be described later.
After the speed residual calculation process (S410), the process proceeds to S420.

In S420, the Kalman filter 150 receives the velocity residual at the time and calculates an IMU error estimated value at the time.
It progresses to S430 after S420.

In S430, the IMU processing unit 140 corrects the next inertial data using the IMU error estimated value at the time as an error correction amount.
It progresses to S440 after S430.

In S440, the IMU processing unit 140 performs inertial navigation using the corrected inertial data, and calculates the position, posture, and speed of the vehicle 200 at the time (next time) when the inertial data is acquired.
After S440, the ODO / INS combined navigation processing (S400) ends.

Next, the specifics of the double phase difference residual calculation process (S310) performed in the GPS / INS combined navigation process (S300) and the speed residual calculation process (S410) performed in the ODO / INS combined navigation process (S400). An example will be described.
A specific example of the skid characteristic learning process (S500) will be described.

FIG. 9 is a functional configuration diagram of the GPS processing unit 120 of the self location apparatus 100 according to the first embodiment.
A specific example of the functional configuration of the GPS processing unit 120 in the first embodiment will be described below with reference to FIG.

The GPS processing unit 120 calculates a translation system double phase difference residual and a posture system double phase difference residual.
The GPS processing unit 120 is configured to calculate a translation system double phase difference residual, a translation system double phase difference calculation unit 121, a translation system double phase difference prediction unit 122, and a translation system double phase difference residual calculation unit. 123.
The GPS processing unit 120 is configured to calculate a posture system double phase difference residual, a posture system double phase difference calculation unit 124, a posture system double phase difference prediction unit 125, and a posture system double phase difference residual calculation unit. 126.
Further, the GPS processing unit 120 includes a design matrix calculation unit 127.
Details of each component of the GPS processing unit 120 will be described later.

  The Kalman filter 150 calculates an IMU error estimated value (and a navigation error estimated value to be described later) based on the translational double phase difference residual and the attitude system double phase difference residual calculated by the GPS processing unit 120. .

FIG. 10 is a flowchart showing the double phase difference residual calculation process (S310) in the first embodiment.
A specific example of the double phase difference residual calculation process (S310) in the first embodiment will be described below with reference to FIG.

In S <b> 311, the translational double phase difference calculation unit 121 acquires the GPS data of the target time of the reference station GPS and the main station GPS 210 from the GPS data group 191, and the carrier phase A and GPS of the GPS satellite A from the acquired GPS data. The carrier phase B of the satellite B is acquired.
The translation system double phase difference calculation unit 121 calculates the difference between the carrier phase A of the reference station GPS and the carrier phase A of the main station GPS 210 as the carrier phase difference A, and calculates the carrier phase B of the reference station GPS and the carrier phase of the main station GPS 210. The difference from B is calculated as the carrier phase difference B.
The translation system double phase difference calculation unit 121 calculates the difference between the carrier phase difference A and the carrier phase difference B as a translation system double phase difference.

  The translation system means that the position of the vehicle 200 is obtained from the relative position of the main station GPS 210 with respect to the reference station GPS.

  After S311, the process proceeds to S312.

In S312, the translation system double phase difference prediction unit 122 converts the position of the reference station GPS 299, the position of the vehicle 200 at the target time calculated by the IMU processing unit 140, and the translation system design matrix calculated by the design matrix calculation unit 127. Based on this, a translational double phase difference is calculated.
The design matrix calculator 127 translates a matrix representing the relationship between the position of the two GPS receivers and the translation system double phase difference based on the ephemeris (orbit information included in the navigation message) included in each GPS data. Calculate as a design matrix.
It progresses to S313 after S312.

In S313, the translation system double phase difference residual calculation unit 123 determines the difference between the translation system double phase difference calculated in S311 and the translation system double phase difference calculated in S312 as the translation system double phase difference at the target time. Calculated as phase difference residual.
It progresses to S314 after S313.

In S <b> 314, the attitude system double phase difference calculation unit 124 acquires GPS data of the target times of the master station GPS 210, the slave station GPS 211, and the slave station GPS 212 from the GPS data group 191. The attitude system double phase difference calculation unit 124 inputs the carrier phase A of the GPS satellite A and the carrier phase B of the GPS satellite B from the acquired GPS data.
The attitude system double phase difference calculation unit 124 calculates the difference between the carrier phase A of the master station GPS 210 and the carrier phase A of the slave GPS 211 as the first carrier phase difference A, and calculates the carrier phase B of the master station GPS 210 and the slave station GPS 211. Is calculated as a first carrier phase difference B. Similarly, the attitude system double phase difference calculation unit 124 calculates the second carrier phase difference A / B based on the carrier phase A / B of the master station GPS 210 and the carrier phase A / B of the slave station GPS 212.
The attitude system double phase difference calculation unit 124 calculates the difference between the first carrier phase difference A and the first carrier phase difference B as the first attitude system double phase difference, and the second carrier phase difference A. And the second carrier phase difference B are calculated as a second attitude system double phase difference.

  The attitude system means that the attitude of the vehicle 200 is obtained from the relative positions (baseline vectors) of the slave station GPS 211 and the slave station GPS 212 with respect to the master station GPS 210.

  It progresses to S315 after S314.

In S315, the attitude system double phase difference prediction unit 125 determines the relative position (predetermined measurement value) of the master station GPS 210, the slave station GPS 211, and the slave station GPS 212, and the attitude and design of the vehicle 200 at the target time calculated by the IMU processing unit 140. Based on the posture system design matrix calculated by the matrix calculator 127, the first and second posture system double phase differences are calculated.
Based on the ephemeris included in each GPS data, the design matrix calculation unit 127 calculates a matrix representing the relationship between the positions of the two GPS receivers and the posture system double phase difference as the posture system design matrix.
It progresses to S316 after S315.

In S316, the posture system double phase difference residual calculation unit 126 targets the difference between the first posture system double phase difference calculated in S314 and the first posture system double phase difference calculated in S315. Calculated as the first attitude system double phase difference residual at time.
The posture system double phase difference residual calculation unit 126 calculates the difference between the second posture system double phase difference calculated in S314 and the second posture system double phase difference calculated in S315 at the target time. 2 is calculated as the attitude system double phase difference residual.
After S316, the double phase difference residual calculation process (S310) ends.

  The Kalman filter 150 receives the translation system double phase difference residual calculated in the double phase difference residual calculation process (S310) and the first and second attitude system double phase difference residuals as inputs, and outputs the target time. An IMU error estimated value (and a navigation error estimated value described later) is calculated.

FIG. 11 is a functional configuration diagram of the ODO processing unit 130 and the IMU processing unit 140 of the self-positioning apparatus 100 according to the first embodiment.
A specific example of the functional configuration of the ODO processing unit 130 and the IMU processing unit 140 will be described below with reference to FIG.

The ODO processing unit 130 includes a speed / acceleration calculation unit 131, a speed / acceleration prediction unit 132, and a speed / acceleration residual calculation unit 133.
Details of each component of the ODO processing unit 130 will be described later.

The Kalman filter 150 outputs an estimated value of each measurement error of the gyro sensor and the acceleration sensor constituting the inertial measurement device 220 as an IMU error estimated value.
Further, the Kalman filter 150 outputs error estimation values of the position, posture, and speed of the vehicle 200 calculated by the IMU processing unit 140 as navigation error estimation values.

  The IMU processing unit 140 includes a correction calculation unit 141 and a strapdown calculation unit 142.

  The correction calculation unit 141 adds the IMU error estimated value calculated by the Kalman filter 150 as an error correction amount to the angular velocity vector and the acceleration vector included in the inertial data at the target time. By adding the respective error correction amounts to the angular velocity vector and the acceleration vector, the angular velocity vector and the acceleration vector can be corrected.

The strapdown calculation unit 142 performs strapdown calculation (an example of inertial navigation) using the angular velocity vector and the acceleration vector corrected by the correction calculation unit 141.
In the strapdown calculation, the attitude (attitude angle vector) is obtained by integrating the angular velocity vector, and the speed (speed vector) is obtained by integrating the acceleration vector.
Further, the distance (distance vector) is obtained by integrating the velocity (velocity vector), and the position (three-dimensional coordinate value) of the target time is obtained by adding the distance vector to the previously calculated position.

  The strapdown calculation unit 142 adds the navigation error estimated value calculated by the Kalman filter 150 as an error correction amount to the position, posture, and speed of the vehicle 200 at the target time obtained by the strapdown calculation. By adding the respective error correction amounts to the position, orientation, and speed, the position, orientation, and speed can be corrected.

The skid characteristic learning unit 110 calculates the cornering power of the vehicle 200, the offset error of the odometry 230, and the scale factor error based on the speed vector calculated by the strapdown calculation unit 142.
Cornering power, offset error, and scale factor error are examples of ODO error estimates.

FIG. 12 is a relationship diagram between the vehicle coordinate system xyz and the offset error os of the odometry 230 in the first embodiment.
FIG. 13 is a diagram showing the side slip angle β in the first embodiment.
The offset error os of the odometry 230 will be described below based on FIG. 12 and FIG.

In FIG. 12, the vehicle 200 is tilted according to the balance of a mounted object (for example, a person on board), the undulation of the road, etc., and skids as shown in FIG. 13.
Therefore, unlike the tire direction x of the vehicle 200 and the traveling direction V of the vehicle 200, the speed in the front direction x (speed scalar) calculated from the vehicle speed pulse of the odometry 230 has an error relative to the speed in the traveling direction V. Have. This error amount is called an offset error of the odometry 230.
os [0] indicates an offset error in the z-axis direction, and os [1] indicates an offset error in the y-axis direction.

  In FIG. 13, the vehicle 200 receiving the lateral force travels in a direction of an angle shallower than the tire inclination. That is, the vehicle 200 skids during cornering or the like, and produces a skid angle β. The skid angle β indicates the yaw angle formed by the tire direction and the traveling direction of the vehicle 200.

FIG. 14 is a graph showing the cornering power S _slip and the offset error os [1] in the first embodiment.
The cornering power S _slip and the offset error os [1] will be described below with reference to FIG.

The skid characteristic line L represents the relationship between the unit cornering force _ay and the skid angle β.
The unit cornering force _ay is a horizontal component value of the acceleration vector.
The cornering power S _slip is the slope of the skid characteristic straight line L.
The male set error os [1] corresponds to the skid angle β when the unit cornering force a _y is “0”. Another offset error os [0] is a value for the vertical direction component value of the acceleration vector.

FIG. 15 is a graph showing the scale factor error of the odometry 230 in the first embodiment.
The scale factor error of the odometry 230 will be described below with reference to FIG.

The solid line represents the relationship between the GPS distance calculated based on GPS data and the ODO distance calculated based on odometry data.
The scale factor error of the odometry 230 is a ratio (percentage) of the ODO distance to the GPS distance.
For example, if the GPS distance is “100 centimeters” and the ODO distance is “95 centimeters”, the scale factor error of the odometry 230 is “−5 percent”.
A dotted line indicates a case where the scale factor error of the odometry 230 is “0 percent”. That is, the GPS distance and the ODO distance are equal.

FIG. 16 is a flowchart showing the skid characteristic learning process (S500) in the first embodiment.
A specific example of the skid characteristic learning process (S500) in the first embodiment will be described below with reference to FIG.

In S510, the speed / acceleration calculation unit 131 multiplies the vehicle speed pulse included in the odometry data at the target time by the circumferential length of the tire of the vehicle 200 to obtain a scalar amount of the speed of the vehicle 200 at the target time (hereinafter referred to as a speed scalar). Is calculated.
The speed / acceleration calculation unit 131 integrates the speed scalar to calculate a distance (hereinafter referred to as an ODO distance).
It progresses to S520 after S510.

In S520, the skid characteristic learning unit 110 uses the velocity scalar calculated in S510 as the longitudinal component of the velocity vector, and uses the horizontal component value of the velocity vector at the target time calculated by the strapdown calculator 142.
The skid characteristic learning unit 110 calculates a value obtained by dividing the horizontal component value of the velocity vector by the velocity scalar as the skid angle.
That is, “side slip angle β = value component V _{y in} the horizontal direction of the velocity vector / velocity scalar V _odo ”. The angle at which the value of the tangent (tan) becomes “V _y / V _odo ” may be set as the skid angle β.
After S520, the process proceeds to S530.

In S530, the skid characteristic learning unit 110 associates the skid angle calculated in S520 with the left-right direction component value of the acceleration vector at the target time corrected by the correction calculation unit 141, and adds it to the navigation data storage unit 190 for storage. To do.
The navigation data storage unit 190 stores a plurality of sideslip angles and left-right direction component values (unit cornering forces) of acceleration vectors.
It progresses to S540 after S530.

In S540, the skid characteristic learning unit 110 calculates a skid characteristic line based on a plurality of sideslip angles and unit cornering forces accumulated in the navigation data storage unit 190 (see FIG. 14).
It progresses to S550 after S540.

In S550, the skid characteristic learning unit 110 calculates the cornering power of the vehicle 200 and the offset error of the odometry 230 based on the skid characteristic line (see FIG. 14).
After S550, the process proceeds to S560.

In S560, the skid characteristic learning unit 110 calculates the scale factor error of the odometry 230 based on the distance (GPS distance) calculated based on the velocity vector by the strapdown calculation unit 142 and the ODO distance calculated in S510. (See FIG. 15).
After S560, the skid characteristic learning process (S500) ends.

FIG. 17 is a flowchart showing speed residual calculation processing (S410) in the first embodiment.
A specific example of the speed residual calculation process (S410) in the first embodiment will be described below based on FIG.

In S411, the speed / acceleration calculation unit 131 calculates a speed scalar based on the vehicle speed pulse included in the odometry data at the target time.
The speed / acceleration calculation unit 131 differentiates the speed scalar to calculate an acceleration scalar.
It progresses to S412 after S411.

In step S412, the speed / acceleration prediction unit 132 determines the target time based on the speed scalar calculated in step S411, the ODO error estimated value calculated by the skid characteristic learning unit 110, and the acceleration vector corrected by the correction calculation unit 141. The speed vector of the vehicle 200 is calculated.
The speed / acceleration prediction unit 132 calculates the acceleration vector of the vehicle 200 at the target time based on the acceleration scalar calculated in S411.

A calculation formula (2) of the speed vector ^Vb of the vehicle 200 is shown below.
In Expression (2), “V _odo ” is a velocity scalar, “os [0]” is a pitch angle offset error, “os [1]” is a yaw angle offset error, “S _slip ” is cornering power, and “a “ _y ” indicates a component value (unit cornering force) in the horizontal direction of the acceleration vector.

The speed / acceleration prediction unit 132 calculates an acceleration vector by replacing the speed scalar V _odo of Expression (2) with an acceleration scalar.

  It progresses to S413 after S412.

In S413, the speed / acceleration residual calculation unit 133 calculates the difference between the speed vector calculated in S412 and the speed vector of the target time calculated by the strapdown calculation unit 142 as the speed vector residual of the target time.
The speed / acceleration residual calculation unit 133 calculates the difference between the acceleration vector calculated in S412 and the acceleration vector at the target time corrected by the correction calculation unit 141 as the acceleration vector residual at the target time. After S413, the speed residual calculation process (S410) ends.

  The Kalman filter 150 receives the velocity vector residual and acceleration vector residual calculated in the velocity residual calculation process (S410), and calculates an IMU error estimated value and a navigation error estimated value at the target time.

FIG. 18 is a diagram illustrating an example of hardware resources of the self location apparatus 100 according to the first embodiment. In FIG. 18, the self-positioning device 100 includes a CPU 911 (Central Processing Unit) (also referred to as a microprocessor or a microcomputer). The CPU 911 is connected to the ROM 913, the RAM 914, the communication board 915, and the magnetic disk device 920 via the bus 912, and controls these hardware devices.
The communication board 915 is wired or wirelessly connected to a communication network such as a LAN (Local Area Network), the Internet, or a telephone line.
The magnetic disk device 920 stores an OS 921 (operating system), a program group 923, and a file group 924.
The program group 923 includes programs that execute the functions described as “units” in the embodiment. The program is read and executed by the CPU 911. That is, the program causes the computer to function as “to part”, and causes the computer to execute the procedures and methods of “to part”.
The file group 924 includes various data (input, output, determination result, calculation result, processing result, etc.) used in “˜part” described in the embodiment.
In the embodiment, arrows included in the configuration diagrams and flowcharts mainly indicate input and output of data and signals.
In the embodiment, what is described as “to part” may be “to circuit”, “to apparatus”, and “to device”, and “to step”, “to procedure”, and “to processing”. May be. That is, what is described as “to part” may be implemented by any of firmware, software, hardware, or a combination thereof.

In the first embodiment, for example, the following self-positioning device 100 and self-positioning algorithm have been described.
The position of a platform (for example, the vehicle 200) on which these sensors are mounted is determined by analyzing GPS, inertial measurement device (IMU), and odometry (ODO) data.
At this time, the process using the same data is performed twice, and the first process result (IMU / ODO error estimation initial value) is used for the second time.
Thereby, the self-localization accuracy can be improved as compared with the case where the process is performed only once.

Embodiment 2. FIG.
The first embodiment is a form suitable for post-processing for locating the self-position after all the navigation data collection is completed.
In the second embodiment, a mode suitable for real-time processing for locating the self-position while collecting navigation data will be described.

FIG. 19 is a functional configuration diagram of the self-positioning apparatus 100 according to the second embodiment.
The self-positioning apparatus 100 according to the second embodiment will be described below based on FIG.

  The navigation data storage unit 190 includes a GPS preliminary data group 194 (first GPS data group), an inertia preliminary data group 196 (first inertia data group), and odometry preliminary data collected by preliminary measurement performed in advance. It is assumed that the group 195 is stored in advance.

  In the navigation data storage unit 190, it is assumed that GPS data, inertia data, and odometry data collected by the current measurement currently performed are stored as needed.

The self-localization device 100 performs the first navigation processing (S100) using the GPS preliminary data group 194, the inertia preliminary data group 196, and the odometry preliminary data group 195 obtained by the preliminary measurement before the main measurement.
The self-localization device 100 uses the IMU / ODO error estimation initial value calculated in the first navigation process (S100) in the second navigation process (S200).
In the second navigation processing (S200), the self-positioning device 100 processes the GPS data group 191, inertia data group 193 (second inertia data group), and odometry data group 192 obtained by the main measurement to obtain a position. , Orientation and speed.

  Preliminary measurement is preferably performed immediately before the main measurement in the same area as the main measurement. This is because the closer the condition to the actual measurement is, the higher the reliability of the IMU / ODO error estimation initial value is and the more the orientation accuracy can be improved.

  DESCRIPTION OF SYMBOLS 100 Self-localization apparatus, 110 Side slip characteristic learning part, 120 GPS processing part, 121 Translation system double phase difference calculation part, 122 Translation system double phase difference prediction part, 123 Translation system double phase difference residual calculation part, 124 Posture system double phase difference calculation unit, 125 Posture system double phase difference prediction unit, 126 Posture system double phase difference residual calculation unit, 127 Design matrix calculation unit, 130 ODO processing unit, 131 Speed / acceleration calculation unit, 132 Speed / acceleration prediction unit, 133 Speed / acceleration residual calculation unit, 140 IMU processing unit, 141 correction calculation unit, 142 strapdown calculation unit, 150 Kalman filter, 190 navigation data storage unit, 191 GPS data group, 192 odometry data group, 193 inertia data group, 194 GPS preliminary data group, 195 odometry preliminary data group, 196 Preliminary data group, 200 vehicle, 201 top plate, 210 master station GPS, 211, 212 slave station GPS, 220 inertia measurement device, 230 odometry, 299 reference station GPS, 911 CPU, 912 bus, 913 ROM, 914 RAM, 915 communication board, 920 Magnetic disk device, 921 OS, 923 program group, 924 file group.

Claims (7)

Navigation data for storing a plurality of GPS data acquired at different times by a GPS (Global Positioning System) receiver included in the mobile body and a plurality of inertia data acquired at different times by an inertial measurement device included in the mobile body. A storage unit;
An error estimation unit that calculates an error estimation initial value as an estimation value of the measurement error of the inertial measurement device based on a plurality of GPS data and a plurality of inertial data stored in the navigation data storage unit;
An inertial data correction unit that corrects initial inertial data acquired first among the plurality of inertial data based on an error estimation initial value calculated by the error estimation unit;
An inertial navigation unit that calculates an orientation position of the start time as the position of the moving body at the start time at which the start inertia data was acquired based on the start inertia data corrected by the inertia data correction unit. Position location device. The navigation data storage unit stores a plurality of GPS data including a carrier phase,
The inertial navigation unit is configured to obtain the initial inertia data at the start time when the start inertia data is acquired based on the start inertia data acquired first among a plurality of inertia data before the error estimation initial value is calculated by the error estimation unit. A reference position of the start time is calculated as the position of the moving body, and the remaining inertia data excluding the start inertia data among the plurality of inertia data is calculated based on the inertia data corrected by the inertia data correction unit. Calculate the reference position of the time as the position of the moving body at the time at which the inertial data was acquired,
The position locator further comprises:
Based on the reference position of the time calculated by the inertial navigation unit and the carrier phase included in the GPS data acquired at the time, the carrier phase residual at the time is calculated as the carrier phase residual at the time A carrier phase residual calculation unit,
The error estimation unit calculates an error estimation value at the time as an estimation value of the measurement error of the inertial measurement device with respect to the time using the carrier phase residual at the time calculated by the carrier phase residual calculation unit. 2. The position locating apparatus according to claim 1, wherein the error estimation initial value is output based on an error estimation value of an acquisition time of the last GPS data acquired last among the plurality of GPS data. The carrier phase residual calculation unit calculates a carrier phase residual at the start time based on the position of the start time calculated by the inertial navigation unit and the carrier phase included in the start GPS data acquired at the start time. For each of the remaining GPS data excluding the head GPS data among the plurality of GPS data, the position of the time calculated by the inertial navigation unit and the carrier phase included in the GPS data acquired at the time And calculates the carrier phase residual at that time based on
The error estimation unit calculates an error estimation value at the time based on the carrier phase residual at the time calculated by the carrier phase residual calculation unit;
The inertia data correction unit corrects the next inertia data acquired next to the inertia data acquired at the time based on the error estimated value at the time calculated by the error estimation unit,
3. The position locating apparatus according to claim 2, wherein the inertial navigation unit calculates a position at the next time when the next inertia data is acquired based on the next inertia data corrected by the inertia data correction unit. A plurality of GPS data acquired at different times within a first time by a GPS (Global Positioning System) receiver included in the mobile object is stored as a first GPS data group, and the inertial measurement device provided in the mobile object A plurality of inertial data acquired at different times within the first time are stored as a first inertial data group, and a plurality of inertial data acquired at different times within the second time are stored in the first inertial data group. A navigation data storage unit for storing two inertial data groups;
An error estimation unit that calculates an error estimation initial value as an estimation value of the measurement error of the inertial measurement device based on the first GPS data group and the first inertial data group stored in the navigation data storage unit;
An inertial data correction unit that corrects the initial inertial data acquired first in the second inertial data group based on the error estimation initial value calculated by the error estimation unit;
An inertial navigation unit that calculates an orientation position of the start time as the position of the moving body at the start time at which the start inertia data was acquired based on the start inertia data corrected by the inertia data correction unit. Position location device. Navigation data for storing a plurality of GPS data acquired at different times by a GPS (Global Positioning System) receiver included in the mobile body and a plurality of inertia data acquired at different times by an inertial measurement device included in the mobile body. In the position locating method of the position locating device comprising a storage unit,
An error estimation unit calculates an error estimation initial value as an estimation value of the measurement error of the inertial measurement device based on a plurality of GPS data and a plurality of inertial data stored in the navigation data storage unit,
The inertia data correction unit corrects the first inertia data acquired first among the plurality of inertia data based on the error estimation initial value calculated by the error estimation unit,
The inertial navigation unit calculates an orientation position of the start time as the position of the moving body at the start time when the start inertia data is acquired based on the start inertia data corrected by the inertia data correction unit. A location method of the location device. A plurality of GPS data acquired at different times within a first time by a GPS (Global Positioning System) receiver included in the mobile object is stored as a first GPS data group, and the inertial measurement device provided in the mobile object A plurality of inertial data acquired at different times within the first time are stored as a first inertial data group, and a plurality of inertial data acquired at different times within the second time are stored in the first inertial data group. In a position locating method of a position locating apparatus comprising a navigation data storage unit for storing as a group of inertial data of 2,
An error estimation unit calculates an error estimation initial value as an estimation value of the measurement error of the inertial measurement device based on the first GPS data group and the first inertial data group stored in the navigation data storage unit,
The inertia data correction unit corrects the initial inertia data acquired first in the second inertia data group based on the error estimation initial value calculated by the error estimation unit,
The inertial navigation unit calculates an orientation position of the start time as the position of the moving body at the start time when the start inertia data is acquired based on the start inertia data corrected by the inertia data correction unit. A location method of the location device.   A position location program for causing a computer to execute the position location method according to claim 5 or 6.

Images