JPH0814923A - On-vehicle positioning apparatus - Google Patents

Abstract

PURPOSE:To make the positioning accuracy of a vehicle position more precise. CONSTITUTION:A relative sensor which measures the relative position of a vehicle to the surface of the ground, a GPS which detects the relative position of a plurality of satellites to the vehicle on the basis of signals sent by the respective satellites and steps 31 to 35, 42 to 53 which can measure a position on the basis of information from the plurality of satellites detected by the GPS, which measure the relative position of the vehicle only on the basis of the output of the GPS when the estimated error of a positioning operation is smaller than a prescribed level and which measure the absolute position of the vehicle only on the basis of the output of the GPS and the output of the relative sensor or the output of the relative sensor in cases other than it are installed in an on-vehicle positioning apparatus.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an on-vehicle positioning device which measures the position of a vehicle such as an automobile and informs the current position information.

[0002]

2. Description of the Related Art For example, as disclosed in JP-A-61-137009 and JP-A-61-167886, a global positioning system (Grob
2. Description of the Related Art Conventionally, a technology for an automobile positioning device using an al Positioning System (hereinafter referred to as GPS) has been proposed.
GPS comprises multiple satellites. The satellite transmits data indicating accurate time, orbital function, and accuracy of information on radio waves to the ground at a predetermined timing. On the ground, the position of the satellite can be known from the time obtained from the satellite and the function of the orbit, and the difference between the time of the satellite and the time of the receiving point, that is, the propagation delay time of the radio wave You can know the distance. For each of the three different satellites, if the position of the satellite and the distance from the satellite to the receiving point are known, the three-dimensional simultaneous equations can be solved to obtain the three-dimensional position of the receiving point, which is an unknown number. However, since the time at the receiving point usually includes a relatively large error, the time at the receiving point needs to be set at 4
Information on one satellite is required.

Further, assuming that the altitude of the receiving point hardly changes, the two-dimensional position of the receiving point on the ground surface can be obtained from the information of the three satellites.

However, the number of GPS satellites that actually exist is limited, and radio waves are blocked in an automobile, for example, in the tunnel or in the shadow of a building, and three or four GPS satellites are simultaneously transmitted.
It may be difficult to receive radio waves from two satellites.
Therefore, in the technique disclosed in JP-A-61-137009,
Even when the direction change of the own vehicle is detected using the optical fiber gyro, the moving distance of the own vehicle is detected using the vehicle speed sensor, and only the radio wave from one satellite can be received by using the detected information as an auxiliary. The position of the own vehicle can be detected. Further, in the technique disclosed in Japanese Patent Laid-Open No. 61-167887, the position of the vehicle can be detected even if only the radio waves from two satellites can be received by using the magnetic azimuth meter and the distance meter mounted on the vehicle as an auxiliary. I have to.

[0005]

However, switching between positioning by GPS alone and positioning by another sensor is determined by the number of satellites that can be received. Therefore, even if there is less error in the positioning by the relative sensor that measures the relative position between the vehicle and the ground surface than in the positioning by the satellite, the positioning by the satellite is prioritized. It cannot be reduced.

Therefore, the first object of the present invention is to make the positioning accuracy more accurate.

A second problem is to prevent incorrect information from being given when positioning cannot be performed at all or when positioning accuracy is poor.

A third object is to prevent the positioning result from changing suddenly when the positioning method is switched.

[0009]

In order to solve the first problem, in the invention according to claim 1, a relative sensor for measuring a relative position between a vehicle and the ground surface; signals emitted by a plurality of satellites Satellite information detecting means for detecting the relative position of each satellite and vehicle from the satellite; positioning is possible from the information of a plurality of satellites detected by the satellite information detecting means, and the positioning estimation error is smaller than a predetermined level, the satellite Positioning the absolute position of the vehicle only from the output of the information detecting means, at other times, positioning the absolute position of the vehicle only from the output of the satellite information detecting means and the output of the relative sensor, or only the output of the relative sensor,
Positioning means;

In order to solve the first problem, according to the invention described in claim 2, a gyro for detecting a heading deviation of a vehicle; a distance sensor for detecting a traveling distance of the vehicle; and a signal emitted from a plurality of satellites, respectively. Relative speed detecting means for detecting the relative speed between the satellite and the vehicle; azimuth calculating means for obtaining the azimuth angle of the vehicle from the output of the gyro; integrated distance calculating means for obtaining the integrated distance of the vehicle from the output of the distance sensor; When the relative velocity detecting means detects the relative velocity with three or more satellites, and the positioning estimation error of the positioning using the relative velocity with the three or more satellites is smaller than a predetermined level, the relative velocity is three or more satellites. Positioning using the relative speed between
(2) The relative speed detecting means detects the relative speed between the two satellites, the azimuth estimation error of the azimuth calculated by the azimuth calculating means is smaller than a predetermined level, and the relative speed between the two satellites is small. When the positioning estimation error of the positioning using the velocity and the azimuth angle calculated by the azimuth angle detection means is smaller than a predetermined level,
Positioning using relative velocity and azimuth between two satellites,
(3) The azimuth estimation error of the azimuth angle calculated by the azimuth angle calculation means is smaller than a predetermined level, the integrated distance estimation error of the integrated distance calculated by the integrated distance calculation means is smaller than the predetermined level, and the azimuth angle detection means And a positioning unit that performs positioning using the azimuth angle and the integrated distance when the positioning estimation error of the positioning using the calculated azimuth angle and the integrated distance calculated by the integrated distance calculation unit is smaller than a predetermined level.

In order to solve the second problem in addition to the first problem, the invention according to claim 3 provides
When the positioning means of the invention of (1) does not correspond to any of (1), (2), and (3), positioning is not performed.

In order to solve the third problem in addition to the first problem, in the invention according to claim 4, a positioning solution of the invention according to claims 1 and 2 is filtered by a Kalman filter. I tried to apply.

[0013]

According to the means of claim 1, the relative sensor measures the relative position between the vehicle and the ground surface. The satellite information detecting means detects the relative position between each satellite and the vehicle from the signals emitted by the plurality of satellites. The positioning means can measure the absolute position of the vehicle only from the output of the satellite information detecting means when the positioning information can be measured from the information of the plurality of satellites detected by the satellite information detecting means and the estimation error of the positioning is smaller than a predetermined level. At other times, the absolute position of the vehicle is measured only from the output of the satellite information detecting means and the output of the relative sensor or only the output of the relative sensor. As a result, only the output of the satellite information detection means,
The absolute position of the vehicle can be determined only from the output of the satellite information detecting means and the output of the relative sensor, or the output of the relative sensor.

Here, even if the positioning means can perform positioning from the information of the plurality of satellites detected by the satellite information detecting means, when the positioning estimation error is smaller than a predetermined level, only the output of the satellite information detecting means is used. The absolute position of the vehicle is not measured, but the absolute position of the vehicle is measured only from the output of the satellite information detecting means and the output of the relative sensor or the output of the relative sensor.

Therefore, positioning with less error can be performed.

According to the second aspect of the invention, the gyro detects the heading deviation of the vehicle. The distance sensor detects the traveling distance of the vehicle. The relative velocity detecting means detects the relative velocity between each satellite and the vehicle from the signals emitted by the plurality of satellites. The azimuth calculation means obtains the azimuth of the vehicle from the output of the gyro. The integrated distance calculation means obtains the integrated distance of the vehicle from the output of the distance sensor. The positioning means selects any one of the following (1), (2), and (3) according to the outputs of the relative speed detection means, the azimuth angle calculation means, and the integrated distance calculation means.

(1) The relative velocity detecting means detects the relative velocity with three or more satellites, and the positioning estimation error of the positioning using the relative velocity with three or more satellites is smaller than a predetermined level. At this time, positioning is performed using the relative speed between three or more satellites.

(2) The relative speed detecting means detects the relative speed between the two satellites, the azimuth estimation error of the azimuth calculated by the azimuth calculating means is smaller than a predetermined level, and 2
When the positioning estimation error of the positioning using the relative speed between the satellite and the azimuth angle calculated by the azimuth detecting means is smaller than a predetermined level, the positioning using the relative speed between the two satellites and the azimuth angle is performed. To do.

(3) The azimuth estimation error of the azimuth angle calculated by the azimuth angle calculation means is smaller than a predetermined level, the integrated distance estimation error of the integrated distance calculated by the integrated distance calculation means is smaller than the predetermined level, and the azimuth angle is When the positioning estimation error in the positioning using the azimuth angle calculated by the detection means and the integrated distance calculated by the integrated distance calculation means is smaller than a predetermined level, positioning is performed using the azimuth angle and the integrated distance.

Therefore, the positioning estimation error of each positioning is obtained before each positioning, and if the positioning estimation error is large, the positioning is not performed, so that the positioning method with high accuracy is selected. Further, when the azimuth angle calculated by the azimuth angle calculation means is used for positioning, the azimuth estimation error is considered, and when the integrated distance calculated by the integrated distance calculation means is used for positioning, the integrated distance estimation error is considered. Therefore, positioning using inaccurate information is excluded, and as a result accurate positioning can be performed.

According to the means of claim 3, the positioning means does not perform positioning if none of the above (1), (2) and (3). Therefore, positioning is not performed when the positioning estimation error, the bearing estimation error, and the integrated distance estimation error are all bad.

According to the means of claim 4, in the positioning means, the Kalman filter is used to filter the positioning solution. Therefore, even if the positioning method changes while the vehicle is running or stopped, an abnormal change in the solution is suppressed.

[0023]

DESCRIPTION OF THE PREFERRED EMBODIMENTS The present invention will be described below with reference to the drawings. FIG. 1 shows an embodiment of a vehicle-mounted positioning device. This device is installed in an automobile and includes a receiving antenna 10, a GPS receiver 11, a GPS demodulator 12, a display device 13, a vehicle speed sensor 14, a piezoelectric vibration gyro 15, an altitude sensor 16, and an electronic control unit 17. Radio waves of 1.57542 GHz are transmitted from each GPS satellite. The GPS receiver 11 receives radio waves from each GPS satellite via the receiving antenna 10. The received signal is the GPS demodulator 12
Sent to The GPS demodulator 12 uses the information on the radio wave,
That is, the function indicating the orbit of the satellite, the time, and the code indicating the accuracy of the information are demodulated and output to the electronic control unit 17.
The vehicle speed sensor 14 outputs a pulse signal to the electronic control unit 17 according to the rotation of the wheel shaft of the vehicle. The vehicle speed sensor 14 is composed of, for example, a magnet provided around an output shaft of a vehicle transmission or an axis that rotates in synchronization with the output shaft, and a reed switch provided in the vicinity of the magnet. The reed switch may be turned on / off. Piezoelectric vibration gyro 1
5 is fixed to the automobile and outputs to the electronic control unit 17 an analog signal of a level proportional to the rotational angular velocity ω about the vertical axis of the automobile. The piezoelectric vibrating gyro 15 is capable of outputting a voltage of 0 to 5 Volts. A voltage of 2.5 Volts or more is required for clockwise rotation and 2.5 Volts or less for counterclockwise rotation with a center of 2.5 Volts. It is tuned to output a signal. The altitude sensor 16 outputs to the electronic control unit 17 an analog signal whose level changes according to a change in atmospheric pressure, that is, a change in altitude. The display device 13 displays the latitude, longitude, altitude, etc. of the automobile, and receives and displays information from the electronic control unit 17.

The electronic control unit 17 includes a microcomputer 19, an input / output interface (I / O) 18,
And an analog / digital converter (A / D converter) 20. The input / output interface (I / O) 18 is G
PS receiver 11, GPS demodulator 12, and vehicle speed sensor 1
The signal from the microcomputer 4 is sent to the microcomputer 19, and the information from the microcomputer 19 is displayed on the display device 1.
Send to 3. Analog / digital converter (A / D converter)
Reference numeral 20 A / D-converts the analog signal from the piezoelectric vibration gyro 15 and the altitude sensor 16 into a digital signal, and sends it to the microcomputer 19. The signal from the vehicle speed sensor 14 is connected to the external interrupt terminal of the microcomputer 19.

The microcomputer 19 operates according to the flow chart of FIG. Microcomputer 19
When the power is turned on and starts, first, in step 30, initialization is performed. Here, I / O port settings,
Memory initialization and interrupt settings are performed. As the interrupt, a timer interrupt that is executed at regular time intervals and an external interrupt that is executed according to the level change of the external interrupt terminal are set. After that, the processing of the main routine is temporarily interrupted at regular intervals, and the gyro direction calculation routine of step 54 is executed. In this embodiment, an interrupt is made every 10 mS. Also, each time the level of the external interrupt terminal changes, the processing of the main routine is temporarily suspended and the integrated distance calculation routine of step 55 is executed. When the initialization step ends, it is determined in step 31 whether demodulation of three satellites or more has been completed. If the number of satellites that have been demodulated is 3 or more, the navigation equation is set in the navigation equation 1 routine in step 32, and the positioning estimation error is calculated in the positioning estimation error calculation routine in step 33. Next, in step 34, it is determined whether the positioning estimation error is smaller than a preset upper limit (for example, 300 m). If the positioning estimation error is smaller than the upper limit, step 35
Positioning is performed by the positioning routine of step 3, the altitude is corrected by the altitude correction routine of step 36, and further, step 37 is performed.
The gyro and the vehicle speed are corrected by the gyro correction routine. Then, in step 38, the current position information (latitude, longitude, altitude, etc.) is output to the display device 13. next,
In step 39, it is determined whether the gyro direction can be displayed. If the gyro direction can be displayed, step 40
The gyro direction is output to the display device 13. After that, the gyro is corrected in the gyro zero-point correction routine in step 41, and the process returns to step 31.

If the number of satellites that can be demodulated is 2 or less in step 31, or if the positioning estimation error is larger than the upper limit in step 34, it is determined in step 42 whether or not demodulation of 2 or more satellites has been completed. . Here, if the number of satellites that have been demodulated is two, it is determined in step 43 whether the azimuth estimation error is smaller than a preset upper limit (for example, 30 °). As will be described later, the azimuth estimation error is calculated in the gyro azimuth calculation routine of the timer interruption. If this bearing estimation error is smaller than the upper limit, the navigation equation is set in the navigation equation 2 routine in step 44, and step 45
The positioning estimation error is calculated by the positioning estimation error calculation routine of. Next, in step 46, it is determined whether or not the positioning estimation error is smaller than a preset upper limit (for example, 300 m).

If the positioning estimation error is smaller than the upper limit, positioning is performed by the positioning routine of step 47, and step 36
The altitude is corrected by the altitude correction routine of. Then, jump to step 38.

In step 42, if the number of satellites that can be demodulated is 1 or less, or in step 43, the direction estimation error is larger than the upper limit, or in step 46, the positioning estimation error is larger than the upper limit, In step 48, it is determined whether the azimuth estimation error is smaller than a preset upper limit (for example, 30 °). When the azimuth estimation error is smaller than the upper limit, the integrated distance estimation error STEPerror is calculated in step 49. The accumulated distance estimation error STEPerror is the number of pulses within a fixed time taken in by the accumulated distance calculation routine described later, and the rotation error ROLLerro obtained by the numerical correction routine described later.
Multiply by r to find. In step 50, the accumulated distance estimation error STEPerror is set to a preset upper limit (eg 10
It is determined whether or not it is smaller than 0 m). If the accumulated distance estimation error STEPerror is smaller than the upper limit, set the navigation equation in the navigation equation 3 routine in step 51,
The positioning estimation error is calculated in the positioning estimation error calculation routine in step 52. Next, in step 53, it is determined whether the positioning estimation error is smaller than a preset upper limit (for example, 300 m). If the positioning estimation error is smaller than the upper limit, the process jumps to the positioning routine of step 47.

If the bearing estimation error is greater than the upper limit in step 48, or if the integrated distance estimation error is greater than the upper limit in step 50, or if the positioning estimation error is greater than the upper limit in step 53, Jump to step 41.

The details of each subroutine will be described below.

FIG. 3 is a flowchart of the gyro direction calculation routine. Here, first, in step 56, the azimuth deviation Δθ is obtained according to the following equation.

[0032]

[Mathematical formula-see original document] [Delta] [theta] = [Delta] [theta] + ADA (ADG-ADC) where ADG is a value obtained by digitally converting the analog output of the piezoelectric vibration gyro 15, and ADA and ADC are conversion coefficients. The coefficient ADC is set to a value obtained by digitally converting the analog voltage value (2.5 volts) when the rotation angular velocity ω is zero.

The coefficient ADA is a value set so that a value obtained by digitally converting the analog output of the piezoelectric vibration gyro 15 is further converted into an angle. Since the calculation of Formula 1 is performed at each timer interrupt cycle, Δθ represents the amount of azimuth deviation per unit time. Next, in step 57, it is judged whether or not the vehicle speed is zero, and if the vehicle speed is zero, step 58
In step 59, the gyro heading error turn is updated based on the equation 2, and in step 59, the gyro offset value offset is updated based on the equation 3. If the vehicle speed is not zero in step 57, steps 58 and 59 are skipped and the process proceeds to the next step.

[0034]

[Equation 2] turn = turn + ADA error (ADG-ADC)

[0035]

## EQU00003 ## offset = offset + ADCerror Here, ADAerror is a conversion coefficient that represents the deviation of the inclination of the output of the piezoelectric vibration gyro, and ADCerror is a conversion coefficient that represents the deviation of the piezoelectric vibration gyro from the zero point.

Next, at step 60, it is judged whether or not the reference azimuth is valid. If the reference azimuth is valid, the process proceeds to step 62. If the reference azimuth is not valid, the azimuth estimation error ψ error is set to a value equal to or higher than the display error upper limit.

Next, at step 64, the digital output value A is added to the interrupt output value sum ADS1 of the piezoelectric vibration gyro 15.
DG is added to update the interrupt output value sum ADS1. Next, in step 65, it is determined whether it is the positioning solution output timing. This timing is set to 1 second in this embodiment. If it is the positioning solution output timing, step 62 and subsequent steps are executed, and if it is not the positioning solution output timing, the gyro direction calculation routine is ended. At step 62, the azimuth deviation Δθ is added to the interrupt azimuth ψ1 to update the interrupt azimuth ψ1. Next, at step 63, the azimuth deviation Δθ is added to the interruption azimuth ψ1raw, and the interruption azimuth ψ1raw is updated. Next, at step 66, an orientation estimation error calculation routine is executed to obtain a standard error. Next, at step 67, a correction amount is added to the interrupt direction ψ1. Next, at step 68, the interrupt azimuth ψ1 is substituted for the azimuth ψ, the interrupt azimuth ψ1raw is substituted for the azimuth ψraw, and the interrupt output value sum ADS1 is substituted for the output value sum ADS. Next, step 6
At 9, the delay data of the output of the altitude sensor 16 is updated.
The delay data of the output of the altitude sensor 16 includes an altitude H [-3] of 3 seconds before, an altitude H [-2] of 2 seconds before, and an altitude H of 1 second before.
[-1], current altitude H

It has four output data of [0]. Here, the altitude H [-2] 2 seconds ago, the altitude H [-3] 3 seconds ago, the altitude H [-1] 1 second before the altitude H [-2] 2 seconds ago, Altitude H

[0] is set to the altitude H [-1] one second before, and the output of the altitude sensor 16 is set to the current altitude H [-1].

Update to [0] respectively. Next, step 7
At 0, the delay data of the output of the piezoelectric vibration gyro 15 is updated. The delay data of the direction of the piezoelectric vibration gyro 15 includes
Azimuth ψ [-3] 3 seconds ago, azimuth ψ [-2] 2 seconds ago, 1
Azimuth ψ [-1] before second, current azimuth ψ

It has four output data of [0]. Here, the azimuth ψ two seconds ago
[-2] to azimuth ψ [-3] 3 seconds ago azimuth ψ 1 second ago
[-1] to the azimuth ψ [-2] 2 seconds before, and the current azimuth ψ

[0] is the azimuth ψ [-1] one second before, and the azimuth ψ obtained in step 63 is the current azimuth ψ.

Update to [0] respectively. Next, at step 71, the delay data of the number of pulses of the vehicle speed sensor 14 within a certain period of time is updated.

The pulse number delay data includes the pulse number P [-3] three seconds before, the pulse number P [-2] two seconds before, the pulse number P [-1] one second before, and the current pulse. Number P

4 of [0]
It has one output data. Here, the pulse number P [-2] two seconds ago is the pulse number P [-3] three seconds ago, and the pulse number P [-1] one second ago is the pulse number P [-2] two seconds ago. ]
And the current pulse count P

[0] is the number of pulses P one second ago
In [-1], the output of the altitude sensor 16 is set to the current pulse number P

Update to [0] respectively. When step 66 ends, the gyro direction calculation routine ends.

FIG. 4 is a flowchart of the orientation estimation error calculation routine. Here, first, in step 72,
It is determined whether the flag sign is 1, and if the flag sign is 1, step 73 is executed, and if the flag sign is not 1, step 74 is executed. This flag sign indicates whether the vehicle is rotating to the right or to the left, and the initial value is set to 1. In step 73, the gyro heading error turn is added to the heading deviation RL. In step 74, the gyro heading error turn is subtracted from the heading deviation RL. Next, at step 75, it is judged whether the bearing deviation RL is higher than zero,
If it is less than zero, the sign of the azimuth deviation RL is inverted in step 76, and the state of the flag sign is changed in step 77 (0 if 1 and 0 if 1). Next, in step 78, it is determined whether or not the azimuth absolute value is corrected. If the azimuth absolute value is corrected, the GPS estimation error GPSerror is substituted for the offset value offset in step 79, and the rotation is performed in step 80. The direction signal RL is set to zero.
Then, in step 81, the bearing deviation estimation error Δθ error
Is calculated according to the equation (4), and the azimuth estimation error calculation routine ends. By this processing, the azimuth estimation error Δθ error considering the offset error amount is obtained.

[0040]

## EQU4 ## Δθ error = RL + offset FIG. 5 is a flowchart of the integrated distance calculation routine.
Here, first, at step 82, the number of pulses within a certain period of time is fetched. Next, in step 83, the variable ROLL is multiplied by the number of pulses in a certain period of time to obtain the integrated distance STEP. The variable ROLL is a variable obtained by considering the effective radius of the tire and the output characteristics of the vehicle speed sensor 14, and is for converting the number of pulses of the vehicle speed sensor 14 into a distance. Step 83
When is finished, the integrated distance calculation routine is finished.

FIG. 6 is a flowchart of the navigation equation 1 routine. Here, first, in step 84, it is determined whether the demodulation of four or more satellites has been completed. If four or more satellites have been demodulated, equation 1 in Table 1 is set in step 86, and the σ _m ² calculation routine is executed in step 89 to calculate σ _m ² (where σ _m ² is the prediction error of each satellite). are shown. _m represents the number of satellites demodulated finished, each sigma ₁ ² with respect to 4-th ^{_{satellite, σ 2 2, σ 3 2}} , σ 4 2 is calculated)
Then, in step 93, the matrix Σε _{i + 1} ( _i indicates the number of positionings up to the previous time) indicating the prediction error of the measured value according to the determinant 1 in Table 2 in step 93. _{i + 1} is the current positioning. .) Is defined and this routine ends. Here, the navigation equation is expressed in the form of AX = L. A is L _m , M _m , N _m (
_m is the number of satellites that have been demodulated and has _m = 1 to 4) 4x
4 is a matrix, X is a [Δx Δy Δz cΔt] ^T , and L is a 1 × 4 matrix.

[0042]

[Table 1]

As shown in FIG. 27, (L _m , M _m ,
N _m ) indicates a vector from the m-th satellite to the previous position (positioning point at the time of last positioning). This vector is derived from information from the satellite. (Δx, Δy, Δz) indicates a vector from the previous position to the positioning point. Δt indicates the time difference between the time at the previous position and the time when the positioning point is reached. c indicates the speed of light. Here, since demodulation has been completed for four or more satellites, the matrix A is represented by using the elements of the vectors from the four satellites with _m = 1 to 4 to the previous position.
The matrix L is represented by [ΔL1 ΔL2 ΔL3 ΔL4] ^T. The ΔL1, ΔL2, ΔL3, and ΔL4 are 1 from the distance L _m ( _m = 1 to 4) between the first to fourth satellites and the positioning point, respectively.
The distance L _0m between the fourth satellite and the previous position is subtracted. By solving this navigation equation 1, the three-dimensional relative position (Δx, Δ
y, Δz) can be obtained.

In FIG. 6, when the number of satellites demodulated in step 84 is 3, step 85 is executed.
In step 85, it is determined whether there is altitude data. If there is altitude data, the equation 2 in Table 1 is calculated in step 87.
Is set, the σ _m ² calculation routine is executed in step 90 to obtain σ ₁ ² , σ ₂ ² and σ ₃ ² for the _three satellites, and in step 92 the σ _a ² calculation routine is executed to calculate σ _a ^2.
After obtaining _a ² (σ _a ² indicates the prediction error with respect to altitude), in step 94, the matrix Σε _{i + 1} indicating the prediction error of the measured value is determined according to the determinant 2 in Table 2, and this routine is determined. To finish.

[0045]

[Table 2]

Here, the navigation equation 2 is expressed in the form of AX = L, like the navigation equation 1. Here, since the demodulation of three satellites has been completed, the matrix A is represented using the elements of the vectors from the three satellites of _m = 1 to 3 toward the previous position. Also, the matrix L is [ΔL1 ΔL2 ΔL3 Δ
h] is represented by ^T. This Δh indicates the difference between the altitude of the positioning point obtained by the altitude sensor 16 and the altitude of the previous position. By solving this navigation equation 2, the three-dimensional relative position (Δx, Δy, Δz) can be obtained from the information obtained from the three satellites and the altitude difference Δh obtained from the altitude sensor.

If there is no altitude data in step 85, set equation 3 in Table 1 in step 88,
In step 91, the σ _m ² calculation routine is executed to obtain σ ₁ ² , σ ₂ ² , and σ ₃ ² for _three satellites, and then in step 95, the prediction error of the measurement value is calculated according to the determinant 3 in Table 2. Then, the matrix Σε _{i + 1} is defined, and this routine ends. Here, the navigation equation 3 is, like the navigation equation 1,
It is expressed in the form of AX = L. Here, since the demodulation of three satellites has been completed, the matrix A is represented using the elements of the vectors from the three satellites of _m = 1 to 3 toward the previous position. The matrix L is represented by [ΔL1 ΔL2 ΔL3 0] ^T. The difference Δz in the height direction is assumed to be zero. By solving the navigation equation 3, the two-dimensional relative position (Δx, Δy) can be obtained from the information obtained from the three satellites.

FIG. 7 is a flowchart of the navigation equation 2 routine. Here, first, in step 96, it is determined whether or not there is altitude data. If there is altitude data, set equation 4 in Table 1 in step 97, and step 9
In step 9, the σ _m ² calculation routine is executed to find σ ₁ ² and σ ₂ ² for the ^two satellites, in step 101, the σ _a ² calculation routine is executed to find σ _a ² , and in step 102 σ _After executing the _G ² calculation routine and obtaining σ _G ² (σ _G ² indicates the prediction error of the output of the piezoelectric vibration gyro),
In step 104, the matrix Σε _{i + 1} indicating the prediction error of the measured value is determined according to the determinant 4 in Table 2, and this routine is ended. Here, since two satellites have been demodulated,
_The matrix A is represented using the elements of the vector from two satellites of _m = 1 to 2 to the previous position, and Gx and Gy. There is the following relationship between Δx and Δy and Gx and Gy.

[0049]

Δy / Δx = sin (180 ° −ψ) / cos (180 ° −ψ) = Gx / Gy Further, the matrix L is represented by [ΔL1 ΔL2 Δh 0] ^T. By solving the navigation equation 4 using the equation 3,
The three-dimensional relative position (Δx, Δy, Δz) can be obtained from the information obtained from the two satellites, the azimuth ψ obtained from the piezoelectric vibration gyro, and the altitude difference Δh obtained from the altitude sensor.

If there is no altitude data in step 96,
In step 98, equation 5 in Table 1 is set, in step 100, the σ _m ² calculation routine is executed to obtain σ ₁ ² and σ ₂ ² for ^two satellites, and in step 103, σ
_{After the G} ² calculation routine is executed to obtain σ _G ² , the matrix Σε _{i + 1} indicating the prediction error of the measurement value is determined according to the determinant 5 in Table 2 in step 105, and this routine is ended. Here, since two satellites have been demodulated, _m = 1
The matrix A is represented using Gx and Gy, and the elements of the vector from the two satellites of ~ 2 to the previous position. The matrix L is represented by [ΔL1 ΔL2 0 0] ^T. The difference Δz in the height direction is assumed to be zero. The two-dimensional relative position (Δx, Δy) can be obtained from the information obtained from the two satellites and the azimuth ψ obtained from the piezoelectric vibration gyro by solving the navigation equation 5 using the equation (3).

FIG. 8 is a flowchart of the navigation equation 3 routine. Here, first, in step 106, it is determined whether the demodulation of one or more satellites has been completed. If the demodulation of one or more satellites has been completed, step 107 is executed. In step 107, it is determined whether there is altitude data. If there is altitude data, equation 6 in Table 1 is set in step 109, the σ _m ² calculation routine is executed in step 113 to obtain σ ₁ ² for one satellite, and step 11
In step 5, σδ _x ² and σδ _y ² calculation routines are executed to obtain σδ
_x ² , σ δ _y ² (where σ δ _x ² , σ δ _y ² indicate the prediction error of the X coordinate and Y coordinate of the two-dimensional position obtained from the output of the vehicle speed sensor and the piezoelectric vibration gyro), and step 119.
After determining the sigma _a ² running sigma _a ² calculation routine at,
In step 121, the matrix Σε _{i + 1} indicating the prediction error of the measured value is determined according to the determinant 6 in Table 2, and this routine is ended. Here, since one satellite has been demodulated,
_The matrix A is represented using the elements of the vector from one satellite with _m = 1 to the previous position. Also, the matrix L is [Δ
L1 δ _x δ _y Δh] ^T. Of these, δ _x and δ _y are calculated in the positioning estimation error calculation routine described later by the following equation.

[0052]

[Equation 6] δ _x = STEP · cos (180−ψ) δ _y = STEP · sin (180−ψ) Information obtained from one satellite by solving the navigation equation 6 using the equation 6, piezoelectric vibration The three-dimensional relative position (Δx, Δ) from the azimuth ψ obtained from the gyro, the integrated distance STEP obtained from the vehicle speed sensor, and the altitude difference Δh obtained from the altitude sensor.
y, Δz) can be obtained.

If there is no altitude data in step 107, set equation 7 in Table 1 in step 110,
At step 114, the σ _m ² calculation routine is executed to obtain σ ₁ ² for one satellite, and at step 116, σ δ
_x ² , σδ _y ² Calculation routine is executed and σδ _x ² , σδ
_{After y} ² is determined, the determinant 7 in Table 2 is calculated in step 122.
Then, the matrix Σε _{i + 1} indicating the prediction error of the measured value is determined according to the above, and this routine is ended. Here, since one satellite has been demodulated, the matrix A is represented by using the elements of the vector from one satellite with _m = 1 to the previous position.
Further, the matrix L is represented by [ΔL1 δ _x δ _y 0] ^T. The difference Δz in the height direction is assumed to be zero.

Information obtained from one satellite, direction ψ obtained from the piezoelectric vibration gyro and accumulated distance STEP obtained from the vehicle speed sensor are obtained by solving the navigation equation 7 using the equation (6).
From this, the two-dimensional relative position (Δx, Δy) can be obtained.

If there is no satellite that has been demodulated in step 106, step 108 is executed. Step 1
At 08, it is determined whether there is altitude data. If there is altitude data, the equation 8 in Table 1 is set in step 111, the σδ _x ² , σδ _y ² calculation routine is executed in step 117 to obtain σδ _x ² , σδ _y ² , and step 1
After the σ _a ² calculation routine is executed at 20 to obtain σ _a ² , the matrix Σε _{i + 1} indicating the prediction error of the measured value is determined according to the determinant 8 in Table 2 at step 123, and this routine is determined. To finish. Here, since no satellite has been demodulated, the matrix A is represented by a unit matrix. Also, the matrix L
Is represented by [δ _x δ _y Δh 0] ^T. By solving the navigation equation 8 using the equation 6, the direction ψ obtained from the piezoelectric vibration gyro and the integrated distance STE obtained from the vehicle speed sensor
The three-dimensional relative position (Δx, Δy, Δz) can be obtained from P and the altitude difference Δh obtained from the altitude sensor.

If there is no altitude data in step 108, set equation 9 in Table 1 in step 112,
In step 118, the σδ _x ² , σδ _y ² calculation routine is executed to obtain σδ _x ² , σδ _y ² , and then step 12
In step 4, the matrix Σε _{i + 1} indicating the prediction error of the measured value is determined according to the determinant 9 in Table 2, and this routine is ended. Here, since no satellite has been demodulated, the matrix A is represented by a unit matrix.

Further, the matrix L is [δ _x δ _y 0 0] ^T
It is represented by. The difference Δz in the height direction is assumed to be zero.
By solving the navigation equation 9 using the equation 6, the two-dimensional relative position (Δx, Δy) can be obtained from the azimuth ψ obtained from the piezoelectric vibration gyro and the integrated distance STEP obtained from the vehicle speed sensor.

FIG. 9 is a flowchart of the σ _m ² calculation routine. Here, first, in step 125, the UER
The E calculation routine is executed, and among i = 1 to 4 satellites,
The UARE of each satellite after demodulation (UERE indicates a 1σ error amount that does not depend on the satellite arrangement) is obtained. Next, at step 126, σ _m ² is calculated for each satellite based on the following equation.

[0059]

Equation 7] σ _m ² = UERE ² After this calculation, the routine ends.

FIG. 10 is _a flowchart of the σ _a ² calculation routine. Here, first, in step 127, it is determined whether or not there are UARE and VDOP for error (VDOP indicates an error index for altitude). U for error
If there is ERE and VDOP, (V
Substitute DOP × UERE) ² for σ _a ² and terminate this routine. If there is no UARE and VDOP for the error, the predetermined constant const ^{2 is set} to σ _{a in} step 129.
Substitute in ² to end this routine.

FIG. 11 is a flowchart of the σ _G ² calculation routine. Here, first, in step 130, the integrated distance STEP is read from the memory. Next, in step 131, the azimuth deviation estimation error Δθ error calculated by the azimuth estimation error calculation routine is taken out, and in step 132 σ _G ² is calculated based on the following equation.

[0062]

Σ _G ² = (n × STEP × sin Δθ error) ² where n is a constant. After performing the above calculation, this routine ends.

FIG. 12 is a flowchart of the σδ _x ² , σδ _y ² calculation routine. Here, first, step 1
At 33, the Eψ _x and Eψ _y calculation routines are executed to obtain Eψ
_x and E ψ _y (E ψ _x and E ψ _y indicate errors in the X coordinate and the Y coordinate due to the piezoelectric vibration gyro) are obtained. Next, at step 134, the accumulated distance estimation error STEPerro
Find r. The accumulated distance estimation error STEPerror is the number of pulses within a fixed time taken in by the accumulated distance calculation routine described above, and the rotation error ROLL obtained by the numerical correction routine described later.
Calculate by multiplying error. Next, in steps 135 and 136, σδ _x ² and σδ _y ² are calculated based on the following equations.

[0064]

## EQU9 ## σ δ _x ² = (E ψ _x + STEPerror · cos
(180 ° -ψ)) ² σδ _y ² = (E ψ _y + STEPerror · sin (180
° -ψ)) ² where ψ is the azimuth obtained in the gyro azimuth calculation routine. After performing the above calculation, this routine ends.

FIG. 13 is a flowchart of the Eψ _x , Eψ _y calculation routine. Here, first, step 137.
The azimuth ψ obtained by the gyro azimuth calculation routine is taken out. Next, in step 138, the azimuth estimation error ψerror obtained by the azimuth estimation error calculation routine is extracted. Next, in step 139, the integrated distance S obtained by the integrated distance calculation routine
Take out the TEP. Next, in step 140, ψ _T is calculated by the equation (10). Next, steps 141 and 142
Then, Eψ _x and Eψ _y are calculated based on the equation (11).

[0066]

[Equation 10] ψ _T = ψ−ψ error

[0067]

[Equation 11] Eψ _x = STEP [cos (180 ° −ψ)
-Cos (180 ° -ψ _T)] Eψ _y = STEP [sin (180 ° -ψ) -sin
(180 ° −ψ _T )] After the above calculation, this routine is ended.

FIG. 14 is a flowchart of the positioning estimation error calculation routine. Here, first, in step 143, it is determined whether or not there is a reference orientation. If the reference orientation is present, the process proceeds to step 145, and if not, the process proceeds to step 144. In step 144, it is determined whether or not it is the first positioning. If it is the first positioning, step 145 is executed, and if it is the second or later positioning, steps 146 and thereafter are executed. In step 145, ΣX ₁ is calculated based on the following equation.

[0069]

ΣX ₁ = (A ₁ ^T Σε ₁ ⁻¹ A ₁ ) ^−1, where A ₁ is the matrix A on the left side of the navigation equation at the time of the first positioning.

Σε ₁ is shown in Table 2 at the time of the first positioning.
Matrix Σε showing the prediction error of the measured value obtained from the determinant of
_{i + 1} (i = 0). ΣX _{i + 1} is a matrix representing an error defined by the following equation 13, and ΣX ₁ is at the time of the first positioning. In step 156, the positioning estimation error is calculated according to the equation 14 using σ _x ² and σ _y ² obtained from the equation 12 and the equation 13, and this routine ends.

[0071]

(Equation 13)

[0072]

[Equation 14] Positioning estimation error = (σ _x ² + σ _y ² ) ^{1/2 In the} second and subsequent positioning, first, the integrated distance STEP is read in step 146, and the azimuth ψ is read in step 147. Next, in step 148, δ _x and δ _y are obtained according to the equation (6). Next, in step 149, the matrix Y _{i + 1} is defined according to the equation (15).

[0073]

[Formula 15] Y _{i + 1} = [δ _x δ _y 0 0] ^T Next, in step 150, the above-mentioned σ δ _x ² , σ δ _y ²
The calculation routine is executed to obtain σδ _x ² and σδ _y ² .
Next, in step 151, σ _a1 ² is set to the constant const ² . Next, in step 152, a matrix ΣUi indicating the prediction error model is defined as shown in Expression 16.

[0074]

[Equation 16]

Next, at step 153, the estimation error matrix ΣX _i and the matrix ΣU _i obtained from the information of the previous time (i-th time).
From this, the matrix ΣY _{i + 1} indicating the predicted value of the estimation error is calculated according to the equation (17).

[0076]

ΣY _{i + 1} = ΣX _i + ΣU _i Next, in step 154, the matrix Kg indicating the Kalman gain is calculated according to the formula 18. Here, A _{i + 1} is a matrix on the left side of equations 1 to 9 set in the navigation equations 1 to 3 routines based on Table 1, and Σε _{i + 1} is Table 2
Is a matrix defined based on

[0077]

[Number 18] _{Kg = ΣY i + 1 A i} + 1 T _{[Σε i + 1 + A i +} 1
ΣY _{i + 1} A _{i + 1} ^T ] ⁻¹ Next, at step 155, the present estimation error matrix ΣX _{i + 1} is obtained based on the equation (19). Here, I is a 4 × 4 identity matrix.

[0078]

ΣX _{i + 1} = 2 · [I−Kg · A _{i + 1} ] ΣY _{i + 1 Then} , in step 156, the formula 19 is calculated to calculate the formula 13
Using σ _x ² and σ _y ² obtained by applying the equation, the positioning estimation error is calculated according to equation 14, and this routine is ended.

The calculation method of the estimation error matrix ΣX _{i + 1 in} this routine is to apply an optimum filter called Kalman filter, and the variance / covariance matrix (ΣX _{i + 1)} of the error by the Kalman filter for finding the positioning solution is used. ) 1
The sum of 1 component (σ _x ² ) and 22 components (σ _y ² ) was taken as a square root. If necessary, 33 components (σ _z ² ) may be added to obtain the square root.

FIG. 15 is a flowchart of the positioning routine. Here, first, in step 157, it is determined whether or not the positioning is the second or subsequent positioning. If it is the first positioning, step 161 is executed, and if it is the second or later positioning, steps 158 to 160 are executed.

In step 161, X ₁ is calculated based on the following equation.

[0082]

X ₁ = A ₁ ⁻¹ L ₁ Here, A ₁ is the matrix A on the left side of the navigation equation at the time of the first positioning.

L ₁ is the matrix L on the right side of the navigation equation at the time of the first positioning. X ₁ indicates the solution to be obtained at the time of the first positioning. In step 162, the obtained solution (XYZ coordinates) is converted into latitude, longitude, and altitude, and this routine ends.

In step 157, in the case of the second and subsequent positioning, in step 158, the matrix Y _{i + 1} indicating the predicted value of the solution obtained by the positioning estimation error calculation routine is taken out.
Next, in step 159, the matrix D indicating the improvement amount is given by
Calculate based on the formula 1.

[0085]

D = L _{i + 1} −A _{i + 1} Y _{i + 1} where A _{i + 1} is the matrix A on the left side of the navigation equation in this positioning. L _{i + 1} is the matrix L on the right side of the navigation equation in this positioning. Next, in step 160, the matrix X _{i + 1} indicating the positioning solution of the current positioning is calculated based on the formula 22.

[0086]

X _{i + 1} = Y _{i + 1} + Kg · D Here, Kg is a matrix showing the Kalman gain calculated according to the formula 18 in the above-mentioned positioning estimation error calculation routine. Thereafter, in step 162, the obtained solution (XYZ coordinates) is converted into latitude, longitude and altitude, and this routine is ended.

The calculation method of this routine is to apply an optimum filter called Kalman filter,
It suppresses abnormal changes in the solution. Since the solution using the output of the piezoelectric vibration gyro and the vehicle speed sensor gives the prediction of the solution of the equation, it becomes a stop when the positioning solution fluctuates greatly due to the abnormality of the satellite. Especially, vehicle speed = 0
In the case of, there is no movement as a solution prediction, and the prediction error is 0
Therefore, there is an effect of suppressing the fluctuation of the solution while the vehicle is stopped.

FIG. 16 is a flowchart of the altitude correction routine. Here, first, in step 163, VDO
It is determined whether P is lower than the VDOP upper limit. VDOP
Is below the upper limit of VDOP, jump to step 164
If DOP is higher than the VDOP upper limit, this routine is terminated without doing anything. In step 164, the difference between the altitude obtained by the positioning and the altitude obtained by the altitude sensor is taken to obtain the correction amount. Here, in this altitude correction routine, demodulation of three or more satellites is completed and positioning is performed by four satellites, so the altitude obtained by the positioning is obtained only by GPS. Since positioning by GPS is usually obtained with a delay of several seconds, there is a time difference with the altitude obtained by the altitude sensor. Therefore, the past altitude data updated in step 69 in the gyro direction calculation routine is used in accordance with the GPS delay. For example, GPS delay is 3
In case of seconds, from the altitude obtained by positioning to the altitude H [-3]
To be the correction amount. Next, in step 165, the correction amount is added to the past altitude data for correction. Next, in step 166, the UARE calculation routine is executed to obtain the UARE. Next, in step 167, VDOP and UARE are stored, and this routine ends. VDOP is σ _z
Calculate by taking the square root of ² .

FIG. 17 is a flow chart of the UARE calculation routine. Here, first, SVACC (SVaccu
racy: An index calculated by calculating the influence of one satellite on positioning from the internal state of each satellite), and step 168
At, it is determined whether SVACC is 0 or more and less than 6. If SVACC is 0 or more and less than 6, step 17
At 0 User Range Accuracy URA (User Range
Accuracy) is calculated by the equation 23. If SVACC is 7 or more, the user range accuracy URA is calculated by the formula 24 in step 169.

[0090]

[ ^Equation 23] URA = 2 ^{(1 + SVACC / 2)} (0 ≦ AVACC <7)

[0091]

[ ^Expression 24] URA = 2 ^(SVACC-2) (7 ≦ AVACC) Next, in step 171, the UARE is calculated based on the expression 25.

[0092]

URE = (UEA ² + Σ (value other than the intentional deterioration coefficient) ² ) ^1/2 = (UEA ² +119.7) ^{1/2 Of} these, the intentional deterioration coefficient is the error of the satellite clock or This is due to the prediction error of the satellite orbit. Values other than the intentional deterioration coefficient are perturbation prediction errors of satellite orbit (value of 1σ =
1.0 m), ionosphere propagation delay correction error (value of 1σ = 7.
5 m), tropospheric propagation delay correction error (1σ value = 2.0
m), receiver noise (value of 1σ = 7.5 m), multipath (value of 1σ = 1.2 m), other error based on satellite (value of 1σ = 0.5 m), other error based on control (Value of 1σ = 0.5 m) and other errors based on the user (value of 1σ = 0.5 m). When the magnitudes of these errors are changed, the numerical value of the equation 25 may be changed. After that, this routine ends.

FIG. 18 is a flowchart of the gyro correction routine. The correction mode is a flag indicating the correction status. Here, first, it is determined which of the correction modes is selected, and steps 173, 174, and 17 are performed according to the correction mode.
5, any one of the mode-1, mode 0, mode 1, and mode 2 routines is executed, and this routine is terminated.

Here, the first correction is that the correction sequentially passes during traveling.
Gyro or GP between the second point, the third point and the second point
The output data of S is used. In mode-1, the start of correction is determined, in mode 0, the data at the first point is collected, in mode 1, the data at the second point is collected, and in mode 2, the data at the third point is collected. Correction is performed based on these data.

FIG. 19 is a flow chart of the mode-1 routine. This routine is executed when the correction mode is mode-1. The correction mode is set by another routine.
The mode is set to mode-1 after the initial state and after the correction is completed. Therefore, the mode-1 routine is selected in the initial state. First, in step 177, it is determined whether the vehicle speed is higher than a predetermined lower limit value. The vehicle speed is the lower limit (eg 10K
If it is lower than m / h), the correction mode is kept at mode-1 in step 183, and the process ends. If the vehicle speed is higher than the lower limit value, it is checked in step 178 whether gyro data for the delay time is stored. Here, when the gyro data for the delay time is stored, step 1
However, if the gyro data corresponding to the delay time is not stored, the process proceeds to step 183, the correction mode remains as mode-1, and the process ends. In step 179, the correction mode is set to mode 0, and in step 180, the azimuth is φ.
m1

Substitute [0] and the GPS delay amount into φm1 [1]. Next, in step 181, the timer Tφ is set to 0.
Is set, the counter count0 is set to 0 in step 182, and this routine ends. The timer Tφ is
After this, the elapsed time is counted.

As described above, in the mode-1 routine, the gyro correction is started when the vehicle speed is equal to or higher than the lower limit value and the gyro data for the delay time of the data output by the GPS is stored. .

FIG. 20 is a flowchart of the mode 0 routine. This routine is executed when the correction mode is mode 0. First, at step 184, the counter coun
t0 is incremented by 1. Next, step 18
At 5, it is determined whether the vehicle speed is higher than a predetermined lower limit value. In step 186, it is determined whether or not the gyro data for the delay time is stored. In step 187, it is determined whether or not the mode-1 is continued. In step 188, it is determined whether or not the azimuth at this time is the same as that of the mode-1. If any of steps 185 to 188 is not established, the correction mode is changed to mode-1 in step 189, and this routine ends. Step 18
When all of 5 to 188 are established, step 19
At 0, it is determined whether or not the azimuth stabilization is OK. If the orientation is unstable, this routine ends. If the orientation is stable, the numerical correction routine is executed in step 191, and the correction mode is set to mode 1 in step 192.
Although details of the numerical correction routine will be described later, the difference between the GPS azimuth and the gyro azimuth is used as a correction amount, and the gyro azimuth and the like are corrected according to the correction amount. Next, Step 19
At 3, the azimuth, delay, and error measured by GPS are 0

Substitute for [0], φ0 [1], and φ0 [2]. Next, at step 194, the timer Tφ is set to 0. Next, in step 195, the gyro azimuth gldelay-raw [delay amount] according to the delay amount is substituted into φ0raw. Next, at step 196, counter count0 and counter co
This routine is ended by setting unt1 to 0.

As described above, in the mode 0 routine, the start condition in the mode-1 (the vehicle speed is equal to or higher than the lower limit value, and
The gyro data corresponding to the delay time of the data output from the GPS is stored), the gyro is continuously executed from the mode-1 and the direction is not changed from the mode-1. Azimuth φ0 raw, GPS azimuth φ0

Data such as [0] is stored. This point is the first
It becomes the point of.

FIG. 21 is a flowchart of the mode 1 routine. This routine is executed when the correction mode is mode 1. First, at step 197, the counter coun
t1 is incremented by 1. Next, Step 19
At 8, it is determined whether the vehicle speed is higher than a predetermined lower limit value. In step 199, it is determined whether or not the gyro data for the delay time is stored. Step 198, 1
If any of 99 is not established, the counter count1 is set to 0 in step 203, and this routine ends. When both steps 198 and 199 are established, it is determined in step 200 whether or not the mode 1 is the second time or later. If it is the first time, the azimuth determined in step 201,
Each delay is φ1

Substituting [0] and φ1 [1], this routine ends. Mode 1 is 2 at step 200
If it is after the first time, in step 202, the previous mode 1
And whether the azimuth at this point is the same or not. If the azimuths are not continuous, in step 203 the counter co
unt1 is set to 0, and this routine ends. If the azimuths are the same, it is determined in step 204 whether the azimuth stabilization is OK. If the orientation is unstable, this routine ends. If the azimuth is stable, a numerical correction routine is executed in step 205, and the azimuth and delay measured by GPS in step 206 are φ1 respectively.

[0], φ1 [1]
To. Next, in step 207, the error is φ0.
[2] It is determined whether or not it is greater than. If it is larger, the error is substituted into φ1 [2] in step 208, and if it is smaller, φ0 [2] is substituted into φ1 [2] in step 209.
Next, in step 210, the gyro direction according to the delay amount
Substitute φ1raw with gldelay-raw [delay amount]. next,
At step 211, φ0 [1] is added to the value of the timer Tφ and φ1 [1] is subtracted to obtain T10. Next, in step 212, φ1

[0] to φ0

Subtract variable [0]
11 Next, in step 213, it is determined whether or not this angle difference is acceptable. If adoption is not OK, this routine ends. If the adoption is OK, at step 214, φ1raw is subtracted from φ0raw to obtain raw1. next,
At step 215, col1 and raw1 are each divided by T10 and updated. Next, in step 216, the correction mode is changed to mode 2, and in step 217 the timer Tφ is set to 0.
, And the counter count1 and the counter count2 are set to 0 in step 218, and this routine ends.

As described above, in the mode 1 routine, the start condition in the mode-1 (the vehicle speed is equal to or higher than the lower limit value, and
Gyro data for the delay time of the data output by GPS is stored), Mode 1 is executed twice or more continuously, and the same azimuth is continuously stable. , Gyro direction is φ1 raw, GPS direction is φ1

Data such as [0] is stored. This is the second point. Further, the time difference T10 between the first point and the second point, the GPS direction difference φ1

[0] -φ0

[0]
(Col1 in step 212), and the difference in gyro direction φ
1raw−φ0raw (raw1 in step 214) is calculated, and in step 215 the GPS direction col1 per hour,
And the gyro direction raw1 per hour is obtained.

FIG. 22 is a flow chart of the mode 2 routine. This routine is executed when the correction mode is mode 2. First, at step 219, the counter coun
t2 is incremented by 1. Next, step 22
At 0, it is determined whether the vehicle speed is higher than a predetermined lower limit value. In step 221, it is determined whether or not the gyro data for the delay time is stored. Step 220, 2
If any of the items 21 is not satisfied, the correction mode is kept at mode 2 in step 225, the counter count2 is set to 0 in step 238, and this routine is ended. When both steps 220 and 221 are satisfied, it is determined in step 222 whether or not the mode 2 is the second time or later. If it is the first time, the azimuth determined in step 223,
Each delay is φ2

Substituting [0] and φ2 [1], this routine is ended. Mode 1 is set to 2 in step 222
If it is the second time or later, in step 224, the previous mode 2
And whether the azimuth at this point is the same or not. If the azimuths are not continuous, the correction mode remains at mode 2 in step 2025, and the counter co is set in step 238.
unt2 is set to 0, and this routine ends. If the azimuths are the same, it is determined in step 226 whether the azimuth stabilization is OK. If the orientation is unstable, this routine ends. If the azimuth is stable, the numerical correction routine is executed in step 227, and the azimuth and delay measured in step 228 are φ2 respectively.

Substitute for [0] and φ2 [1]. Next, in step 229, it is determined whether or not the error is larger than φ1 [2]. If larger, the error is substituted into φ2 [2] in step 231, and if smaller, step 230
Substitute φ1 [2] for φ2 [2]. Next, in step 232, the gyro direction gldelay-raw according to the delay amount
Substitute [delay amount] into φ2raw and set the value of timer Tφ to φ
Add 1 [1] and subtract φ2 [1] to T21, φ2

[0] to φ1

Subtract [0] to make col2. Next, in step 233, it is determined whether or not this angle difference is acceptable.
If the adoption is not OK, the counter coun at step 234
t2 is set to 0, col2 is set to 999 (maximum value) in step 235, and this routine ends. If adopted, in step 236, φ2raw is subtracted from φ1raw to obtain raw2, and col2 and raw2 are each divided by T21 to be updated. Next, in step 237, it is determined whether or not the absolute value of the difference between col1 and col2 is greater than the predetermined lower limit value divided by T21. If it is smaller, the counter count2 is set to 0 in step 238, and this routine ends. If it is larger, step 239 is executed. In step 239, co
Whether the absolute value of the difference between l1 and col2 is larger than the predetermined upper limit value divided by T21, and the absolute value of the difference between raw1 and raw2 is larger than the predetermined upper limit value divided by T21. To judge. If not established, jump to step 246, and if established, step 240 is executed. Raw1 in step 240
It is determined whether or not the absolute value of the difference between RAW and raw2 is larger than a value obtained by dividing a predetermined lower limit value by T21. If it is smaller, this routine is ended, and if it is larger, step 241 is executed. In step 241, it is determined whether the sum of correction amounts is larger than a predetermined lower limit value. If it is larger, the AD conversion coefficient correction value is initialized in step 242, and the AD conversion coefficient correction routine is executed in step 243. If it is smaller, the successive approximation routine is executed in step 244. Then, step 245
Then, the correction mode is set to the mode-1 and the routine proceeds to step 246. In step 246, φ2

Φ0 for [0]

Substitute into [0], substitute φ2 [1] into φ1 [1], and obtain φ2 [2]
To φ1 [2], col2 to col1, and raw2 to ra
Substitute in w1. Next, at step 247, the timer Tφ
Is reset to 0 and this routine ends.

As described above, in the mode 2 routine, the start condition in the mode-1 (the vehicle speed is equal to or higher than the lower limit value, and
Gyro data for the delay time of the data output by GPS is stored), Mode 2 is executed twice or more continuously, and the same azimuth is continuously stable. , Gyro direction is φ2 raw, GPS direction is φ2

Data such as [0] is stored. This is the third point. In addition, the time difference T21 between the third point and the second point and the GPS direction difference φ2

[0] -φ1

[0]
(Col2 in step 232), and the difference in gyro direction φ
2raw-φ1raw is calculated, and in step 235 the GPS direction col2 per hour and the gyro direction per hour are calculated.
Get raw2.

FIG. 23 is a flowchart of the numerical correction routine. First, in step 248, the correction amount is obtained by subtracting the azimuth ψ [delay] in which the delay amount is added from the GPS azimuth.
Next, in step 249, it is determined whether or not this correction amount is N times or more the GPS heading error and is half or more of the minimum unit of the GPS heading output. If this condition is satisfied, the correction is made in step 250. The correction amount is added to the sum of amounts to obtain an integrated value of the correction amounts. Here, the GPS bearing error is calculated based on the following equation. Ve is a velocity error constant of GPS.

[0104]

[Equation 26] GPS azimuth error = arctan (Ve / (vehicle speed + Ve)) Next, in step 251, a correction amount is added to the gyro azimuth,
Correct the gyro direction. Next, at step 252, azimuth delay amounts ψ [−3], ψ [−2], ψ [−
1], ψ

A correction amount is added to each of [0] to perform correction. Next, in step 253, the reference direction is validated, and in step 2
At 54, the orientation estimation error ψ error is set to zero.

Next, at step 255, it is judged whether or not the GPS vehicle speed is equal to or higher than a predetermined vehicle speed correction lower limit (for example, 10 Km / h), and if it is equal to or lower than the vehicle speed correction lower limit, this routine is finished and the vehicle speed correction is performed. If it is above the lower limit, step 25
In step 6, the GPS vehicle speed is divided by the number of pulses P [delay] in consideration of the delay to obtain the variable ROLLnew. Also, the GPS vehicle speed error is divided by the number of pulses P [delay] in consideration of the delay to obtain the variable ROLLerrornew. Next, in step 257, the variable ROLL is corrected based on the equation 27, and in step 258, the variable ROLL error is corrected based on the equation 28.

[0106]

[Equation 27]

[0107]

[Equation 28]

Next, at step 259, the delay amount of the number of pulses P [-3], P [-2], P [-1], P

A correction amount is added to each of [0], the correction is performed, and this routine ends.

FIG. 24 is a flowchart of the AD conversion coefficient correction routine and the successive approximation routine. First, here, the GPS directions are col1 and col2, and the gyro direction is ra.
The matrices A _i ^G and L _i ^G are defined as follows using w1 and raw2.

[0110]

[Equation 29]

[0111]

## EQU30 ## L _i ^G = [col1 col2] ^-T Furthermore, the correction coefficient α and θ ₀ are used to define the matrix X _i ^G as follows.

[0112]

X _i ^G = [α θ ₀ ] ^-T Here, the relationship between the correction coefficients α and θ ₀ will be described with reference to FIG. Here, while the vehicle is traveling on the road R, the mode 1 is set between the first point a1 and the second point a2,
It is assumed that the mode 2 is set between the second point a2 and the third point a3. The actual heading of the vehicle at the first point a1 is d1, and the actual heading of the vehicle at the second point a2 is d.
2, the actual direction of the vehicle at the third point a3 is d3.
Here, it is assumed that it takes time T10 from the first point a1 to the second point a2 and time T21 takes from the second point a2 to the third point a3. Here, the direction change amount of GPS in mode 1 is col1 · T10, and the direction change amount per time is col1. Further, the GPS direction change amount in mode 2 is col2 · T21, and the time direction change amount is col2. The gyro direction change amount in mode 2 is raw1 · T10, and the time direction change amount is raw1. Also, the gyro direction change amount in mode 2 is raw2 · T21, and the direction change amount per time is
It becomes raw2. Here, if A _i ^G · X _i ^G = L _i ^G (i is an integer), then A _i ^G and L _i ^G can be calculated.
_The correction factors α and θ ₀ that are the respective terms of _i ^G are obtained.

In the AD conversion coefficient correction routine, first, at step 260, variable i is set to 0, and then,
G used in calculating col1 and col2 in step 261
1 / σθ ² is obtained from φ 2 2 showing the maximum value of the PS azimuth error by the formula 32.

[0114]

Equation 32] ^{1 / σθ 2 = 1 / φ2} 2 2 Next, in step 262, calculated based matrix Σε ₁ ^{G -1} to the following equation.

[0115]

[Expression 33]

Next, at step 263, the matrix ΣX ₁ ^G is calculated based on the following equation.

[0117]

ΣX ₁ ^G = (A ₁ ^GT Σε ₁ ^{G −1} A ₁ ^G ) ⁻¹ Then, in step 264, the matrix X ₁ ^G is calculated based on the following equation.

[0118]

[Expression 35] X ₁ ^G = A ₁ ^{G -1} L ₁ ^G If there is no inverse matrix being corrected, the correction is canceled. As a result, the matrix X ₁ ^G is obtained, and the correction coefficients α and θ ₀ are obtained. Next, at step 265, the conversion coefficient ADA of the piezoelectric vibration gyro 15 is multiplied by the correction coefficient α to update the conversion coefficient ADA. Further, in step 266, the square root of the 11 components of the matrix ΣX _{i + 1} ^G is taken to obtain the error coefficient ADAerror of the piezoelectric vibration gyro 15. Where i =
Since it is 0, the square root of 11 elements of the matrix ΣX ₁ ^G is taken here. Next, in step 267, a value obtained by dividing θ ₀ by the number of samples samples and the conversion coefficient ADA is converted into the conversion coefficient A.
Subtract from DC to update the conversion factor ADC. Next, in step 268, the square root of 21 components of the matrix ΣX _{i + 1} ^G is taken and divided by the number of samples samples and the conversion coefficient ADA to obtain the error coefficient ADCerror of the piezoelectric vibration gyro 15.
Since i = 0 here, 2 of the matrix ΣX ₁ ^G is used here.
Take the square root of one component. Next, in step 269, re-collection of the delay data is designated, and in step 270, the sum of correction amounts is set to zero, and this routine ends.

In the successive approximation routine, first, step 27
At 1, it is determined whether or not the coefficient correction is for the second shift. If it is the first coefficient correction, the process proceeds to step 260, where AD
The same processing as the conversion coefficient correction routine is performed. If it is the second or subsequent coefficient correction, the process proceeds to step 272 and 1 is added to the variable i. Next, in step 273, φ2 indicating the maximum value of the GPS heading error used when calculating col1 and col2
Σθ ² is obtained from ² according to the equation 36.

[0120]

Σθ ² = φ 2 2 ² Next, in step 274, the matrix Σε _{i + 1} ^{G −1} is calculated based on the following equation.

[0121]

(37)

Next, step 275 and step 276.
At matrix ΣX _i ^G and matrix ΣU _i ^G and matrix ΣY, respectively.
Substitute in _i ^G. Next, in step 277, the matrix Y
_{i + 1} ^G is defined based on the following equation.

[0123]

Y _{i + 1} ^G = [1 0] ^T Next, in step 278, the matrix Kg ^G indicating the Kalman gain is obtained based on the following equation.

[0124]

[Number 39] _{^{Kg G = ΣY i + 1 G}} A i + 1 GT [Σε _{i + 1} ^G
+ A _{i + 1} ^G ΣY _{i + 1} ^G A _{i + 1} ^GT ] ⁻¹ Next, in step 279, the estimation error matrix Σ is calculated based on the following equation.
_{Find i + 1} ^G. Here, I is a 2 × 2 identity matrix.

[0125]

ΣX _{i + 1} ^G = 2 [I−Kg ^G A _{i + 1} ^G ] ΣY _{i + 1} ^G Next, in step 280, the matrix D ^G indicating the improvement amount is obtained based on the following equation.

[0126]

D ^G = L _{i + 1} ^G −A _{i + 1} ^G Y _{i + 1} ^G Next, in step 281, the matrix Y _{i + 1} ^G is multiplied by the Kalman gain by the improvement amount based on the following equation. Add value and update.

[0127]

Y _{i + 1} ^G = Y _{i + 1} ^G + Kg ^G D ^G After that, the process proceeds to step 265. At step 265,
The conversion coefficient ADA of the piezoelectric vibration gyro 15 is multiplied by α to update the conversion coefficient ADA. Further, in step 266, the square root of the 11 components of the matrix ΣX _{i + 1} ^G is taken to obtain the error coefficient ADAerror of the piezoelectric vibration gyro 15. Next, in step 267, a value obtained by dividing θ ₀ by the number of samples samples and the conversion coefficient ADA is subtracted from the conversion coefficient ADC to update the conversion coefficient ADC. Next, in step 268, the square root of the 21 components of the matrix ΣX _{i + 1} ^G is taken and the number of samples samp
Piezoelectric vibration gyro 15 divided by les and conversion coefficient ADA
Error coefficient ADCerror. Then, step 269
In step 270, the correction amount sum is set to zero, and this routine ends.

The correction coefficients α and θ ₀ are obtained by the AD conversion coefficient correction routine, and the gyro conversion coefficient AD is obtained.
A, ADC and error coefficients ADAerror, ADCerror are G
Corrected by the measured values of PS and gyro, it becomes possible to perform more accurate positioning.

FIG. 25 is a flowchart of the gyro zero point correction routine. First, here, in step 282, it is determined whether or not the vehicle speed = 0 and the correction amount sum is less than or equal to a predetermined lower limit value. If not, this routine is ended, and if not, the process proceeds to the next step 283. . In step 283, it is determined whether the power has just been turned on or whether drift has been detected. If no, step 285 takes the average of the vehicle speed = 0 for one second, and if not, step 28
At 4, average all delay data. Next in Step 286.
The maximum value and the minimum value of the sum for 1 second are calculated. In step 287, it is determined whether or not the difference between the maximum value and the minimum value is smaller than a predetermined upper limit value (e.g., 20 deg / s equivalent). If it is smaller, the average value obtained in step 288 is changed to the zero point.

After that, re-collection of delay data is designated in step 289, the correction mode is designated as mode-1 in step 290, and the sum of correction amounts is set to zero in step 291 to terminate this routine.

In this gyro zero point correction routine,
The zero point of the gyro is measured and corrected so that the drift amount becomes zero.

As described above, in this embodiment, the vehicle speed sensor 14 and the piezoelectric vibration gyro 15, which are relative sensors for measuring the relative position between the vehicle and the ground surface, and the signals emitted from a plurality of satellites are used to detect the relative speed. When GPS is a satellite information detecting means for detecting a relative position between a satellite and a vehicle, and positioning is possible based on information of a plurality of satellites detected by the GPS, and an estimated positioning error is smaller than a predetermined level, the GPS output The absolute position of the vehicle is measured only from
Steps 31 to 35 and 42 to 53, which are positioning means for positioning the absolute position of the vehicle from only the output of the GPS and the output of the relative sensor or the output of the relative sensor, are provided. Therefore,
Accurate positioning is possible.

Further, the piezoelectric vibrating gyro 15 which is a gyro for detecting the heading deviation of the vehicle, the vehicle speed sensor 14 which is a distance sensor for detecting the traveling distance of the vehicle, and the satellites and the vehicles based on the signals emitted from the plurality of satellites. GPS which is a relative speed detecting means for detecting a relative speed with respect to the vehicle, a gyro azimuth calculating routine 54 which is an azimuth angle calculating means for obtaining an azimuth angle of the vehicle from the output of the piezoelectric vibration gyro 15, and an output of the vehicle speed sensor 14 of the vehicle. An integrated distance calculation routine 55, which is an integrated distance calculating means for obtaining an integrated distance, and (1) the relative speed detecting means detects a relative speed between three satellites or more, and a relative distance between the three satellites or more. When the positioning estimation error of the positioning using the velocity is smaller than a predetermined level, the positioning using the relative velocity between three or more satellites is performed (step 3
2, 35), (2) GPS detects the relative velocity between the two satellites, the direction estimation error of the gyro azimuth angle is smaller than a predetermined level, and the relative velocity between the two satellites and the gyro azimuth. When the positioning estimation error of the positioning using the angle is smaller than a predetermined level, the positioning using the relative velocity and the azimuth between the two satellites is performed (steps 44 and 47), and (3) the azimuth estimation of the gyro azimuth. When the error is smaller than a predetermined level, the accumulated distance estimation error of the accumulated distance by the vehicle speed sensor is smaller than the prescribed level, and the positioning estimation error of positioning using the gyro azimuth and the accumulated distance by the vehicle speed sensor is smaller than the prescribed level. , Positioning using azimuth and integrated distance (steps 49 and 47), positioning means (steps 31 to 3).
5, steps 42-53). Therefore, more accurate positioning can be performed.

Further, by steps 48, 50 and 53,
If none of the above (1), (2), and (3) is satisfied, positioning is not performed, so that inaccurate information is not output and reliability is high.

Positioning is performed in step 1 of the positioning routine.
Since the positioning solution is filtered by the Kalman filter as indicated by 57 to 160, an abnormal solution is not output even if the positioning method is changed while the vehicle is running or stopped, and the reliability is high.

[0136]

According to the inventions of claims 1 to 4, more accurate positioning can be performed.

According to the third aspect of the invention, since the information with low accuracy is not output, the reliability is high.

Further, according to the invention of claim 4, even if the positioning method is changed while the vehicle is running or stopped, an abnormal solution is not output and the reliability is high.

[Brief description of drawings]

FIG. 1 is a circuit configuration diagram of an embodiment of the present invention.

FIG. 2 is a flowchart of a main routine according to the embodiment of this invention.

FIG. 3 is a flowchart of a gyro azimuth calculation routine according to the embodiment of this invention.

FIG. 4 is a flowchart of a bearing estimation error calculation routine according to the embodiment of this invention.

FIG. 5 is a flowchart of an integrated distance calculation routine according to the embodiment of this invention.

FIG. 6 is a flowchart of a navigation equation 1 routine according to the embodiment of this invention.

FIG. 7 is a flowchart of a navigation equation 2 routine according to the embodiment of this invention.

FIG. 8 is a flowchart of a navigation equation 3 routine according to the embodiment of this invention.

FIG. 9 is a flowchart of a σ i ² calculation routine according to the embodiment of this invention.

FIG. 10 is a flowchart of a σ a ² calculation routine according to the embodiment of this invention.

FIG. 11 is a flowchart of a σ G ² calculation routine according to the embodiment of this invention.

FIG. 12 is a flowchart of a σδx ² , σδy ² calculation routine according to the embodiment of this invention.

FIG. 13 is a flowchart of an Eψx Eψy calculation routine according to the embodiment of this invention.

FIG. 14 is a flowchart of a positioning estimation error calculation routine according to the embodiment of this invention.

FIG. 15 is a flowchart of a positioning routine according to the embodiment of this invention.

FIG. 16 is a flowchart of an altitude correction routine according to the embodiment of the present invention.

FIG. 17 is a flowchart of a UARE calculation routine according to the embodiment of this invention.

FIG. 18 is a flowchart of a gyro correction routine according to the embodiment of the present invention.

FIG. 19 is a flowchart of a mode-1 routine according to the embodiment of this invention.

FIG. 20 is a flowchart of a mode 0 routine according to the embodiment of this invention.

FIG. 21 is a flowchart of a mode 1 routine according to the embodiment of this invention.

FIG. 22 is a flowchart of a mode 2 routine according to the embodiment of this invention.

FIG. 23 is a flowchart of a numerical correction routine according to the embodiment of the present invention.

FIG. 24 is a flowchart of an AD conversion coefficient correction routine and a successive approximation routine according to the embodiment of this invention.

FIG. 25 is a flowchart of a gyro zero-point correction routine according to the embodiment of the present invention.

FIG. 26 is an explanatory diagram of an example of the present invention.

FIG. 27 is an explanatory diagram of an example of the present invention.

[Explanation of symbols]

 10 reception antenna 11 GPS receiver 12 GPS demodulator 13 display device 14 vehicle speed sensor 15 piezoelectric vibration gyro 16 altitude sensor 17 electronic control unit 18 input / output interface 19 microcomputer 20 analog / digital converter

 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Takeshi Ito 2-1, Asahi-cho, Kariya city, Aichi Prefecture Aisin Seiki Co., Ltd. (72) Tomio Hota 2-chome, Asahi-cho, Kariya city, Aichi prefecture Aisin Seiki Co., Ltd.

Claims (4)

[Claims] 1. A relative sensor for measuring the relative position between a vehicle and the surface of the earth; satellite information detecting means for detecting the relative position between each satellite and the vehicle from the signals emitted by a plurality of satellites;
When positioning is possible from the information of the plurality of satellites detected by the satellite information detecting means, and the positioning estimation error is smaller than a predetermined level, the absolute position of the vehicle is measured only from the output of the satellite information detecting means. In other cases, an on-vehicle positioning device comprising a positioning means for positioning the absolute position of the vehicle only from the output of the satellite information detecting means and the output of the relative sensor or the output of the relative sensor. 2. A gyro for detecting an azimuth deviation of a vehicle; a distance sensor for detecting a traveling distance of the vehicle; a relative speed detecting means for detecting a relative speed between each satellite and the vehicle from signals emitted by a plurality of satellites; Azimuth calculation means for obtaining the azimuth angle of the vehicle from the output of the gyro; Integrated distance calculation means for obtaining the integrated distance of the vehicle from the output of the distance sensor; (1) Relative speed between the relative speed detection means and three or more satellites Is detected and the positioning estimation error of the positioning using the relative speed with three or more satellites is smaller than a predetermined level, the positioning using the relative speed with three or more satellites is performed, and (2 ) The relative velocity detecting means detects the relative velocity between the two satellites, the azimuth estimation error of the azimuth angle calculated by the azimuth angle calculating means is smaller than a predetermined level, and 2
When the positioning estimation error of the positioning using the relative speed between the satellite and the azimuth angle calculated by the azimuth detecting means is smaller than a predetermined level, the positioning using the relative speed between the two satellites and the azimuth angle is performed. (3) The azimuth estimation error of the azimuth angle calculated by the azimuth angle calculation means is smaller than a predetermined level, the accumulated distance estimation error of the accumulated distance calculated by the accumulated distance calculation means is smaller than a predetermined level, and When the positioning estimation error of the positioning using the azimuth angle calculated by the azimuth angle detection means and the integrated distance calculated by the integrated distance calculation means is smaller than a predetermined level, the positioning means performs positioning using the azimuth angle and the integrated distance. An on-vehicle positioning device including; 3. The positioning device according to claim 2, wherein the positioning means does not correspond to any of (1), (2) and (3),
An on-vehicle positioning device characterized by not performing positioning. 4. The on-vehicle positioning device according to claim 1, wherein the positioning means filters a positioning solution with a Kalman filter.