JP3505494B2 - Moving body direction estimation device and position estimation device - Google Patents

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a device for estimating the direction of a moving body and a device for estimating the position of the moving body.

[0002]

2. Description of the Related Art As a technique for estimating the position of a moving body, there is a device for estimating the position of the moving body by a global positioning system (hereinafter abbreviated as GPS). This device may not be able to receive the radio waves from the satellite, or may have a large error due to the influence of the reflected waves reflected by the radio waves from the satellite in a building or the like. Therefore, in such a device using GPS, it is necessary to interpolate and correct the position by GPS in combination with a sensor of another principle.

As a technique for estimating the direction of a moving body, G
There is a device that estimates the direction of a moving body by PS. This device, like the device that estimates the position, may not be able to obtain the orientation and may have large errors. In addition, the azimuth obtained from the GPS has a large error when the moving body moves at a low speed. As a device for estimating the azimuth, there is a device using a gyro. With this device, a highly accurate azimuth cannot be obtained when the moving body moves at a low speed to draw a large circle, or when the direction changes greatly at a small angular speed.

There is a device using a geomagnetic sensor as a device capable of obtaining a direction with high accuracy even when a moving body moves at a low speed. The geomagnetic sensor is affected by the magnetic field in the vicinity of a magnetic field source other than the geomagnetic field.
It causes a big error. Therefore, even in a device using such a geomagnetic sensor, it is necessary to perform correction using another sensor.

As described above, in the conventional technique for detecting an error due to an environmental change based on the difference between the same physical quantities derived from different kinds of sensors, that is, the azimuth difference or the position difference to reduce the error, the change in the speed of the moving body is considered. There are few things. Although there is a method in which the movement amount is taken into consideration, since the change in the movement amount in a certain period of time is used, it lacks the responsiveness to a sudden change in speed, resulting in low accuracy.

As a technique for estimating the azimuth based on information from a plurality of sensors, for example, there is a vehicle traveling azimuth detecting device disclosed in JP-A-5-34157. This device
The geomagnetic component detected by the geomagnetic sensor is decomposed into two-direction components orthogonal to each other on the horizontal plane, and based on the direction toward the coordinate position indicated by both geomagnetic components from the center coordinate value of the output circle that should include this coordinate position. Then, the traveling direction of the vehicle is detected. Furthermore, the travel direction change amount is detected by the gyro sensor, the difference between the output point movement amount detected by the geomagnetic sensor and the output point movement amount detected by the gyro sensor on the output circle during traveling for a certain distance is calculated, and this movement is calculated. The quality of the surrounding magnetic field environment is determined based on the amount difference. Based on the determined magnetic field environment, the number of sampling data of the geomagnetic sensor is calculated, the average value data of the sampling data of the number is calculated, and the candidate value of the output circle is calculated based on the plurality of average value data. The center point is corrected based on the candidate value.

As another conventional technique for estimating the azimuth based on information from a plurality of sensors, for example, Japanese Patent Laid-Open No. 6-2 is available.
There is a vehicle running azimuth detecting device 88776.
This device evaluates a GPS error dependent on a vehicle speed, an error in a gyro offset value, and a time change rate of the error based on the azimuth by GPS and the gyro, and the GPS by the evaluation. The azimuth of the vehicle is estimated from the azimuth and the gyro azimuth.

As a technique for estimating a position based on information from a plurality of sensors, there is an AVM system disclosed in Japanese Patent Laid-Open No. 5-79849. This system is GPS
The position detection means, the geomagnetic sensor, the gyro and the vehicle speed sensor are provided, and when the radio waves from the satellite can be received, the GPS position is set as the moving body position, and the geomagnetic sensor, the gyro and the vehicle speed are used only when the radio waves from the satellite cannot be received. Estimate the moving body position based on the output value of the sense,
When it returns to a state where it can receive radio waves from satellites, GPS
Returning to the position estimation by, when the GPS position and the position based on the output values of the geomagnetic sensor, the gyro and the vehicle speed sensor are deviated at the time of this return, the GPS position is corrected.

Further, as another conventional technique for estimating the position based on information from a plurality of sensors, Japanese Patent Laid-Open No. 8-33 is available.
There is a navigation device shown at 4348. This device calculates the relative position by a gyro and
The absolute position is obtained by PS, a correction range based on the error range related to the moving distance of the vehicle in the relative position is determined, and the absolute position by GPS is corrected on the common range between this correction range and the positioning error range by GPS. Is set as the vehicle position. This reduces the cumulative error related to the moving distance.

[0010]

[Patent Document 1] Japanese Patent Application Laid-Open No. 5-34157
In the vehicle traveling direction detection device shown in Fig. 3, the difference between the output point movement amount of the geomagnetic sensor and the output point movement amount of the gyro sensor is calculated during traveling for a certain distance, and the quality of the surrounding magnetic field environment is determined based on this movement amount difference. Is determined, the output circle center is corrected for orientation estimation, and if it does not move for a certain distance,
It is not possible to judge the quality of the magnetic field environment, and the accuracy of azimuth estimation deteriorates. Also, in this device, the estimation of the azimuth is based only on the output point by the magnetic sensor, and even if the gyro is more reliable than the magnetic sensor, the output by the magnetic sensor The azimuth is estimated based on points, and the azimuth estimation accuracy is low.

In the vehicle traveling direction detecting device disclosed in Japanese Patent Laid-Open No. 6-288767, the azimuth is obtained by evaluating the GPS and gyro errors. However, these errors are evaluated based on the accuracy of the device itself. Is the evaluation of
Errors have not been evaluated for the effects of the surrounding environment. Therefore, in this device, for example, when the GPS is affected by the surrounding environment, the estimation accuracy of the bearing becomes low.

Further, A shown in JP-A-5-79849
In the VM system, the position is basically estimated by GPS, and the position is estimated based on the output values of the geomagnetic sensor, the gyro and the vehicle speed sensor only when the radio waves from the satellite cannot be received. Therefore, this system is
If PS is affected by the surrounding environment, the accuracy of position estimation will be low.

Further, in the navigation device disclosed in Japanese Patent Laid-Open No. 8-334348, the position on the common range within each error range is only estimated as the own vehicle position in consideration of the constant error of the GPS and the gyro. Therefore, in this device, the accuracy of position estimation becomes low if the error exceeds a certain level for a while due to the influence of the surrounding environment.

Therefore, an object of the present invention is to provide an azimuth estimating device which can always estimate the azimuth with high accuracy without being influenced by the surrounding environment.

Another object of the present invention is to provide a position estimating device which can always estimate the position with high accuracy without being affected by the surrounding environment.

[0016]

According to the present invention, (a) first azimuth related information means for detecting an azimuth angular velocity which is a temporal change rate of azimuth of a moving body, and (b) detecting an absolute azimuth of the moving body. And (c) the detected azimuth angular velocity detected by the first azimuth related information means by the azimuth correction value based on the detected absolute azimuth detected by the second azimuth related information means. An azimuth estimation calculation means that corrects to obtain a calculated azimuth angular velocity and integrates the calculated azimuth angular velocity to obtain an azimuth estimated value that represents the absolute azimuth of the moving body, which is the difference between the azimuth estimated value and the detected absolute azimuth. The difference is divided by the speed of the moving body to obtain the azimuth judgment value, and this azimuth judgment value is in a range of a small value, a range of a large value, or an intermediate value range between these ranges. To determine if If the judgment value is in a small value range, the azimuth correction value is decreased, and if the azimuth judgment value is in an intermediate value range, the azimuth correction value is increased and the azimuth judgment value is large. When in the range, the azimuth estimating device for a moving body is included, which further includes azimuth estimation calculation means for making the azimuth correction value smaller than the azimuth correction value when the azimuth judgment value is in a small value range. Is.

According to the present invention, the first azimuth related information means detects the azimuth angular velocity as the azimuth related information, and the second azimuth related information means detects the absolute azimuth as the azimuth related information. Further, the azimuth estimation calculation means corrects the detected azimuth angular velocity with the azimuth correction value based on the detected absolute azimuth to obtain the calculated azimuth angular velocity, and integrates the calculated azimuth angular velocity to obtain the azimuth estimated value. The azimuth correction value is determined based on the reliability of each azimuth related information means such that the azimuth related information detected by the highly reliable azimuth related information means greatly affects the estimated azimuth value. Furthermore, since the reliability of each azimuth related information means is determined based on the azimuth related information acquired by each azimuth related information means, only the accuracy in the normal time that is not affected by the disturbance factor of each azimuth related information means itself. The reliability under the influence of the surrounding environment can be determined instead of the reliability based on the above. In such an environment in which the azimuth is to be estimated, the highly reliable azimuth related information means of each azimuth related information means is determined, and the azimuth is estimated so as to depend on the highly reliable azimuth related information means. You can The reliability of each azimuth related information means is determined based on the azimuth judgment value obtained by dividing the azimuth difference based on the azimuth related information acquired by each azimuth related information means by the speed, so that the influence of the speed of the moving body It is possible to make a stable determination of reliability. Moreover, if the reliability is judged based on the value obtained by dividing the heading difference by the speed, it is not necessary to obtain the tendency in a relatively long section, so that the heading can be detected while detecting an abnormality in real time regardless of the moving distance of the moving body. Can be estimated. By further determining the reliability, classifying into three states, and adjusting to a gain suitable for each state,
The azimuth can be estimated with high accuracy.

The present invention further includes (a) first azimuth related information means for detecting an azimuth angular velocity which is a temporal change rate of the azimuth of the moving body, and (b) a second azimuth for detecting an absolute azimuth of the moving body. Related information means, and (c) azimuth estimation calculation means for obtaining an azimuth estimated value representing the absolute azimuth of the moving body, and (c1) correction for azimuth from the detected azimuth angular velocity detected by the first azimuth related information means. An azimuth angular velocity subtraction unit that obtains a calculated azimuth angular velocity by subtracting a value, (c2) an integration unit that integrates the calculated azimuth angular velocity obtained by the azimuth angular velocity subtraction unit, and obtains an azimuth estimated value, and (c3) an integration unit Direction estimation value and second
Based on the azimuth difference and the azimuth gain obtained by the azimuth subtraction unit (c3), the azimuth angular velocity calculation unit is provided to the azimuth angular velocity calculation unit. An azimuth adjustment unit that obtains a given azimuth correction value and (c4) the azimuth difference obtained by the azimuth subtraction unit is divided by the speed of the moving object to obtain the azimuth judgment value, and the azimuth judgment value is 0 or more and is predetermined. If less than the first threshold,
It is determined that the reliability of both the first and second azimuth-related information acquisition means is high, the detected azimuth angular velocity and the detected absolute azimuth both influence the azimuth estimated value, and the detected azimuth angular velocity is the azimuth estimated value rather than the detected absolute azimuth. Set the first gain value that has a large effect on the azimuth gain given to the azimuth adjustment unit,
When the azimuth determination value is greater than or equal to the first threshold and less than the second threshold that is greater than the first threshold, it is determined that the reliability of the second azimuth related information means is higher than that of the first azimuth related information means. The detected azimuth angular velocity and the detected absolute azimuth both influence the estimated azimuth value, and the detected absolute azimuth influences the estimated azimuth value more than the detected azimuth angular velocity.
The gain value is set as the azimuth gain given to the azimuth adjusting unit, and if the azimuth judgment value is equal to or greater than the second threshold value, the first value is set.
The third gain value smaller than the first gain value is determined such that the reliability of the azimuth related information means is higher than that of the second azimuth related information means, and the detected absolute azimuth has no or little effect on the azimuth estimated value. An azimuth estimation calculation unit having a azimuth estimation calculation unit having an azimuth reliability determination unit that sets the following gain value as a azimuth gain given to the azimuth adjustment unit.

According to the present invention, the first azimuth related information means detects the azimuth angular velocity as the azimuth related information, and the second azimuth related information means detects the absolute azimuth as the azimuth related information. Further, in the azimuth estimation calculating means, the azimuth correction value based on the detected absolute azimuth is subtracted from the detected azimuth angular velocity by the azimuth velocity subtraction unit, and the obtained azimuth angular velocity is integrated by the integration unit to obtain the azimuth estimation value. The azimuth correction value determines the reliability of each azimuth related information means by the azimuth reliability determination unit, and the azimuth related information detected by the highly reliable azimuth related information means has a great influence on the azimuth estimated value. It is obtained by the azimuth adjusting unit using the azimuth gain set to. Since the reliability of each azimuth related information means is determined based on the azimuth related information acquired by each azimuth related information means, it is based only on the accuracy at the normal time that is not affected by the disturbance factor of each azimuth related information means itself. It is possible to determine not reliability but reliability under the influence of the surrounding environment. In such an environment in which the azimuth is to be estimated, the highly reliable azimuth related information means of each azimuth related information means is determined, and the azimuth is estimated so as to depend on the highly reliable azimuth related information means. You can The reliability of each azimuth related information means is determined based on the azimuth judgment value obtained by dividing the azimuth difference based on the azimuth related information acquired by each azimuth related information means by the speed, so that the influence of the speed of the moving body It is possible to make a stable determination of reliability. Moreover, if the reliability is judged based on the value obtained by dividing the heading difference by the speed, it is not necessary to obtain the tendency in a relatively long section, so that the heading can be detected while detecting an abnormality in real time regardless of the moving distance of the moving body. Can be estimated. Further judging the reliability, 3
The bearing can be estimated with high accuracy by classifying into two states and adjusting to a gain suitable for each state.

The present invention also includes (a) first position-related information means for detecting the velocity of the moving body, (b) second position-related information means for detecting the absolute position of the moving body, and (c) The detected position detected by the first position related information means is corrected by the position correction value based on the detected absolute position detected by the second position related information means to obtain the calculated speed, and this calculated speed is integrated. A position estimation calculation means for obtaining a position estimation value representing the absolute position of the moving body,
The position difference, which is the difference between the position estimation value and the detected absolute position, is divided by the speed of the moving object to obtain the position determination value, and the position determination value is a small value range, a large value range and these ranges. The intermediate value range between the two is determined, and if the position determination value is in a small value range, the position correction value is reduced and the position determination value is an intermediate value. If it is in the range of, the position correction value is increased, and if the position determination value is in the range of large values, the position correction value is set to be larger than the position correction value when the position determination value is in the range of small values. And a position estimation calculation unit for further reducing the position of the mobile unit.

According to the invention, the first position-related information means detects the velocity as the position-related information, and the second position-related information means detects the absolute position as the position-related information. Further, the position estimation calculation means corrects the detected speed by the position correction value based on the detected absolute position to obtain the calculated speed, and integrates the calculated speed to obtain the position estimated value. The correction value for position is based on the reliability of each position-related information means,
The position-related information detected by the highly reliable position-related information means is determined so as to greatly affect the position estimated value. Further, since the reliability of each position-related information means is determined based on the position-related information acquired by each position-related information means, only the accuracy at the normal time not affected by the disturbance factors of each position-related information means itself The reliability under the influence of the surrounding environment can be determined instead of the reliability based on the above. In such an environment where the position is to be estimated, the highly reliable position-related information means of each position-related information means is determined, and the position is estimated so as to depend on the highly reliable position-related information means. You can Further, the reliability of each position-related information means is determined based on the position determination value obtained by dividing the position difference based on the acquired position-related information by each position-related information means by the speed. It is possible to make a stable determination of reliability. Moreover, if the reliability is judged based on the value obtained by dividing the position difference by the speed, it is not necessary to obtain the tendency in a relatively long section, so that the position can be detected while detecting the abnormality in real time regardless of the moving distance of the moving body. Can be estimated. Further, by determining the reliability, classifying into three states, and adjusting to a gain suitable for each state, the position can be estimated with high accuracy.

The present invention also includes (a) first position-related information means for detecting the speed of the moving body, (b) second position-related information means for detecting the absolute position of the moving body, and (c) Position estimation calculation means for obtaining a position estimation value representing the absolute position of the moving body, wherein (c1) the position correction value is subtracted from the detected speed detected by the first position-related information means to obtain the calculation speed. A speed subtraction unit, (c2) an integration unit that integrates the calculated speeds obtained by the speed subtraction unit to obtain a position estimation value, and (c3) a position estimation value obtained by the integration unit;
A position subtraction unit for obtaining a position difference which is a difference from the detected absolute position detected by the position-related information means, and (c3) a position difference given to the speed calculation unit based on the position difference and the position gain obtained by the position subtraction unit. A position adjustment unit that obtains a position correction value that is obtained, and (c4) obtain a position determination value by dividing the position difference obtained by the position subtraction unit by the speed of the moving body. If less than 1 threshold,
It is determined that the reliability of the first and second position-related information acquisition means is both high, the detected speed and the detected absolute position both influence the position estimated value, and the detected speed is larger than the detected absolute position in the position estimated value. If the first gain value that affects is set as the position gain given to the position adjusting unit, and if the position determination value is greater than or equal to the first threshold value and less than the second threshold value greater than the first threshold value, the second It is determined that the reliability of the position-related information means is higher than that of the first position-related information means, the detected speed and the detected absolute position both influence the position estimated value, and the detected absolute position changes from the detected speed to the position estimated value. A second gain value that is larger than the first gain value that has a large effect is set as the position gain given to the position adjustment unit, and the position determination value is set to the second gain value.
If it is equal to or greater than the threshold, it is determined that the reliability of the first position-related information means is higher than that of the second position-related information means, and the detected absolute position does not affect the position estimation value or hardly affects the position estimated value. A position estimation calculation unit having a position reliability determination unit that sets a gain value smaller than a first gain value and equal to or less than a third gain value, as a position gain given to the position adjustment unit. It is an estimation device.

According to the invention, the first position-related information means detects the velocity as the position-related information, and the second position-related information means detects the absolute position as the position-related information. Further, in the position estimation calculation means, the position correction value based on the detected absolute position is subtracted from the detected speed by the speed subtraction unit, and the obtained calculation speed is integrated by the integration unit to obtain the position estimation value. The position correction value determines the reliability of each position-related information unit by the position reliability determination unit, and the position-related information detected by the highly reliable position-related information unit has a great influence on the position estimation value. The position gain is set by the position adjustment unit. Further, since the reliability of each position-related information means is determined based on the position-related information acquired by each position-related information means, only the accuracy at the normal time not affected by the disturbance factors of each position-related information means itself The reliability under the influence of the surrounding environment can be determined instead of the reliability based on the above. In such an environment where the position is to be estimated, the highly reliable position-related information means of each position-related information means is determined, and the position is estimated so as to depend on the highly reliable position-related information means. You can The reliability of each position-related information means is
Since it is determined based on the position determination value obtained by dividing the position difference based on the position-related information acquired by each position-related information means by the speed, it is possible to eliminate the influence of the speed of the moving body and to make a stable reliability determination. Become. Moreover, if the reliability is judged based on the value obtained by dividing the position difference by the speed, it is not necessary to obtain the tendency in a relatively long section, so that the position can be detected while detecting the abnormality in real time regardless of the moving distance of the moving body. Can be estimated. Further, by determining the reliability, classifying into three states, and adjusting to a gain suitable for each state, the position can be estimated with high accuracy.

[0024]

1 is a block diagram showing a schematic configuration of a position estimation device 1 for a moving body according to an embodiment of the present invention. The position estimation device 1 is, for example, 2 along the ground surface.
This device is mounted on a moving body (illustrated in FIG. 3 to be described later) 20 that moves dimensionally and estimates the position of the moving body 20. The moving body 20 may be a moving object, for example, a vehicle such as an automobile, a machine such as a construction machine or an industrial machine that is remotely controlled, or an autonomous mobile robot. It may be a moving robot. In the drawings, a vehicle is schematically shown as the moving body 20.

The position estimating device 1 includes a differential type global positioning system (DGPS) receiver 2 which is position detecting means, a speedometer 3 which is speed detecting means, a geomagnetic sensor 4 which is direction detecting means, and an azimuth angular velocity. It includes a gyro 5 as a detection means and a position / orientation calculation section 6 as an arithmetic processing means. The DGPS receiver 2, the speedometer 3, the geomagnetic sensor 4, and the gyro 5 are position-related information means, and the position / azimuth calculation unit 6 is position estimation calculation means. Hereinafter, the DGPS receiver 2, the speedometer 3, the geomagnetic sensor 4, and the gyro 5 may be collectively referred to as position-related information means.

As described above, the position estimation device 1 includes a plurality of position-related information means 2-5 and a position / azimuth calculation unit 6.
The respective position-related information acquisition means, that is, the DGPS receiver 2, the speedometer 3, the geomagnetic sensor 4 and the gyro 5 acquire information related to the position of the mobile body 20 by different principles. The position-related information includes information for calculating the position P of the moving body 20 and the position of the moving body 20, for example, the velocity V of the moving body 20, the azimuth θ, and the azimuth angular velocity ω. The position / orientation calculation unit 6, which is a position estimation calculation means,
The reliability of each position-related information means 2-5 is determined based on the acquired position-related information by each position-related information means 2-5.
The position of the moving body 20 is calculated from the acquired position related information by the position related information units 2 to 5 so that the acquired position related information by the highly reliable position related information units 2 to 5 has a great influence on the position estimated value. Estimate P0.

The position estimating device 1 of the present embodiment also includes an azimuth estimating device 7. The azimuth estimating device 7 includes the geomagnetic sensor 4, the gyro 5, and the position / azimuth calculator 6. including. Geomagnetic sensor 4 and gyro 5
Is both the position-related information means and the azimuth-related information means, and the position / azimuth calculation unit 6 is both the position estimation calculation means and the azimuth estimation calculation means. Hereinafter, the geomagnetic sensor 4 and the gyro 5 may be collectively referred to as azimuth-related information means.

As described above, the azimuth estimating apparatus 7 included in the azimuth estimating apparatus 1 includes a plurality of azimuth related information means 4 and 5 and a position / azimuth calculating unit 6. The respective azimuth related information means, that is, the geomagnetic sensor 4 and the gyro 5 acquire the information related to the azimuth of the moving body 20 by different principles. The azimuth-related information includes the azimuth θ of the moving body 20 and information for calculating the azimuth of the moving body 20, for example, the azimuth angular velocity ω. The position / azimuth calculation unit 6 which is the azimuth estimation calculation means determines the reliability of each azimuth related information means 4 and 5 based on the acquired azimuth related information obtained by the azimuth related information means 4 and 5, and the reliability among them is calculated. The azimuth θ0 of the moving body 20 is estimated from the azimuth-related information acquired by the azimuth-related information means 4 and 5 so that the azimuth-related information acquired by the azimuth-related information means 4 and 5 having high property has a great influence on the estimated azimuth value. .

FIG. 2 is a block diagram showing a specific configuration of the position estimation device 1. The DGPS receiver 2 receives GPS signals which are radio signals from a plurality of satellites,
The absolute position P of the moving body 20 expressed in longitude and latitude is calculated based on the information for obtaining the position represented by the signal and the correction information sent from the reference station whose accurate position has been detected.
Is detected with high accuracy. Hereinafter, the absolute position P may be referred to as DGPS positioning data.

The speedometer 3 detects the speed V of the moving body 20. For example, when the moving body 20 has wheels such as a vehicle, the speedometer 3 may be configured to detect the rotation speed of the wheels by an encoder or the like to obtain the speed V. Alternatively, the speedometer 3 may be configured to emit infrared rays from a fixed object, receive a reflected wave reflected by the moving body 20, and detect the speed V from a change in wavelength.

The geomagnetic sensor 4 detects the geomagnetic azimuth with respect to the moving body (the azimuth of the N pole of the geomagnetism), and obtains the absolute azimuth θ represented by the azimuth angle of the traveling azimuth of the moving body 20 with respect to the geomagnetic azimuth. The gyro 5 detects the azimuth angular velocity ω, which is the temporal change rate of the azimuth of the moving body 20. Gyro 5
May be realized by, for example, a mechanical gyro, a gas rate gyro, a vibrating gyro, an optical fiber gyro, or the like.

The position / azimuth calculation unit 6 includes an azimuth estimation unit 10
And a position estimation unit 11. The azimuth estimating unit 10 estimates an absolute azimuth (hereinafter, may be referred to as an “azimuth estimation value”) of the moving body 20 based on each detection value (each acquisition azimuth related information) of the geomagnetic sensor 4 and the gyro 5. The azimuth estimating unit 10 includes an offset setter 12, an azimuth angular velocity subtractor 13, an azimuth angular velocity integrator 14, and an azimuth subtractor 1.
5, an azimuth reliability determiner 16 and an azimuth adjuster 17.

The offset setter 12 determines the offset value ω1 based on the absolute orientation of the moving body 20 in the initial state.
Is set and stored. Azimuth and angular velocity subtractor 13
Is given the detected azimuth angular velocity ω and the offset value ω1, and the azimuth adjuster 17 gives the azimuth correction value ω2. The azimuth angular velocity subtractor 13 uses the detected azimuth angular velocity ω to correct the azimuth correction value ω2 and the offset value ω1.
Is subtracted.

The azimuth angular velocity integrator 14 calculates the azimuth angular velocity ω3 (= ω-ω1-ω2) from the azimuth angular velocity subtractor 13.
Is given. The azimuth angular velocity integrator 14 integrates the calculated azimuth angular velocity ω3 to obtain the absolute azimuth of the moving body 20, and outputs the calculated absolute azimuth as the azimuth estimated value θ0. The azimuth subtractor 15 is provided with the azimuth estimated value θ0 and the detected absolute azimuth θ detected by the geomagnetic sensor 4. The azimuth subtractor 15 subtracts the detected absolute azimuth θ from the azimuth estimated value θ0 to obtain a azimuth difference Δθ (= θ0−θ).

The azimuth reliability determiner 16 is provided with the azimuth difference Δθ and the velocity V. In the azimuth reliability determiner 16, the reliability of the geomagnetic sensor 4 and the gyro 5, specifically the geomagnetic sensor 4 and the gyro, is determined based on the azimuth judgment value Jθ obtained by dividing the azimuth difference Δθ by the detection speed V. It is determined which one of 5 has higher reliability, and the azimuth gain Gθ based on the result of the determination is given to the azimuth adjuster 17.

The azimuth adjuster 17 is provided with the azimuth difference Δθ and the azimuth gain Gθ. The azimuth adjuster 17 calculates the azimuth difference Δθ according to a predetermined transfer function, and obtains the azimuth correction value ω2 to be given to the azimuth angular velocity subtractor 13. The azimuth gain in the transfer function for obtaining the azimuth correction value ω2 is variable and is the azimuth gain Gθ given from the azimuth reliability determiner 16. The azimuth gain Gθ is determined so that the azimuth-related information of the geomagnetic sensor 4 and the gyro 5, which has a higher reliability, greatly affects the estimated azimuth value θ0.

In this position estimating device 1, the azimuth estimated value θ0 calculated based on the azimuth angular velocity by the gyro 5 is
The azimuth of the moving body 20 is estimated by performing correction based on the absolute azimuth θ detected by the geomagnetic sensor 4. That is, the azimuth of the moving body 20 is estimated by combining the gyro 5 which is an integral type sensor obtained by performing an integral calculation in the direction estimation and the geomagnetic sensor 4 which is a non-integrating sensor obtained without performing an integral calculation in the direction estimation. It

FIG. 3 is a diagram for explaining the influence of speed in the reliability judgment of the respective azimuth related information means 4 and 5.
FIG. 3 (1) is a plan view showing the movement route 21 of the moving body 20, and FIG. 3 (2) is a graph showing the relationship between the time t and the azimuth difference Δθ when the moving body 20 moves along the movement route 21. 3 (3) is a graph showing the relationship between the time t when the moving body 20 moves on the moving route 21 and the bearing determination value (= bearing difference / speed) Jθ. In FIG. 3 (2), the horizontal axis represents time t, and the vertical axis represents azimuth difference Δθ. In FIG. 3C, the horizontal axis is the time t, and the vertical axis is the azimuth determination value Jθ. Although the reliability of the geomagnetic sensor 4 and the gyro 5 cannot be determined simply by looking at the azimuth difference Δθ, the azimuth difference due to the accuracy of the gyro 5 itself does not change rapidly, whereas the magnetic field generation source S1 Since the bearing difference due to the influence of the disturbance factor on the geomagnetic sensor 4 changes rapidly, the bearing difference Δ
It can be determined by looking at the rate of change of θ.

When the moving body 20 moves along the linear moving path 21 adjacent to the magnetic field generating source S1 which is a disturbance factor as shown in FIG. 3A, the azimuth based on the gyro 5 does not change, but the geomagnetism is changed. The detection direction θ by the sensor 4 is
It changes under the influence of the magnetic field generation source S1. Therefore, the azimuth difference Δθ changes as the time t elapses.

The relationship between the azimuth difference Δθ and the time t is as follows when the moving body 20 indicated by the graph line 23 moves at a low speed.
This differs from the case where the moving body 20 indicated by the graph line 24 moves at a high speed. In other words, the rate of change of the azimuth difference per unit time varies depending on the speed of the moving body. Therefore, since the moving speed V of the moving body 20 affects, the azimuth difference Δθ
It is difficult to judge the reliability with the time change rate of.

In this embodiment, the azimuth judgment value Jθ obtained by dividing the azimuth difference Δθ by the detected velocity V of the moving body is used.
The relationship between the judgment value Jθ for azimuth and the time t is shown by the graph line 2
When the moving body 20 shown by 5 moves at a low speed, and when the moving body 20 shown by the graph line 26 moves at a high speed,
It has a similar relationship. In this way, the value can be converted into a value proportional to the rate of change per unit moving distance, and can be a value that is not affected by the velocity V of the moving body. Therefore, it is possible to determine which of the geomagnetic sensor 4 and the gyro 5 has higher reliability by using the azimuth determination value Jθ.

FIG. 4 is a graph showing the azimuth gain Gθ. In FIG. 4, the horizontal axis is the azimuth determination value Jθ, and the vertical axis is the azimuth gain Gθ. Further, FIG. 5 is a diagram showing a state transition of the azimuth adjuster 17. The bearing gain Gθ is determined based on the bearing determination value Jθ as described above.

The judgment value Jθ for azimuth is 0 or more and the first
When the value is smaller than the threshold value Jθ1, it is determined that the reliability of both the geomagnetic sensor 4 and the gyro 5 is high, and the azimuth adjuster 17 is set to the normal state 30. In this case, a high estimation accuracy can be ensured regardless of the influence of the geomagnetic sensor 4 or the gyro 5, but in the present embodiment, the detected azimuth angular velocity ω has a small dispersion and is stable without being influenced by the surrounding environment. The accuracy of the azimuth estimation is further enhanced by increasing the influence of the gyro 5 that is operating. Therefore, in the no-abnormality state 30, the azimuth gain Gθ is compared so that both the geomagnetic sensor 4 and the gyro 5 affect the estimated azimuth value θ0, and the azimuth angular velocity ω detected by the gyro 5 greatly affects the estimated azimuth value θ0. The first gain value Gθ1 is set to be relatively small.

Further, the azimuth gain Gθ is set to a relatively small value G
By setting to θ1, the direction estimation device 7 weakens the force for removing the error when an error occurs due to the influence of the surrounding environment, so the sensitivity to the direction difference Δθ with respect to the error is increased, and the detection of the abnormality is performed. Making it easy. It should be noted that even if the respective errors of the geomagnetic sensor 4 and the gyro 5 are not so small, they may be in the same state by chance, but since each sensor is based on a principle different from each other, this state does not last long. Eventually, an error will become apparent and can be detected.

When the judgment value Jθ for the azimuth is equal to or larger than the first threshold value Jθ1 and is an intermediate value less than the second threshold value Jθ2 (> Jθ1), the azimuth difference Δθ is obtained.
It is difficult to determine which is the cause of the gyro and gyro 5. The geomagnetic sensor 4 is smaller in size than the error due to the influence of the environment even if an error occurs even if there is no disturbance factor for the geomagnetism in the surroundings.
Moreover, as shown in FIG. 6, since it is short-term, even if the influence of the gyro 5 is mistakenly considered to be large and the influence on the estimated value of the geomagnetic sensor is increased, the influence is small. On the other hand, when an error occurs, the gyro 5 gradually accumulates in the azimuth estimation value, so if the influence of the geomagnetic sensor 4 is mistakenly considered to be large and the influence on the estimation value of the gyro 5 is increased, as shown in FIG. Accumulation of error is accelerated and the estimated value deviates from the true value. Therefore, in the present embodiment, it is determined that the azimuth difference is likely due to the error of the gyro 5, and the azimuth adjuster 17 is set to the integral suppression state 31. In other words, the influence of the gyro 5 which is an integral type sensor is suppressed with respect to the azimuth, and the influence of the geomagnetic sensor 4 is increased. Therefore, azimuth gain G
θ has a relatively large second gain value Gθ2 (so that both the geomagnetic sensor 4 and the gyro 5 affect the estimated bearing value θ0, and the absolute bearing θ detected by the geomagnetic sensor 4 greatly affects the estimated bearing value θ0. > Gθ1).

When the azimuth judgment value Jθ is a large value equal to or larger than the second threshold value Jθ2, it is judged that the geomagnetic sensor 4 is affected by the disturbance factor, and the azimuth adjuster 17 is in the difference neglecting state 32. It is said that In this case, the orientation is estimated based on the gyro 5 alone or on the basis of only the gyro 5 in a reliable manner. Therefore, in the difference neglecting state 32, the azimuth gain Gθ is smaller than the first gain value Gθ1 and is close to almost zero so that the absolute azimuth θ detected by the geomagnetic sensor 4 does not affect the azimuth estimated value θ0. The value is equal to or less than the value Gθ3, and is therefore 0 or a value close to 0. Moreover, in this case, the azimuth gain Gθ is determined so as to decrease as the azimuth determination value Jθ increases, and the azimuth can be estimated more preferably with high accuracy.

The direction of the azimuth adjuster 17 is determined based on the reliability judgment as described above. When the azimuth determination value Jθ is small, the state transitions to the no-abnormal state 30 and the azimuth determination value J
When θ is an intermediate value, the transition to the integral suppression state 31 occurs,
When the judgment value Jθ for azimuth is large, the state shifts to the neglected difference state 32.

Since the geomagnetic sensor 4, which is a non-integral type sensor with respect to the azimuth, does not have a cumulative error, the absolute azimuth can be detected with high accuracy when there is no disturbance factor. Due to the influence, a large error will be included in the detected absolute direction. Further, the gyro 5 which is an integral type sensor with respect to the azimuth is not affected by a disturbance factor unlike the geomagnetic sensor 4, but is less accurate than the geomagnetism sensor 4 when there is no disturbance factor due to accumulated error or the like. .

Based on this point, the reliability of each azimuth related information acquisition means 4 and 5 is determined as described above, and one of the acquired position related information obtained by the highly reliable azimuth related information acquisition means is the more reliable one. The azimuth of the moving body 20 is estimated by the azimuth estimating unit 10 so as to greatly affect Furthermore, since the reliability of each azimuth related information means 4 and 5 is determined based on the acquired position related information, the reliability of each azimuth related information means 4 and 5 is high in the environment where the azimuth is to be estimated. The position related information means is determined. Therefore, the azimuth can always be estimated with high accuracy while detecting the abnormality in real time.

Further, since the reliability of each azimuth-related information means 4 and 5 is judged based on the judgment value Jθ for the azimuth obtained by dividing the azimuth difference Δθ by the speed V, the influence of the speed of the moving body 20 is eliminated and the stability is stable. It is possible to judge the reliability.

Referring again to FIG. 2, position estimating section 11
Is the azimuth estimated value θ0 and DG by the azimuth estimating unit 10.
Each detected value of the PS receiver 2 and the speedometer 3, in other words, D
Based on each detection value (each acquisition position related information) of the GPS receiver 2, the speedometer 3, the geomagnetic sensor 4 and the gyro 5,
The absolute position of the moving body 20 (hereinafter sometimes referred to as “position estimated value”) is estimated. The position estimation unit 11 includes a velocity component calculator 35, a velocity subtractor 36, a velocity integrator 37,
A position subtractor 38, a position reliability determiner 39, and a position adjuster 40 are included.

The velocity component calculator 35 has an estimated bearing value θ0.
And the detection speed V is given. This velocity component calculator 3
In 5, the component of the velocity V of the moving body 20 in the directions of two orthogonal axes is calculated. The biaxial directions are directions parallel to the longitude line and the latitude line, respectively, and the velocity component Vx in the latitude line direction and the velocity component Vy in the longitude line direction are expressed by the following equations (1) and (2).
Required by. Vx = Vcos θ0 (1) Vy = Vsin θ0 (2)

The velocity subtracter 13 includes velocity components Vx and Vx.
In addition to y, the position adjuster 40 provides position correction values Vx1 and Vy1. This speed subtractor 13
Then, the position correction value Vx1 in the latitude line direction is subtracted from the detected speed component Vx in the latitude line direction, and the position correction value Vy1 in the longitude line direction is subtracted from the detected speed component Vy in the longitude line direction to obtain the calculated speed Vx2. Vy2 is required.

The speed integrator 37 has a calculation speed Vx2 (=
Vx-Vx1) and Vy2 (= Vy-Vy2) are given. In this speed integrator 37, the calculation speeds Vx2 and Vy2
Are integrated to obtain the absolute position of the moving body 20 and output as the position estimated value P0. The position estimated value P0 is specifically represented by the latitude x0 and the longitude y0 obtained by integrating the respective calculation speeds Vx2 and Vy2.

Position estimate value P0 and detected absolute position P are applied to position subtractor 38. The position subtractor 38 subtracts the detected absolute position P from the position estimated value P0. Specifically, regarding the longitude and the latitude, the estimated latitude x0 is subtracted from the detected latitude x, and the estimated longitude y0 is subtracted from the detected longitude y to obtain a latitude difference Δx (= x−x0) and a longitude difference Δy (= y. Position difference ΔP (= P0
-P).

The position reliability determining unit 39 includes a position difference ΔP,
The detection speed V is given, and the DGPS receiver 2 gives information on whether the DGPS positioning data is valid or invalid. In this position reliability determination device 39, the position determination value JP obtained by dividing the position difference ΔP by the speed V of the moving body 20 and the position difference Δ
Based on whether the P and DGPS positioning data is valid or invalid, the reliability of the DGPS receiver 2, the speedometer 3, the geomagnetic sensor 4, and the gyro 5, specifically, the DGPS receiver 2
It is determined which is more reliable, that is, the combination including the speedometer 3, the geomagnetic sensor 4 and the gyro 5, and the position gain GP based on the determination result is given to the position adjuster 40. In order to avoid a redundant expression, the configuration including the speedometer 3, the geomagnetic sensor 4 and the gyro 5 will be referred to as a “combination” below.
May be written.

When the DGPS receiver 2 determines that the conditions necessary for highly accurate positioning such as the number of receivable GPS satellites and the presence or absence of correction information from the reference station are not satisfied, whether the data is valid together with the DGPS positioning data. It is also possible to output determination data indicating whether it is invalid. If invalid, subtractor 3
The output ΔP of 8 is also invalid data. However, the determination by the DGPS receiver 2 cannot sufficiently deal with disturbance factors such as reflected waves. Therefore, based on the determination data of the DGPS receiver 2, the position reliability determiner checks the validity / invalidity of the DGPS positioning data, and if invalid, the position gain GP.
Is set to 0 to block the influence of invalid data on the position estimation value P0, and a value obtained by directly integrating the velocities Vx and Vy is used as the position estimation value P0.

A position difference ΔP and a position gain GP are given to the position adjuster 40. The position adjuster 40 calculates the position difference ΔP according to a predetermined transfer function, and obtains the position correction values Vx1 and Vy1 to be given to the speed subtractor 36.
The position gain in the transfer function for obtaining the position correction values Vx1, Vy1 is variable, and the position reliability determiner 3
9 is the position gain GP given from 9. Position gain G
P is determined such that the position-related information of the DGPS receiver 2 or the combination having higher reliability has a large influence on the position estimation value.

In this position estimation device 1, the geomagnetic sensor 4
The position estimation value P0 calculated based on the azimuth estimated value θ0 calculated based on the azimuth angular velocity by the gyro 5 and the speed measured by the speedometer 3 while being corrected by the azimuth
The position of the moving body 20 is estimated by performing correction based on the absolute position P detected by the GPS receiver 2. That is, the speedometer 3, the gyro 5 and the geomagnetic sensor 4 which are integral type sensors obtained by performing integral calculation in position estimation.
And the DGPS receiver 2 which is a non-integral type sensor that is obtained without integrating in position estimation, the position of the moving body 20 is estimated.

In determining the reliability of the DGPS receiver 2 and the combination, the determination cannot be made simply by looking at the position difference ΔP, except when the DGPS receiver 2 cannot receive the DGPS signal. On the other hand, in the combination, the azimuth difference caused by the accumulated error of each means 3 to 5 does not change abruptly, and for example, the DGPS receiver of the disturbance factor such as the reflected wave of the GPS signal at the reflector which is the building. Since the position difference due to the influence on 2 rapidly changes, the reliability of the DGPS receiver 2 and the combination can be determined by looking at the rate of change of the position difference ΔP.

When the moving body 20 moves along the movement route such that the disturbance factor of the geomagnetism (magnetic field generation source S1) of FIG. 3 (1) is replaced with the disturbance factor of the GPS signal, the calculation is performed based on the combination. However, the detected position P by the DGPS receiver 2 changes under the influence of disturbance factors. Therefore, the position difference Δ obtained by the position subtractor 38
P changes as the time t elapses.

The relationship between the position difference ΔP and the time t is shown in FIG.
The relationship in which the vertical axis of (1) is replaced by the position difference ΔP is shown, and the relationship becomes different depending on the speed of the moving body 20. In other words, the rate of change of the position difference per unit time varies depending on the speed of the moving body. Therefore, it is difficult to judge the reliability by looking at the rate of change of the position difference ΔP with time.

In this embodiment, the position determination value JP obtained by dividing the position difference ΔP by the detected velocity V of the moving body is used.
The relationship between the position determination value JP and the time t is shown in FIG.
The vertical axis of is replaced with the position determination value JP, and shows the same relationship regardless of the speed of the moving body 20. In this way, by converting the value into a value proportional to the rate of change per unit moving distance, a value that is not affected by the velocity V of the moving body can be obtained. Therefore, by using this position determination value JP, it is possible to determine a decrease in reliability of the DGPS receiver 2 due to a disturbance factor of the GPS signal such as a reflected wave due to a reflector of the GPS signal. It is possible to determine which of them has higher reliability.

Position determination value JP for determining reliability
May be a value (Δx / Vx, Δy / Vy) divided by the velocity for each component in the latitude line direction and the longitude line direction, and the distance between the position estimation value P0 and the detection position P is divided by the velocity V. It may be a value (√Δx ² + y ² / V), and based on the azimuth of the moving body, a value separated into the position difference component in the traveling direction of the moving body and in the lateral direction perpendicular thereto is used. Good.

When it is desired to simplify the determination once, a value obtained by dividing the distance by the velocity V is used. On the other hand, if it is calculated for each component, it is possible to detect a case where the direction changes suddenly even if the distance is the same. Further, if the direction of the moving body is used and the direction and the lateral direction are separated, for example, It can be used as one of the judgment materials for abnormality such as moving laterally while moving forward.

The position gain GP is the position difference Δ as described above.
Based on the position determination value JP obtained by dividing P by the speed V, the position difference ΔP, and the information on whether the DGPS positioning data is valid or invalid, the position reliability determination unit 39 determines it according to the following rules 1 to 5. .

Rule 1: When the DGPS positioning data is invalid, position gain GP = 0. Rule 2: the position determination value JP is 0 or more, and the first
When it is less than the threshold value JP1, the position gain GP is set to the first gain value GP1. Rule 3: When the position determination value JP is greater than or equal to the first threshold value JP1 and less than the second threshold value JP2, the position gain GP is set to the second gain value GP2 larger than the first gain value GP1. . Rule 4: When the position determination value JP is equal to or larger than the second threshold value JP2, the position gain GP is set to the third gain value GP3 smaller than the first gain value GP1. Rule 5: When the detected absolute position P is outside the estimation error range by dead reckoning, the position gain GP = 0.

According to these rules 1 to 5, rule 1
The position gain GP is determined by giving priority to the rule 5 and the rule 5. If the GPS signal cannot be received because it is blocked by a shield such as a building, position estimation based on the DGPS receiver 2 is impossible. Therefore, in order to facilitate the determination, in the first step, it is determined based on the information from the DGPS receiver 2 whether the DGPS positioning data is valid or invalid according to Rule 1, and if the DGPS positioning data is invalid, The position adjuster 40 uses dead reckoning (also called dead reckoning) without position estimation based on the DGPS receiver 2.
To be in a state. In this dead reckoning state, the position gain GP
Is set to 0 so as to obtain the position estimation value P0 based only on the position-related information detected by the speedometer 3, the geomagnetic sensor 4, and the gyro 5.

FIG. 7 is a diagram for explaining the determination according to Rule 5. If the judgment conditions of Rule 1 are not met,
That is, when the DGPS positioning data is determined to be invalid, the second step is to proceed to determination based on the position difference ΔP according to Rule 5. After the GPS signal is lost (lost), the GPS signal is captured again, and the DG is immediately received.
The PS receiver 2 may determine that the positioning data is valid, and in this case, the environment regarding the DGPS receiver 2 is GP.
Since it is an unstable environment close to the environment where S signals cannot be received,
The reliability of the DGPS receiver 2 is reduced, and the detected absolute position P by the DGPS receiver 2 is likely to include a large error. Therefore, in order to facilitate the determination, it is determined after Rule 1 that the detected absolute position P is outside the estimation error range by dead reckoning according to Rule 5. When the detected absolute position P is outside the estimation error range, the DGPS receiver 2
Is judged to be extremely low in reliability, the position adjuster 40
Similar to the case of Rule 1, the dead reckoning state without position estimation based on the DGPS receiver 2 is set. The estimation error range is G
Position estimation value P immediately before the PS signal is lost
When position estimation is performed by dead reckoning based on 0, the error that is considered to be included in the position estimation in this dead reckoning is cumulatively calculated and obtained.

If the DGPS receiver 2 determines that the positioning data is valid and the DGPS receiver 2 is reliable enough to contribute to the position estimation, the estimation error range itself has not been calculated. The judgment condition of 5 is not satisfied.
There is a possibility that the judgment conditions of Rule 5 may be met
After the S signal has been lost (lost), the GPS signal can be captured and received again, and after the DGPS receiver 2 determines that the positioning data is valid, the DGPS receiver 2 can contribute to position estimation with high reliability. Until the sex is restored.

As shown in FIG. 7, when the mobile unit 20 has time t =
When the GPS signal is lost at 0 and the vehicle travels along the route 45, the position is estimated by dead-reckoning in an invalid state where the GPS signal is not received. The position estimation value P0 based on this dead reckoning error has a large error with time t, and therefore the estimation error range in which the actual position may be included,
Therefore, the estimation error range centered on the position estimation value increases with the passage of time. Specifically, time t = t1
The estimation error range A2 at the position 47 at time t = t11 (> t10) becomes larger than the estimation error range A1 at the position 46 at 0, and further at the time t = t12 (> t11) than the estimation error range A2. The estimation error range A3 at the position 48 becomes large.

According to Rule 5, it is determined whether or not the detected absolute position P is outside (far away from) such an estimation error range. Lost GPS signal at time t = 0,
In the state where the GPS signal is re-acquired at 11, the predetermined range B1 centered on the absolute position P detected by the DGPS receiver 2 is
If it is outside the estimation error range A2 at time t11, the absolute position P detected by the DGPS receiver 2 is detected.
Is determined to include a large error, the position gain GP
Is set to 0. Further time passes, and at time t12, DG
When the predetermined range B2 centered on the absolute position P detected by the PS receiver 2 intersects with the estimation error range A3 at the time t12, and therefore each range A3, B2 has a common area, the DGPS receiver 2 estimates the position. Is considered to have been restored to a level of reliability that can contribute to the, and is excluded from the gain determination according to Rule 5. Here, the predetermined ranges B1 and B2 are D
It is a range based on the accuracy in a normal time that is not affected by the disturbance factors of the GPS receiver 2 itself.

When the determination condition of rule 5 is not satisfied, the third step is to proceed to determination according to rules 2 to 4 based on the position determination value JG. The determination value JG for position is 0 or more and the first such that the rule 2 is satisfied and the first
When the value is smaller than the threshold value JG1, it is determined that the reliability of both the combination and the DGPS receiver 2 is high, and the position adjuster 40 is set to the normal state. In this case, high estimation accuracy can be ensured regardless of the influence of the combination and the DGPS receiver 2, but in the present embodiment, the estimated orientation value θ0 and the detection speed by the combination not affected by the surrounding environment. Since V has a small variance and is stable, the influence of the combination is increased and the position estimation accuracy is further increased. Therefore, in the normal state, the position gain GP is
Both of the receivers 2 influence the position estimation value P0, and the combination estimation azimuth value θ0 and the detected speed V are the position estimation value P0.
The first gain value GP1 is set to a relatively small value so as to greatly affect 0.

Further, the position gain GP is set to a relatively small value G
Setting to P1 weakens the force with which the position estimation device 11 tries to remove the error when an error due to the influence of the surrounding environment occurs, so that the sensitivity to the position difference ΔP with respect to the error is increased and the detection of abnormality is performed. Making it easy. In addition, DGP
Even if the errors of the S receiver 2 and the combination are not so small, they may be in the same state by chance, but since each sensor is based on a principle different from each other, this state does not last long and The error becomes apparent and can be detected.

When the position determination value JG is equal to or larger than the first threshold value JG1 and less than the second threshold value JG2 (> JG1) such that the rule 3 is satisfied,
It is difficult to determine whether the position difference ΔP is caused by the combination or the DGPS receiver 2. When there is no disturbance factor for the GPS signal in the surroundings, the DGPS receiver 2 is smaller in size than the error due to the influence of the environment even if an error occurs, and as shown in FIG. Therefore, even if the influence on the estimated value of the DGPS receiver 2 is erroneously regarded as the influence of the combination, the influence is small. On the other hand, the union
When an error occurs, the position estimation value is gradually accumulated. Therefore, if the influence of the DGPS receiver 2 is mistakenly considered to be large and the influence on the estimated value of the combination is increased, the accumulation of the error is accelerated and estimated as shown in FIG. The value deviates from the true value. Therefore, in the present embodiment, it is determined that the error included in the combination is large, the position adjuster 17 is in the integral suppression state, and the combination that is the integral sensor obtained by the integral calculation is suppressed. In this integral suppression state, DG
The influence of the PS receiver 2 is increased. Therefore, the position gain GP has a relatively large first value so that both the combination and the DGPS receiver 2 influence the position estimation value P0, and the detected absolute position P by the DGPS receiver 2 greatly influences the position estimation value P0. 2 Gain value GP2 (> G
P1).

When the position determination value JP is a large value equal to or larger than the second threshold value JP2 so that the rule 4 is satisfied, the DGPS receiver 2 is affected by a disturbance factor and a position difference ΔP occurs. Therefore, the position adjuster 40 is set in the difference neglecting state. In this case, the union is relied upon so that the orientation is estimated almost exclusively based on the union.
Therefore, in the state of neglecting the difference, the azimuth gain GP is DGP.
The third gain value GP3, which is smaller than the first gain value GP1 and is close to almost zero, and is therefore a value close to almost zero so that the absolute position P detected by the S receiver 2 has little influence on the position estimation value P0.

The position adjuster 40 switches between a dead reckoning state that satisfies the rules 1 and 5, and a correctable state in which the position gain GP is determined based on the position reliability determination value JP, and in the correctable state, Azimuth adjuster 17 shown in FIG.
The same transition as the transition of.

DGP, a non-integral type sensor for position
Since the S receiver 2 has no accumulated error, the absolute position can be detected with high accuracy when there is no disturbance factor, but if there is a disturbance factor, the S receiver 2 is affected and detected. The absolute position contains a large error. In addition, when the velocity sensor 3, the geomagnetic sensor 4, and the gyro 5 which are integral type sensors with respect to the position are not affected by the reflected waves and the like unlike the DGPS receiver 2, there are no disturbance factors due to accumulated errors or the like. The accuracy is lower than that of the DGPS receiver 2.

Based on this point, as described above, the reliability of each position-related information acquisition means, which is the DGPS receiver 2, the speedometer 3, the geomagnetic sensor 4, and the gyro 5, specifically, the combination and the DGPS receiver. It is determined which of the two is more reliable, the azimuth estimation value θ0 by the combination and the speedometer 3
The position estimation unit 11 moves the moving body 20 so that the detected absolute position P has a higher reliability.
Is estimated. Further, since the reliability is determined based on the acquired position-related information, it is possible to determine which of the position-related information units 2 to 5 is highly reliable among the position-related information units 2 to 5 in the environment where the position is to be estimated. The azimuth is estimated so as to depend on the highly reliable position-related information means 4 and 5. Therefore, it is possible to always estimate the azimuth with high accuracy while determining the abnormality in real time.

Further, since the reliability of each of the position-related information means 2 to 5 is judged based on the position judgment value JP, it is possible to eliminate the influence of the speed of the moving body 20 and to make a stable judgment of reliability. .

FIG. 8 is a graph showing the direction estimation result by the position estimation device 1. Further, FIG. 9 is a graph showing the section S9 of FIG. 8 in an enlarged manner. 8 and 9
The horizontal axis represents time and the vertical axis represents azimuth. FIG. 8 and FIG. 9 show the azimuths when the moving body makes four rounds of a rectangular circulating path. As is clear from FIG. 8, the broken line 51
In the azimuth indicated by the gyro 5, the error accumulates each time the orbit is repeated, and as is clear from FIG. 9, the detected azimuth of the geomagnetic sensor 4 indicated by the solid line 50 includes an error in the place where the disturbance factor is present. I'm out. On the other hand, the position estimation device 1 indicated by the alternate long and short dash line 52, specifically the direction estimation value θ0 by the direction estimation device 7 included therein, has no accumulated error even when the orbit is repeated, and influences the disturbance factor. It can be confirmed that the accuracy is high.

FIG. 10 is a graph showing the position estimation result by the position estimating device 1, where the horizontal axis is the position E in the predetermined first direction, and the vertical axis is the second direction perpendicular to the first direction. Position F. 11 is a graph showing the position estimation result of FIG. 10 converted into the traveling direction and the lateral direction, where the horizontal axis is the position K along the route (traveling direction), and the vertical axis is the position L along the horizontal direction. Is. FIG. 12 is a graph showing the detected azimuth θ by the geomagnetic sensor 4 in the position estimation of FIG. 10, in which the horizontal axis represents time and the vertical axis represents azimuth. FIG. 13 is a graph showing a difference between the absolute position P detected by the DGPS receiver 2 and the position estimation value P0 in the position estimation of FIG. 10, in which the horizontal axis represents time and the vertical axis represents the position difference. 14
Is a graph showing the detected latitude x which is the latitude component of the absolute position P detected by the DGPS receiver in the position estimation of FIG. 10 and the latitude component x0 of the position estimation value P0, where the horizontal axis is the time and the vertical axis is Is the position xm along the latitude line. Here, the positions in FIGS. 10 to 14 are shown in terms of the distance from a predetermined position, not the longitude and the latitude.

In FIGS. 10 to 14, the moving body travels toward the south along a straight moving path extending substantially north and south along the building 60 at a predetermined constant speed, specifically about 5 km / h. It is the result of position estimation when moving. A GPS signal cutoff region 61 and a magnetic field abnormal region 62 due to a disturbance factor exist along the movement route.

As shown by the solid line 65 in FIGS. 10 and 11, and the longitude line component by the solid line 66 and the latitude component by the alternate long and short dash line 67 in FIG. 13, the detected position by the DGPS receiver 2 is It is confirmed that the blocking area 61 is affected. Further, as indicated by the solid line 70 in FIG. 12, the geomagnetic sensor 4 is affected by the magnetic field abnormal region 62,
Further, the error accumulates, and the broken line 70 in FIGS.
As shown by, it is confirmed that the position based only on dead reckoning moves away from the route.

Thus, even if the position by the DGPS receiver 2 and the position by dead reckoning by the combination are different from the actual position, the position estimation indicated by the alternate long and short dashed line 67 as indicated by the alternate long and short dashed line in FIGS. 10 and 11. It can be confirmed that the position estimation value by the device 1 is highly accurate without being affected by the disturbance factor. Also from FIG. 14, the position estimation value by the position estimation device 1 shown by the solid line 77 is not affected by the position by the GDPS receiver 2 shown by the broken line 76, and particularly the influence immediately after the re-acquisition of the DGPS signal is shown. It is confirmed to be highly accurate without receiving.

The above-described embodiment is merely an example of the present invention, and the configuration can be changed. For example, in estimating the azimuth, instead of the geomagnetic sensor 4, another sensor that can obtain the absolute azimuth may be used.
The direction detected by the receiver 2 may be used. To give a specific example, as shown in FIG. 15A, the two passing points Pa and Pb positions (Xa, Ya; Xb, Y) are passed.
The direction may be calculated from b), and FIG.
As shown in FIG. 5 (2), the mobile body 20 may be provided with two GPS antennas 90 and 91, and the azimuth may be calculated from the data received by the GPS antennas 90 and 91. Further, as means for obtaining the speed of the moving body 20, instead of the speedometer 3 as described above, the acceleration of the moving body 20 is detected by an acceleration detector realized by, for example, an inertial sensor, and this acceleration is integrated to calculate the speed. You may make it provide the means to request | require. Further, the position detecting means may use a GPS receiver of another system instead of the DGPS receiver 2. Further, it is not necessary to estimate the absolute azimuth and the absolute position, and the relative azimuth and the relative position may be estimated. Although the azimuth estimation apparatus 7 has been described above as including the position / azimuth calculation section 6, the azimuth estimation apparatus 7 only needs to include at least the azimuth estimation section 10.

Further, the moving body may be other than those exemplified above, and for example, the moving body moving two-dimensionally may be a ship. Further, the moving body is not necessarily a two-dimensional moving body, but may be a three-dimensional moving body such as an airship.

[0088]

According to the present invention as set forth in claim 1, in an environment where an azimuth is to be estimated, a highly reliable azimuth related information means of each azimuth related information means is determined, and a highly reliable azimuth is obtained. Since the azimuth is estimated depending on the related information means, it is possible to always estimate the position with high accuracy while determining the abnormality in real time. Further, in determining the reliability of each azimuth-related information means, it is possible to eliminate the influence of the speed of the moving body and to perform stable reliability determination. Therefore, the direction can be estimated with higher accuracy without being affected by the speed of the moving body. Furthermore, by determining the reliability, classifying into three states, and adjusting to a gain suitable for each state, it is possible to estimate the azimuth with high accuracy.

According to the present invention as set forth in claim 2, in the environment where the azimuth is to be estimated, the highly reliable azimuth related information means of the respective azimuth related information means is determined, and the highly reliable azimuth related information is obtained. Since the azimuth is estimated depending on the means, the position can always be estimated with high accuracy while determining the abnormality in real time. Further, in determining the reliability of each azimuth-related information means, it is possible to eliminate the influence of the speed of the moving body and to perform stable reliability determination. Therefore, the direction can be estimated with higher accuracy without being affected by the speed of the moving body. Furthermore, by determining the reliability, classifying into three states, and adjusting to a gain suitable for each state, it is possible to estimate the azimuth with high accuracy.

According to the third aspect of the present invention, in the environment where the position is to be estimated, the highly reliable position-related information means of each position-related information means is determined, and the highly reliable position-related information is determined. Since the position is estimated depending on the means, it is possible to always estimate the position with high accuracy while determining the abnormality in real time. Further, in determining the reliability of each position-related information means, it is possible to eliminate the influence of the speed of the moving body and to perform stable reliability determination. Therefore, the position can be estimated with higher accuracy without being affected by the speed of the moving body. Further, by determining the reliability, classifying into three states, and adjusting to a gain suitable for each state, the position can be estimated with high accuracy.

According to the present invention described in claim 4, in the environment where the position is to be estimated, the highly reliable position-related information means of each position-related information means is determined, and the highly reliable position-related information is determined. Since the position is estimated depending on the means, it is possible to always estimate the position with high accuracy while determining the abnormality in real time. Further, in determining the reliability of each position-related information means, it is possible to eliminate the influence of the speed of the moving body and to perform stable reliability determination. Therefore, the position can be estimated with higher accuracy without being affected by the speed of the moving body. Further, by determining the reliability, classifying into three states, and adjusting to a gain suitable for each state, the position can be estimated with high accuracy.

[Brief description of drawings]

FIG. 1 is a block diagram showing a schematic configuration of a moving body position estimation device 1 according to an embodiment of the present invention.

FIG. 2 is a block diagram showing a specific configuration of a position estimation device 1.

FIG. 3 is a diagram for explaining the influence of speed in the reliability determination of each azimuth related information means 4 and 5.

FIG. 4 is a graph showing an azimuth gain Gθ.

FIG. 5 is a diagram showing a state transition of the azimuth adjuster 17.

FIG. 6 is a diagram for explaining the determination according to rule 5;

FIG. 7 is a diagram showing a difference in property between an integrating sensor and a non-integrating sensor.

FIG. 8 is a graph showing an estimation result of an azimuth by the position estimation device 1.

9 is an enlarged graph showing a section S9 of FIG.

FIG. 10 is a graph showing a position estimation result by the position estimation device 1.

11 is a graph showing the position estimation result of FIG. 10 converted into a traveling direction and a lateral direction.

FIG. 12 is a geomagnetic sensor 4 for position estimation in FIG.
7 is a graph showing a detection azimuth θ according to FIG.

13 is a graph showing the difference between the absolute position P detected by the DGPS receiver 2 and the position estimation value P0 in the position estimation of FIG.

14 is a graph showing a longitude component x of a position P and a longitude component x0 of a position estimated value P0 by the DGPS receiver in the position estimation of FIG.

FIG. 15 is a diagram showing another means for detecting a direction that can be used in another embodiment of the present invention.

[Explanation of symbols]

1 Position estimation device 2D GPS receiver 3 speedometer 4 Geomagnetic sensor 5 gyro 6 Position / azimuth calculator 7 Direction estimation device 10 Direction estimation unit 11 Position estimation unit 13 Azimuth angular velocity subtractor 14 Azimuth and angular velocity integrator 15 Direction subtractor 16 Directional reliability judgment device 17 Direction adjuster 20 moving bodies 35 Velocity component calculator 36 speed subtractor 37 Velocity integrator 38 Position subtractor 39 Position reliability determiner 40 position adjuster

─────────────────────────────────────────────────── --- Continuation of the front page (56) Reference JP-A-3-221813 (JP, A) JP-A-60-122311 (JP, A) JP-A-5-34157 (JP, A) JP-A-6- 288776 (JP, A) JP-A-5-79849 (JP, A) JP-A-8-334348 (JP, A) (58) Fields investigated (Int.Cl. ⁷ , DB name) G01C 17/00-17 / 38 G01C 19/00-19/72 G01S 5/00-5/14

Claims (4)

(57) [Claims] 1. A first azimuth related information means for detecting (a) an azimuth angular velocity which is a temporal change rate of the azimuth of a moving body, and (b) a second azimuth related information means for detecting an absolute azimuth of the moving body. (C) The detected azimuth angular velocity detected by the first azimuth related information means is corrected by the azimuth correction value based on the detected absolute azimuth detected by the second azimuth related information means to obtain the calculated azimuth angular velocity. , Which is an azimuth estimation calculation unit that integrates the calculated azimuth angular velocity to obtain an azimuth estimated value representing the absolute azimuth of the moving body, and divides the azimuth difference, which is the difference between the azimuth estimated value and the detected absolute azimuth, by the speed of the moving body. To determine the azimuth determination value, determine whether the azimuth determination value is in the range of small values, the range of large values, or the range of intermediate values between these ranges. For the range of small judgment values In some cases, the azimuth correction value is decreased, when the azimuth judgment value is in an intermediate value range, the azimuth correction value is increased, and when the azimuth judgment value is in a large value range, the azimuth correction is performed. An azimuth estimating device for a mobile body, comprising: an azimuth estimating calculation means for making the value smaller than the azimuth correction value when the azimuth judgment value is in a small range. 2. A first azimuth related information means for detecting (a) an azimuth angular velocity which is a temporal change rate of the azimuth of a moving body, and (b) a second azimuth related information means for detecting an absolute azimuth of the moving body. And (c) azimuth estimation calculation means for obtaining a azimuth estimation value representing the absolute azimuth of the moving body, and (c1) subtracting the azimuth correction value from the detected azimuth angular velocity detected by the first azimuth related information means. An azimuth angular velocity subtraction unit that calculates the calculated azimuth angular velocity, (c2) an integration unit that integrates the calculated azimuth angular velocity calculated by the azimuth angular velocity subtraction unit to obtain a azimuth estimated value, and (c3) azimuth estimation calculated by the integration unit. Value and second
Based on the azimuth difference and the azimuth gain obtained by the azimuth subtraction unit (c3), the azimuth angular velocity calculation unit is provided to the azimuth angular velocity calculation unit. An azimuth adjustment unit that obtains a given azimuth correction value, and (c4) obtains an azimuth determination value by dividing the azimuth difference obtained by the azimuth subtraction unit by the speed of the moving body, and the azimuth determination value is 0 or more and is determined in advance. When it is less than the first threshold value, it is determined that the reliability of both the first and second azimuth related information acquisition means is high, and the detected azimuth angular velocity and the detected absolute azimuth both influence the estimated azimuth value and the detected azimuth. The first gain value that the angular velocity has a greater effect on the estimated bearing value than the detected absolute bearing is set as the bearing gain given to the bearing adjustment unit, and the bearing determination value is equal to or greater than the first threshold value and equal to or greater than the first threshold value. If it is less than the second threshold, which is larger than the first threshold, it is determined that the reliability of the second azimuth related information means is higher than that of the first azimuth related information means, and the detected azimuth angular velocity and the detected absolute azimuth both influence the estimated azimuth value. And the detected absolute azimuth is larger than the first gain value that has a greater influence on the azimuth estimated value than the detected azimuth angular velocity.
The gain value is set as the azimuth gain given to the azimuth adjusting unit, and when the azimuth judgment value is equal to or greater than the second threshold value, the reliability of the first azimuth related information unit is higher than that of the second azimuth related information unit. The azimuth to be set as the azimuth gain given to the azimuth adjusting unit so that the detected absolute azimuth has no or little influence on the azimuth estimated value. An azimuth estimation apparatus for a mobile body, comprising: an azimuth estimation calculation section having a reliability determination section. 3. A first position-related information means for detecting the speed of the moving body, a second position-related information means for detecting the absolute position of the moving body, and a first position-related information means. The detected position detected by the position-related information means is corrected by a position correction value based on the detected absolute position detected by the second position-related information means to obtain a calculated speed, and the calculated speed is integrated to obtain a moving body. Position estimation calculation means for obtaining a position estimation value representing the absolute position of the position estimation value by dividing the position difference, which is the difference between the position estimation value and the detected absolute position, by the speed of the moving body. If the position determination value is in the small value range, it is determined whether the position determination value is in the small value range, the large value range, or an intermediate value range between these ranges. , Decrease the position correction value and change the position judgment value Is in an intermediate value range, the position correction value is increased, and the position determination value is in a large value range, the position correction value is in a small value range. A position estimation device for a mobile body, comprising: a position estimation calculation means for making the value smaller than the position correction value. 4. (a) first position-related information means for detecting the speed of the moving body, (b) second position-related information means for detecting the absolute position of the moving body, and (c) of the moving body. A position estimation calculation means for obtaining a position estimation value representing an absolute position, comprising: (c1) a speed subtraction section for obtaining a calculation speed by subtracting a position correction value from the detected speed detected by the first position-related information means. And (c2) an integrating unit that obtains a position estimated value by integrating the calculated velocity obtained by the velocity subtracting unit, and (c3) a position estimated value obtained by the integrating unit;
A position subtraction unit for obtaining a position difference which is a difference from the detected absolute position detected by the position-related information means, and (c3) a position difference given to the speed calculation unit based on the position difference and the position gain obtained by the position subtraction unit. A position adjustment unit that obtains a position correction value that is obtained, and (c4) obtain a position determination value by dividing the position difference obtained by the position subtraction unit by the speed of the moving body. When it is less than 1 threshold value, it is determined that the reliability of the first and second position-related information acquisition means is both high, and the detected speed and the detected absolute position both influence the position estimation value, and the detected speed is detected. The first gain value, which has a greater effect on the position estimation value than the absolute position, is set as the position gain given to the position adjusting unit, and the second judgment value for position is greater than or equal to the first threshold value and greater than the first threshold value. Kii When it is less than the value, it is determined that the reliability of the second position-related information means is higher than that of the first position-related information means, and the detected speed and the detected absolute position both influence the position estimated value, and the detected absolute position is The second gain value, which is larger than the first gain value and has a greater effect on the position estimation value than the detected speed,
It is set as the position gain given to the position adjusting unit, and when the position determination value is equal to or higher than the second threshold value, it is determined that the reliability of the first position related information means is higher than that of the second position related information means. A position reliability determination unit that sets a gain value smaller than a first gain value and equal to or less than a third gain value as a position gain given to the position adjustment unit so that the detected absolute position has little or no influence on the position estimated value. And a position estimation calculation unit having: