JP3473113B2 - Gyro bearing calculation device - Google Patents

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention, in a vehicle-mounted positioning device that measures the position of a vehicle on a vehicle such as an automobile and notifies the current position information, determines the direction indicated by the gyro from the output data of the gyro for measuring the direction. The present invention relates to a gyro direction calculating device for calculating.

[0002]

2. Description of the Related Art As an apparatus of this type, Japanese Patent Application Laid-Open No. 5-172 is known.
There is a technique disclosed in Japanese Patent No. 575. According to this technique, the error at the zero point occurring in the output of the gyro is corrected by the azimuth change error.

[0003]

However, it is usually difficult to detect straight ahead while the vehicle is traveling, and the zero point correction is performed only when the vehicle is stopped. For this reason, when traveling continuously for a long time on a highway or the like, the zero point of the gyro may be misaligned and the positioning accuracy may be deteriorated.

Therefore, it is an object of the present invention to be able to correct the 0 point even while the vehicle is running.

[0005]

In order to solve the above problems, in the invention according to claim 1, a gyro for detecting a heading deviation of a vehicle; a gyro heading calculating means for detecting a gyro heading from an output of the gyro; Azimuth measuring means for detecting the relative speed between each satellite and the vehicle from the signals emitted by a plurality of satellites and measuring the reference azimuth of the vehicle; the azimuth measured by the azimuth measuring means when the speed of the vehicle is higher than a predetermined value VL. When there is no change with time, the output signal of the gyro is set to zero, and 0 point correction means for correcting 0 point is provided .
The point correction means changes the 0 point as the vehicle speed increases.
It means that the time used for the purpose was lengthened . It was

[0006]

In order to solve the above-mentioned problems, in the invention described in claim 2, in the claim 1, the 0-point correction is performed.
The means is a predetermined vehicle speed VL, VU (> VL), a predetermined time.
When TL and TU (> TL) are set, the vehicle speed V is 0 <
When V <VL, 0 point correction is not performed and VL ≦ V ≦ VU
At this time, T = TL + (TU−TL) × (V−VL) / (V
U-VL) for a time period longer than
If there is no change in the set azimuth, 0 point correction is performed and VU <V
When the time indicated by T = TU is exceeded, the azimuth measuring means
When there was no change in the measured azimuth, 0 point correction was performed.

[0008]

[0009]

According to the invention described in claim 1, the gyro azimuth is calculated from the output data of the gyro. On the other hand, the azimuth measuring means detects the relative speed between each satellite and the vehicle from the signals emitted by the plurality of satellites, and measures the reference azimuth of the vehicle. The 0-point correction means sets the output signal of the gyro to be 0 when the vehicle speed is higher than the predetermined value VL and the azimuth measured by the azimuth measuring means does not change with time. to correct. Therefore, the zero point correction of the gyro can be performed even while traveling. In this case, 0 point is changed as the vehicle speed becomes higher
By increasing the time used for this, the zero point correction is not performed unless the time during which the azimuth measured by the azimuth measuring means does not change as the vehicle speed increases. Therefore, when traveling at a high speed on a gentle curve, the zero point correction is not performed, so that the correction can be performed with high accuracy.

[0010]

According to the second aspect of the invention, the zero point correction is not performed when the vehicle speed is low. Therefore, since the data of the azimuth measuring means having low accuracy at low vehicle speed is not used, the accuracy is not deteriorated. In addition, as the vehicle speed increases, the time when the direction measured by the direction measuring means does not change becomes long, and 0
I am not trying to correct points. Therefore, when traveling on a gentle curve during high-speed traveling, the zero point correction is not performed, and therefore accurate correction can be performed.

In this case , the azimuth difference between the reference azimuth from the azimuth measuring means and the azimuth obtained from the output of the gyro azimuth calculating means is opposite to the direction of the azimuth which changes by correcting the zero point. If the 0 point is corrected only when the direction is , the 0 point correction does not correct the directions measured by the azimuth measuring means and the gyro azimuth calculating means to be different from each other. Can be corrected.

[0013]

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 it is determined in step 31 that the number of satellites that can be demodulated is 2 or less, or if the positioning estimation error is greater than the upper limit in step 34, it is determined in step 42 whether or not demodulation of 2 satellites or more is 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 in the positioning routine of step 47 . 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 a gyro direction calculation routine. Here, first, in step 56, the azimuth deviation Δθ is obtained according to the following equation.

[0022]

[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 as to further convert a value obtained by digitally converting the analog output of the piezoelectric vibration gyro 15 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.

[0024]

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

[0025]

## 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 292 and step 62 and subsequent steps are executed, and if it is not the positioning solution output timing, the gyro azimuth calculation routine ends. In step 292, bank correction is performed. This adjusts the control according to the amount of banks on the road surface. 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 interruption azimuth ψ1 is substituted for the azimuth ψ, and the azimuth ψra
Substituting interrupt direction ψ1raw into w, output value sum AD
Substitute the output value sum ADS1 for interrupt into S. Next, at step 69, 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 the altitude H [-3] 3 seconds before, the altitude H [-2] before 2 seconds, the altitude H [-1] before 1 second, and the current altitude H.

It has four output data of [0]. Here, the altitude H [-
2] to the altitude H [-3] three seconds before and the altitude H [-] one second before.
1] to the altitude H [-2] two seconds before, and the current 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, at step 70, the delay data of the output of the piezoelectric vibration gyro 15 is updated. The azimuth delay data of the piezoelectric vibrating gyro 15 includes azimuth ψ [-3] 3 seconds before, azimuth ψ [-2] before 2 seconds, azimuth ψ [-1] before 1 second, and azimuth ψ at the present time.

It has four output data of [0]. Here, 2
Set the azimuth ψ [-2] before 2 seconds to the azimuth ψ [-3] before 3 seconds 1
The azimuth ψ [-1] two seconds ago is changed to the azimuth ψ [-2] two 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 vehicle speed sensor 14
The delay data of the number of pulses during the fixed time of is updated. For the delay data of the pulse number, the pulse number P [-3], 3 seconds before,
Number of pulses 2 seconds before P [-2], number of pulses 1 second before P [-
1], current pulse number P

It has four output data of [0]. Here, the pulse number P [-2] 2 seconds before is changed to the pulse number P [-3] 3 seconds before and the pulse number P 1 second before is changed.
[-1] to the pulse number P [-2] two seconds before, and the current pulse number P

[0] is the number of pulses P [-1] one second before, and the output of the altitude sensor 16 is the number of pulses P at the present time.

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.

[0029]

## 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. A is a planning matrix for positioning.

[0031]

[Table 1]

As shown in FIG. 28, (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.

[0034]

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

[0038]

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

[0041]

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

The information obtained from one satellite, the direction ψ obtained from the piezoelectric vibration gyro and the integrated 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 is a 1σ error amount that does not depend on the satellite arrangement and is corrected according to the vehicle speed V) is obtained. Next, step 12
At 6, calculate σ _m ² for each satellite based on the following equation.

[0048]

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.

[0051]

Σ _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.

[0053]

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

[0055]

[Equation 10] ψ _T = ψ−ψ error

[0056]

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

[0058]

Σ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 given 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.

[0060]

[Equation 13]

[0061]

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

[0062]

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

[0063]

[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) are calculated.
From this, the matrix ΣY _{i + 1} indicating the predicted value of the estimation error is calculated according to the equation (17).

[0065]

Σ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

[0066]

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

[0067]

Σ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 estimated 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.

[0071]

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.

[0074]

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 equation 22.

[0075]

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: Step 16 by obtaining an index that calculates the effect of one satellite on positioning from the internal state of each satellite)
At 8, it is determined whether SVACC is 0 or more and less than 7 . If SVACC is 0 or more and less than 7 , step 1
70. User Range Accuracy URA (User Ran
ge Accuracy) is calculated by the equation 23. SVACC is 7
If so, the user range accuracy URA is obtained in step 169 by the equation (24).

[0079]

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

[0080]

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

[0081]

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.

Next, at step 297, it is determined whether or not the vehicle speed V is within the range of 0 to a predetermined value Vconst.
Here, Vconst applies the maximum vehicle speed. In step 297, if the vehicle speed V is within the range, the variable a is calculated according to the equation 26 in step 298. If the vehicle speed V is out of the range, the variable a is set to 1 in step 299.
And

[0083]

[Mathematical formula-see original document] a = (1-n) V / Vconst + n Then, in step 300, the UERE is multiplied by the variable a.
Update RE. After that, this routine ends. By the processing of steps 297 to 300, UARE is basically set as in the following equation.

[0084]

(27) UARE = ((1-n) V / Vconst +
n) × UERE 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, 175, and 176 are performed according to the correction mode.
Any one of the mode-1, mode 0, mode 1 and mode 2 routines is executed and this routine is terminated.

Here, the correction is the first one which is successively passed through 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 corresponding to 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 [0] to the variable col1
And Next, at step 213, this angle difference is adopted.
Determine whether or not. If adoption is not OK, this routine ends. If adopted, φ1 at step 214
Φ0raw is subtracted from raw to make raw1. Next, in step 215, col1 and raw1 are each divided by T10 and updated. Next, in step 216, the correction mode is changed to mode 2, in step 217 the timer Tφ is reset to 0, and in step 218 the counter count1 and counter
Set count2 to 0 and terminate this routine.

As described above, in the mode 1 routine, the start condition in the mode-1 (the vehicle speed is the lower limit value or more, 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 flowchart 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 orientation is not continuous, and the correction mode remains in Mode 2 Te Step 2 25, the counter in step 238 coun
The t2 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.

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 it is OK to adopt, subtract φ1 raw from φ2 raw in step 236.
Raw2 is set, and col2 and raw2 are each divided by T21 and 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 smaller, counter at step 238
The count2 is set to 0, and this routine ends. If it is larger, step 239 is executed. In step 239, co
l1 and smaller than the value which the absolute value is divided by T21 a predetermined upper limit value of col2 differences, and whether the absolute value of the difference raw1 and raw2 is less than the value divided by T21 a predetermined upper limit value 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 the lower limit value or more, 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.

[0095]

[Equation 28] 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 formula 29, and in step 258, the variable ROLL error is corrected based on the formula 30.

[0097]

[Equation 29]

[0098]

[Equation 30]

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 flow chart 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.

[0101]

[Equation 31]

[0102]

L _i ^G = [col1 col2] ^−T Furthermore, using the correction coefficients α and θ ₀ , the matrix X _i ^G is defined as follows.

[0103]

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, in 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 34.

[0105]

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

[0106]

[Equation 35]

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

[0108]

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

[0109]

[Expression 37] 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 (38).

[0111]

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

[0112]

[Formula 39]

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.

[0114]

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.

[0115]

Equation 41] _{^{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.

[0116]

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

[0117]

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 improvement amount to the Kalman gain based on the following equation. Add value and update.

[0118]

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 the vehicle speed = 0 and the sum of correction amounts is equal to or less than a predetermined lower limit value. If not, the zero point correction routine at V ≠ 0 is executed in step 293. This routine is completed, and if not negative, the routine 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 not, step 285 takes the average for 1 second at vehicle speed = 0, and if not, step 2
At 84, average all delay data. Then step 28
At 6, the maximum value and the minimum value of the sum for 1 second are obtained. In step 287, it is judged 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. Then step 28
Re-collection of delay data is designated in step 9, and step 290
Specify the correction mode to mode-1 in step 291.
Then, the correction amount sum is set to zero and the routine is finished.

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

[0122] FIG. 26, 0-point correction routine at the time of the V ≠ 0
It is a flowchart regarding . First, here, in steps 294 and 295, respectively, GP
It is determined whether S positioning is in progress and whether the vehicle speed is higher than a predetermined value VL. If the GPS positioning is being performed and the vehicle speed is higher than the predetermined value VL, the process proceeds to step 296, otherwise the process proceeds to step 304. In step 296, it is determined whether or not the previous bearing is stored.

If the previous azimuth is stored, it is determined in step 297 whether the previous azimuth is the same as the current azimuth. If the previous azimuth is not stored in step 296 and if the previous azimuth is not the same as the current azimuth in step 297, the process proceeds to step 298 and the current azimuth, which is the GPS azimuth at the present time, is stored as the previous azimuth.

Thereafter, the number of times is set to 1 in step 299. Then, it progresses to step 305. Step 29
If the previous bearing is the same as the current bearing in step 7, step 30
The number is incremented by 1 at 0 and the data is stored. Since this number is incremented by 1 when the previous azimuth and the current azimuth are the same, it indicates the number of times of the same azimuth in which the azimuth does not change. Next, in step 301, it is determined whether or not the number of times of the same azimuth is equal to or more than a predetermined number of times of request. Here, the predetermined number of requests is determined as follows. First, the predetermined vehicle speeds VL and V
U (however , VL <VU) and predetermined time TL, TU
(However , TL <TU) is defined. Where VL is GP
Set the vehicle speed at which the S positioning error increases, and VU is the vehicle
It is good to set the maximum vehicle speed of. Next, the time T is set according to the following equation according to the vehicle speed.

[0125]

T = TL + (TU-TL) × (V-VL) / (VU-VL) ... VL ≦ V ≦ V U T = TU ... VU <V At this time T, the vehicle speed is VL. Minimum time TL when equal to
And the maximum time TU when the vehicle speed is equal to VU
, And the vehicle speed during that time increases in proportion to the vehicle speed. The number of requests is obtained by dividing this time T by the interval at which this routine is executed.

In step 301, if the number of times of the same azimuth is smaller than the required number of times, the process proceeds to step 305, and if satisfied, the process proceeds to step 302. In step 302,
It is determined whether the calculated data is within a predetermined data range. If it is within the data range, the process proceeds to step 303 and a new 0 point is calculated. The data output from the gyro 15 is applied to this new zero point. Then step 304
The number of times is returned to 0 and the process proceeds to step 305.

If GPS positioning is not being performed at step 294, if the vehicle speed is lower than VL at step 295, and if it is not within the data range at step 302, the number of times is set to 0 at step 304. To do. Therefore, in these cases, the new zero point is not calculated.

In step 305, it is determined whether the sum of correction amounts is greater than 0 and the current 0 point is greater than the new 0 point. If the condition is satisfied, the process proceeds to step 307, and if not, the process proceeds to step 306. In step 306, it is determined whether the correction amount sum is less than 0 and the current 0 point is smaller than the new 0 point. If the condition is satisfied, the process proceeds to step 307, and if not, this routine is ended.

At step 307, the new zero point is substituted for the current zero point and updated. The correction amount sum is set in step 250 (FIG. 23) in the numerical correction routine, and is the cumulative difference between the GPS direction and the gyro direction. Step 3
In 05 and 306, it is determined whether or not the deviation direction of the correction amount is opposite to the correction direction of the 0 point, and the 0 point is updated only in the case of the opposite direction. For example, when the sum of correction amounts> 0, the gyro is considered to be shifted counterclockwise, so the 0 point should be corrected clockwise. If there is a contradiction in this situation, 0-point correction is not performed.

As described above, in the present embodiment, the gyro 15 for detecting the heading deviation of the vehicle, the step 56 which is a gyro heading calculating means for detecting the gyro heading from the output of the gyro, and a plurality of satellites are launched. An antenna 10, a GPS receiver 11, a GPS demodulator 12, which are azimuth measuring means for detecting the relative speed between each satellite and the vehicle from the signal, and measuring the reference azimuth of the vehicle, and a high vehicle speed and azimuth measurement. When the azimuth measured by the means does not change with time, the output signal of the gyro is set to zero, and the zero point correction means for correcting the zero point is provided, and steps 294 to 307 are provided. Therefore, the zero point correction of the gyro can be performed even while traveling.

Further, in step 301, the time used for changing the zero point is made longer as the vehicle speed becomes higher. Specifically, predetermined vehicle speeds VL and VU (where VL <VU) and predetermined times TL and TU (where TL <
TU), when the vehicle speed V is 0 <V <VL, no correction is performed, and when VL <V <VU, TL + (TU-TL)
When there is no change in the azimuth measured by the azimuth measuring means for the time indicated by × (V-VL) / (VU-VL), 0 point correction is performed, and when VU <V, the azimuth measurement is performed for the time indicated by TU or more. When there is no change in the azimuth measured by the means, the zero point is corrected.

Therefore, since the data of the azimuth measuring means having low accuracy at low vehicle speed is not used, the accuracy is not deteriorated. Further, when traveling on a gentle curve during high-speed traveling, zero-point correction is not performed, and therefore accurate correction can be performed.

Further, in steps 305 and 306, the direction of the difference between the azimuth measuring means and the azimuth obtained from the output of the gyro azimuth calculating means is opposite to the direction of the azimuth which is changed by correcting the zero point. The 0 point was corrected only at certain times. Therefore, the zero-point correction does not perform correction in directions in which the azimuths measured by the azimuth measuring means and the gyro azimuth calculating means are far apart from each other, so that more accurate correction can be performed.

[0134]

According to the first aspect of the invention, the gyro zero point correction can be performed even during traveling.

In this case , when the vehicle is traveling on a gentle curve during high speed traveling, the zero point correction is not performed, so that accurate correction can be performed.

According to the second aspect of the present invention, since the data of the azimuth measuring means having low accuracy at low vehicle speed is not used, the accuracy is not deteriorated. Further, when traveling on a gentle curve during high-speed traveling, zero-point correction is not performed, and therefore accurate correction can be performed.

[0137]

[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 a flowchart of a zero point correction routine when V ≠ 0 according to the embodiment of this invention.

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

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

[Explanation of symbols]

10 receiving antenna 11 GPS receiver 12 GPS demodulator 13 Display 14 vehicle speed sensor 15 Piezoelectric vibration gyro 16 Altitude sensor 17 Electronic control unit 18 I / O interface 19 Microcomputer 20 analog / digital converter

Continuation of the front page (56) References JP-A-2-38916 (JP, A) JP-A-4-62419 (JP, A) JP-A-6-26865 (JP, A) (58) Fields investigated (Int .Cl. ⁷ , DB name) G01C 19/00-19/72 G01C 21/00

Claims (2)

(57) [Claims] 1. A gyro for detecting an azimuth deviation of a vehicle; a gyro azimuth calculating means for detecting a gyro azimuth from an output of the gyro; a relative speed between each satellite and a vehicle is detected from signals emitted by a plurality of satellites; Azimuth measuring means for measuring the reference azimuth of the vehicle; if the speed of the vehicle is higher than the predetermined value VL and the azimuth measured by the azimuth measuring means does not change with time, the gyro output signal is set to zero and 0 is corrected. Point correction means; and the 0 point correction means changes the 0 point as the vehicle speed increases.
A gyro azimuth calculation device characterized in that the time used for doing is extended . 2. The zero point correction means according to claim 1.
Is a predetermined vehicle speed VL, VU (> VL), a predetermined time T
When L and TU (> TL) are set , 0 point correction is not performed when the vehicle speed V is 0 <V <VL. When VL ≦ V ≦ VU, T = TL + (TU-TL) ×
Before the time indicated by (V-VL) / (VU-VL)
0 points when the direction measured by the direction measuring means does not change
Corrected, when VU <V, more than the time indicated by T = TU
0 point correction when there is no change in the direction measured by the position measuring means
A gyro azimuth calculation device characterized by: