JP3412261B2 - In-vehicle positioning device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [0001] BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention is applied to a vehicle such as an automobile.
In-vehicle measurement that measures the position of the vehicle and informs the current position information
Position device. [0002] 2. Description of the Related Art For example, Japanese Patent Application Laid-Open No. 61-137,091
And JP-A-61-167886.
As in the Global Positioning System (Grob
al Positioning System (hereinafter referred to as GPS)
Conventionally, a technology of a positioning device for a vehicle has been proposed.
The GPS has a plurality of satellites. Satellites have accurate time,
Road function and data indicating the accuracy of information
It is transmitted to the ground at a predetermined timing. On the ground,
Determining satellite position from time and orbit functions obtained from stars
The difference between the time of the satellite and the time of the receiving point,
Based on the propagation delay time, the distance from the satellite to the receiving point
You can know. Each of the three different satellites
About the position of the satellite and the distance from the satellite to the receiving point.
In this case, by solving the ternary simultaneous equation,
The three-dimensional position of the receiving point can be determined. However,
Normally, the time at the reception point contains a relatively large error.
In order to compensate for the clock error at the receiving point,
Information of one satellite is needed. [0003] In addition, the altitude of the receiving point hardly changes.
Assuming that, from the information of the three satellites,
A two-dimensional position can be determined. [0004] However, GPS satellites that actually exist
The number of stars is limited, and cars
In the shadow of the building or inside, the radio wave is cut off, and three or four
It may be difficult to receive radio waves from one satellite.
Then, in the technology of JP-A-61-137909,
Using a fiber optic gyro to detect heading changes
And detects the distance traveled by the vehicle using a vehicle speed sensor.
Then, these detection information is supplementarily specified and transmitted from one satellite.
Enables vehicle position detection even when only waves can be received
are doing. Further, the technique disclosed in Japanese Patent Application Laid-Open No.
In the art, the magnetic compass and rangefinder mounted on the car are supplementary
To receive only radio waves from two satellites.
In this case, the position of the own vehicle can be detected. [0005] However, the gyro is not warm.
There is an effect of drift such as degree. Azimuth with drift
Drift is cumulatively added to the
It will increase with time. About the vehicle speed sensor
Also measures the rotation of the wheel's axis of rotation,
There is a high possibility that errors will occur due to wheel slip, etc.
The error of the integrated distance increases with time. There
Designed to reduce gyro drift,
Measure the absolute speed between the vehicle and the vehicle
It is conceivable to correct the wheel speed by finding the rate
However, the price of each sensor rises,
The cost goes up. Therefore, in the present invention, such a door is used.
A rotational angular velocity sensor or a car with a large error that is prone to lift
Even if a speed sensor is used, accurate positioning can be performed.
The task is to [0007] Means for Solving the Problems To solve the above problems,
According to the first aspect of the present invention, the position of the vehicle is detected.
Emitting position sensor; from signals emitted by multiple satellites
Satellite information detection that detects the relative position between each satellite and the vehicle
Means: less output of position sensor or satellite information detecting means
Positioning means for positioning the absolute position of the vehicle from one side; satellite
To correct the position detected by the position sensor based on information from the
Sensor output correction means;In the in-vehicle positioning device provided,
The azimuth correction means calculates the gyro azimuth and the satellite information.
A correction obtained by integrating the obtained difference in azimuth as a correction amount and integrating the correction amount
Sum, and when the sum of the correction amounts is small, the
Gyro heading change and satellite information per hour
Find the relationship with the azimuth change amount and calculate the azimuth angle calculation means
When the constant is corrected and the sum of the correction amounts is large,
Gyro heading change per interval and satellite information per time
Kalman filter for the relationship with the azimuth change amount obtained from the report
To correct the calculation constant of the azimuth angle calculation means.
It was to so. [0008] [0009] [0010] [0011] [0012] [0013] [0014][Action] Claim 1 According to the invention described in the above, the gyro is
Detects the azimuth deviation of the vehicle. The vehicle speed sensor measures the speed of the vehicle.
To detect. Relative speed detection means Signals fired by multiple satellites
, The relative speed between each satellite and the vehicle is detected. One
The azimuth calculating means calculates the azimuth of the vehicle from the output of the gyro.
You. The integrated distance calculation means calculates the product of the vehicle from the output of the vehicle speed sensor.
Find the arithmetic distance. The positioning means is the azimuth of the vehicle, the total distance,
Positioning is performed using any of the relative velocities with the satellite. Bearing
The angle correction means is based on the information from the satellite.
Correct the calculation constant. The integrated distance correction means uses information from the satellite.
The calculation constant of the integrated distance calculation means is corrected based on the information. Yo
Gyro and wheel slip which drift occurs
High accuracy even when using a vehicle speed sensor where the distance is more likely to go wrong
Positioning is possible and inexpensive sensors can be used. [0015] DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS The present invention will be described below with reference to the drawings. Figure
1 shows an embodiment of an in-vehicle positioning device. This device is
Mounted on a moving vehicle, receiving antenna 10, GPS reception
Device 11, GPS demodulator 12, display device 13, vehicle speed sensor
14, a piezoelectric vibrating gyroscope 15, an altitude sensor 16,
A child control unit 17 is provided. From each GPS satellite
An electric wave of 1.57542 GHz is transmitted. GPS receiver
Is the transmitter 11 a GPS satellite via the receiving antenna 10?
Receive these radio waves. The received signal is a GPS demodulator 12
Sent to The GPS demodulator 12 receives information on radio waves,
In other words, the function indicating the orbit of the satellite, the time, and the accuracy of the information are indicated.
The demodulated code is output to the electronic control unit 17.
The vehicle speed sensor 14 is pulsed according to the rotation of the vehicle wheel shaft.
A signal is output to the electronic control unit 17. car
The speed sensor 14 is, for example, an automobile transmission.
Around the output shaft or the shaft that rotates synchronously with it
Magnet and a lead switch provided near this magnet.
The reed switch is turned off as the wheels rotate.
It may be configured to turn on and off. Piezoelectric vibration gyro 1
5 is fixed to the car, centered on the vertical axis of the car
Analog signal with a level proportional to the rotational angular velocity ω
This is output to the electronic control unit 17. Piezoelectric vibration
The gyro 15 can output a voltage of 0 to 5 volts.
2.5 volts around the center, 2.5 volts for clockwise rotation
In the case of left rotation, a signal of 2.5 volts or less
Has been adjusted to output. The altitude sensor 16 is
An analyzer whose level changes in response to pressure changes, that is, altitude changes
A log signal is output to the electronic control unit 17.
You. The display device 13 displays the latitude, longitude, and altitude of the vehicle.
Etc. are displayed, and the electronic control unit 17
Receives and displays information. The electronic control unit 17 includes a microcomputer.
Computer 19, input / output interface (I / O) 18,
And an analog / digital converter (A / D converter) 20
Prepare. The input / output interface (I / O) 18
PS receiver 11, GPS demodulator 12, and vehicle speed sensor 1
4 to the microcomputer 19
The information from the microcomputer 19 is displayed on the display 1
Send to 3. Analog / digital converter (A / D converter)
20 is from the piezoelectric vibrating gyroscope 15 and the altitude sensor 16
A / D conversion of analog signals into digital signals
To the computer 19. Signal from vehicle speed sensor 14
Is connected to the external interrupt terminal of the microcomputer 19
Have been. The microcomputer 19 operates as shown in FIG.
It works along the chart. 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. interrupt
Is a timer interrupt that is executed at regular intervals and an external
An external interrupt executed in response to a change in the level of the interrupt pin
Set After this, the main routine will be executed at regular intervals.
The gyro direction calculation in step 54.
Execute the exit routine. In this embodiment, the discount is made every 10 ms.
Entrenchment. Also, the level of the external interrupt pin is
The main routine process is temporarily interrupted every time the
The integrated distance calculation routine of step 55 is executed. Initialization
When the step ends, demodulate 3 or more satellites in step 31
It is determined whether the process has been completed. Here, the demodulated satellite
If the number is more than two, the navigation equation 1
The navigation equation is set in the routine and the positioning
The positioning error is calculated by the constant error calculation routine. Next
In step 34, the positioning estimation error is set in advance.
Judge whether it is smaller than the upper limit (for example, 300 m)
You. If the positioning estimation error is smaller than the upper limit, step 35
Positioning is performed by the positioning routine of step 36, and the altitude
The altitude is corrected in the normal routine, and
Gyro and vehicle speed are corrected by the gyro correction routine
U. Thereafter, in step 38, the current position information (latitude, longitude,
(Degree, altitude, etc.) to the display device 13. next,
Determine whether gyro direction can be displayed in step 39
I do. If the gyro heading can be displayed, step 40
Outputs the gyro orientation to the display device 13. So
After that, in the gyro zero point correction routine of step 41,
The gyro is corrected, and the process returns to step 31. In step 31, if the number of satellites that can be demodulated is
If it is below, or the positioning estimation error is the upper limit in step 34
If it is larger, demodulation is completed for two or more satellites in step 42.
Is determined. Here, the number of demodulated satellites is
If 2, the azimuth estimation error is set in advance in step 43
Judge whether it is smaller than the upper limit (for example, 30 °)
I do. As will be described later, the azimuth estimation error
It is calculated in the gyro heading calculation routine. This direction
If the fixed error is smaller than the upper limit, the navigation method in step 44
The navigation equation is set by the equation 2 routine, and step 45 is performed.
Calculate the positioning estimation error with the positioning estimation error calculation routine
You. Next, in step 46, the positioning estimation error is set in advance.
It is determined whether it is smaller than the upper limit (for example, 300 m).
Refuse. If the positioning estimation error is smaller than the upper limit,
Positioning is performed in the positioning routine of step 47, and step 36
Altitude correction is performed by the altitude correction routine. Then
Jump to step 38. At step 42, one or more satellites that can be demodulated
In the case of below, or the azimuth estimation error is the upper limit in step 43
If it is larger, or if the positioning estimation error
If it is larger than the upper limit, the azimuth estimation error is predicted in step 48.
Is smaller than the set upper limit (for example, 30 °)
Judge. If the azimuth estimation error is smaller than the upper limit,
Calculate the accumulated distance estimation error STEPerror at step 49
I do. The accumulated distance estimation error STEPerror
Number of pulses within a certain time captured in the distance calculation routine
And the rotation error ROLLerro obtained by the numerical correction routine described later.
Multiply by r. In step 50, the integrated distance estimation
The error STEPerror is a preset upper limit (for example, 10
0m) is determined. Incorrect estimation of total distance
If the difference STEPerror is smaller than the upper limit, the step
Set the navigation equation with 51 navigation equation 3 routines,
Positioning estimation by the positioning estimation error calculation routine in step 52
Calculate the error. Next, in step 53, the positioning estimation error
The difference is smaller than a preset upper limit (for example, 300 m).
It is determined whether or not. Positioning error is smaller than upper limit
If so, it jumps to the positioning routine of step 47. In step 48, the azimuth estimation error exceeds the upper limit.
If it is large, or if the accumulated distance estimation error
If it is larger than the upper limit, or if positioning estimation error
If the difference is greater than the upper limit, jump to step 41. The details of each subroutine will be described below. FIG. 3 is a flowchart of a gyro heading calculation routine.
It is a chart. Here, first, in step 56
The position deviation Δθ is obtained according to the following equation. [0024] ## EQU1 ## Δθ = Δθ + ADA (ADG-ADC) Here, ADG is an analog output of the piezoelectric vibrating gyroscope 15.
Is the value obtained by digitally converting
Is a number. The coefficient ADC is the analog when the rotational angular velocity ω is zero.
The value obtained by digitally converting the log voltage value (2.5 volts)
Has been set. The coefficient ADA is calculated as follows:
Convert the value obtained by digitally converting the analog output into an angle
It is a value set as follows. Formula 1 is calculated by timer
Δθ is calculated per unit time
Is shown. Next, at step 57, the vehicle speed becomes zero.
It is determined whether the vehicle speed is zero or not.
Updates the gyro heading error turn based on Equation 2, and
In step 59, the gyro offset value offset is counted.
Update based on equation (3). If the vehicle speed is zero at step 57,
If not, skip steps 58 and 59 and proceed to the next step. [0026] ## EQU2 ## turn = turn + ADAerror (ADG-ADC) [0027] [Equation 3] offset = offset + ADCerror Here, ADAerror is the slope of the output of the piezoelectric vibrating gyroscope.
ADCerror is the conversion coefficient representing the deviation of
This is a conversion coefficient indicating a deviation from a gyro zero point. Next, at step 60, the reference direction is valid.
It is determined whether or not there is. If the reference direction is valid,
Proceed to step 62. If the reference direction is not valid, display error
Set a value equal to or larger than the difference upper limit to the azimuth estimation error
To Next, at step 64, the piezoelectric vibrating gyro
The digital output value A is added to the fifteen interrupt output value sum ADS1.
DG is added and interrupt output value sum ADS1 is updated.
Is done. Next, in step 65, the timing of outputting the positioning solution
It is determined whether or not. This timing is 1 second in this embodiment.
And every time. If it is a positioning solution output timing,
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 62
Is calculated by adding the azimuth deviation Δθ to the azimuth
The direction for insertion # 1 is updated. Next, in step 63
Azimuth deviation Δθ is added to azimuth ψ1raw for interrupt
Thus, the interrupt direction # 1raw is updated. Next,
At step 66, the azimuth estimation error calculation routine is executed,
Find the position error. Next, at step 67, the interrupt
The correction amount is added to the position # 1. Next, in step 68, the bearing
Substitute the interrupt direction 割 り 込 み 1 into ψ and divide into the direction ψraw
Ψ1raw is substituted for the output value sum ADS.
The output value sum ADS1 for embedding is substituted. 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 the altitude 3 seconds ago.
H [-3], altitude H 2 seconds ago, H [-2], altitude H 1 second ago
[-1], the four output data of the current altitude H [0]
Have. Here, the altitude H [-2] 2 seconds ago is 3 seconds
Altitude H [-1] one second before to H [-3] before 2 seconds
One second before the current altitude H [0] to the previous altitude H [-2]
The altitude H [-1], the output of the altitude sensor 16 is
The altitude is updated to H [0]. Next, step 7
Update the output delay data of the piezoelectric vibrating gyroscope 15 with 0
I do. The delay data of the direction of the piezoelectric vibrating gyroscope 15 includes:
Azimuth 3 seconds ago [-3], Azimuth 2 seconds ago [-2], 1
Four seconds, the azimuth −１ [-1] two seconds ago and the azimuth ψ [0]
It has output data. Here, the direction 2 seconds ago
[−2] is changed to azimuth 3 seconds ago [−3] to azimuth 1 second before ψ
[-1] is changed to the azimuth two seconds ago {[-2], the current azimuth}
[0] is obtained in step 63 in the azimuth ψ [-1] one second before.
Is updated to the current azimuth ψ [0].
You. Next, at step 71, the vehicle speed sensor 14
Update the delay data of the middle number of pulses. In the delay data of the number of pulses, the pulse 3 seconds before
Pulse number P [-3], pulse number 2 seconds before P [-2], 1 second before
Of the current pulse number P [-1] and the current pulse number P [0]
It has two output data. Here, the pal 2 seconds ago
Pulse number P [-2] to pulse number P [-3] 3 seconds before, 1 second
The previous pulse number P [-1] is changed to the pulse number P [-2] two seconds ago.
In addition, the current pulse number P [0] is changed to the pulse number P one second before.
In [-1], the output of the altitude sensor 16 is
P [0] is updated. Step 66 ends
Then, the gyro azimuth calculation routine ends. FIG. 4 is a flowchart of a direction estimation error calculation routine.
It is a chart. Here, first, in step 72,
Determine whether the flag sign is 1 and if the flag sign is 1,
If the flag sign is not 1, the step 73 is executed.
Execute 74. This flag sign indicates that the vehicle is turning right
Or the counterclockwise rotation. The initial value is 1.
It has been set. In step 73, the azimuth deviation RL is gyrated.
Add the azimuth error turn. In step 74, the azimuth deviation
The gyro azimuth error turn is subtracted from RL. Next,
In step 75, it is determined whether the azimuth deviation RL is above zero,
If it is not greater than zero, at step 76 the azimuth deviation RL
The sign is inverted, and the state of the flag sign is changed in step 77.
Update (change to 0 if 1; change to 1 if 0). next
In step 78, it is determined whether or not the azimuth absolute value is corrected.
If the azimuth absolute value is corrected, it is turned off in step 79
Substitute GPS estimation error GPSerror for set value offset
Then, in step 80, the rotation direction signal RL is set to zero.
Then, in step 81, the azimuth deviation estimation error Δθ error
Is calculated according to Equation 4 and the azimuth estimation error calculation routine is terminated.
Complete. This process takes into account the offset error
The azimuth estimation error Δθerror is obtained. [0032] [Equation 4] Δθ error = RL + offset FIG. 5 is a flowchart of the integrated distance calculation routine.
Here, first, in step 82, the pulse during a certain time
Capture numbers. Next, in step 83, the power during
Multiply the number of looses by the variable ROLL to obtain the total distance STEP
You. The variable ROLL is the effective radius of the tire and the output of the vehicle speed sensor 14.
The variable obtained by taking into account the force characteristics
This is for converting the number of threads into a distance. Step 83
Is completed, the integrated distance calculation routine ends. FIG. 6 is a flowchart of the navigation equation 1 routine.
It is. Here, first, at step 84, four satellites
It is determined whether demodulation has been completed. 4 or more satellites demodulated
If so, at step 86, equation 1 in Table 1 is set.
At step 89_m ^TwoRun the calculation routine
And σ_m ^Two(Σ_m ^TwoIndicates the prediction error of each satellite._mIs demodulated
Indicates the number of satellites that have been terminated.
Each σ₁ ^Two, Σ_Two ^Two, Σ_Three ^Two, Σ_Four ^TwoIs calculated)
Is obtained, in step 93, according to the determinant 1 in Table 2.
Matrix Σε that indicates the prediction error of the measured value_{i + 1}(_iUntil last time
Indicates the number of times of positioning._{i + 1}Is the positioning this time. )
Then, this routine ends. Where the navigation equation is
AX = L. A is L_m, M_m, N_m(
_mIs the number of demodulated satellites_m= 1 to 4)
The matrix of 4, X is [Δx Δy Δz cΔt]^T, L is
It is represented by a 1 × 4 matrix. [0034] [Table 1] As shown in FIG. 27, (L_m, M_m,
N_m) Is the previous position from the m-th satellite (the last position
3 shows a vector heading to a positioning point). This vector is a satellite
From information from (Δx, Δy, Δz)
The vector from the position to the positioning point is shown. Δt is last time
Time difference between the time at
Show. c indicates the speed of light. Here, demodulation is completed for four or more satellites
So_m= 1 to 4 satellites to previous position
The matrix A is represented using the elements of the heading vector.
The matrix L is [ΔL1 ΔL2 ΔL3 ΔL4]^TRepresented by
You. These ΔL1, ΔL2, ΔL3, and ΔL4 are the first to fourth, respectively.
L between the satellite and the positioning point_m(_m= 1 ~ 4) to 1
Distance L between the fourth satellite and the previous position_0mDistance subtracted
Indicates separation. By solving this navigation equation 1, four
Three-dimensional relative positions (Δx, Δ
y, Δz) can be obtained. In FIG. 6, demodulation ends at step 84.
If the number of satellites obtained is 3, step 85 is executed.
In step 85, it is determined whether there is altitude data. High
If there is degree data, at step 87 equation 2 in Table 1
At step 90._m ^TwoExecute the calculation routine
And σ for three satellites₁ ^Two, Σ_Two ^Two, Σ_Three ^TwoSeeking
At step 92_a ^TwoExecute the calculation routine to calculate σ
_a ^Two(Σ_a ^TwoIndicates the prediction error for altitude. )
Then, in step 94, measurement is performed according to the determinant 2 in Table 2.
Matrix Σε indicating the prediction error of the value_{i + 1}Establishing this root
End [0037] [Table 2] Here, the navigation equation 2 is the same as the navigation equation 1
Similarly, it is expressed in the form of AX = L. Here, 3
Since the stars have been demodulated,_m= 3 or 3 satellites
Matrix A is expressed using the elements of the vector
Have been. The matrix L is [ΔL1 ΔL2 ΔL3 Δ
h]^TIt is represented by This Δh is obtained by the altitude sensor 16.
This shows the difference between the measured positioning altitude and the previous position altitude. this
Obtained from 3 satellites by solving navigation equation 2
3D relative from information and altitude difference Δh obtained from altitude sensor
The position (Δx, Δy, Δz) can be obtained. In step 85, if no altitude data exists
Then, in step 88, equation 3 in Table 1 is set,
In step 91, σ_m ^TwoExecute the calculation routine and
Σ for satellite₁ ^Two, Σ_Two ^Two, Σ_Three ^TwoAfter asking
In step 95, the prediction error of the measured value is
Matrix Σε indicating the difference_{i + 1}And terminate this routine.
You. Here, navigation equation 3 is similar to navigation equation 1,
AX = L. Here, three satellites are demodulated
Because it is finished,_m= Previous ranking from 3 satellites of 1-3
Matrix A is represented using the elements of the vector
You. The matrix L is [ΔL1 ΔL2 ΔL30]^TRepresented by
It is. It is assumed that the difference Δz in the height direction is zero. This voyage
Information obtained from three satellites by solving the equation 3
The two-dimensional relative position (Δx, Δy) can be obtained from the report
Wear. FIG. 7 is a flowchart of the navigation equation 2 routine.
It is. Here, first, at step 96, the altitude data
It is determined whether there is data. If you have altitude data,
In step 97, equation 4 in Table 1 is set, and step 9
At 9_m ^TwoRun the calculation routine to target two satellites
Σ₁ ^Two, Σ_Two ^Two, And in step 101, σ_a ^TwoTotal
Execute the calculation routine and_a ^TwoAnd go to step 102
And σ_G ^TwoExecute the calculation routine to calculate σ_G ^Two(Σ_G ^TwoIs piezoelectric
4 shows a prediction error of an output of a vibration gyro. ), Then
In step 104, the measured values are calculated according to the determinant 4 in Table 2.
Matrix Σε indicating prediction error_{i + 1}And define this routine
finish. Here, since demodulation has been completed for two satellites,
_m= Vector heading from two satellites 1-2 to previous position
And the matrix A is represented using Gx and Gy.
You. The following relationship exists between Δx and Δy and Gx and Gy:
is there. [0041] (Equation 5)   Δy / Δx = sin (180 ° -ψ) / cos (180 ° -ψ)             = Gx / Gy The matrix L is [ΔL1 ΔL2 Δh 0]^TRepresented by
You. By solving Navigation Equation 4 using Equation 3,
Information obtained from two satellites, one obtained from a piezoelectric vibrating gyro
3D relative from altitude difference Δh obtained from position ψ and altitude sensor
The position (Δx, Δy, Δz) can be obtained. If there is no altitude data in step 96,
At step 98, equation 5 in Table 1 is set.
Σ at 100_m ^TwoExecute the calculation routine and two satellites
Σ for₁ ^Two, Σ_Two ^TwoAt step 103.
_G ^TwoExecute the calculation routine to calculate σ_G ^TwoAfter asking
In step 105, the prediction of the measured value is incorrect according to the determinant 5 in Table 2.
Matrix Σε indicating the difference_{i + 1}And terminate this routine.
You. Here, since demodulation has been completed for two satellites,_m= 1
Elements of the vector heading from the two satellites to the previous position
And Gx and Gy to represent the matrix A. Ma
The matrix L is [ΔL1 ΔL2 0 0]^TIt is represented by High
It is assumed that the vertical difference Δz is zero. Using Equation 3
Obtained from two satellites by solving navigation equation 5
Information and two-dimensional phase from direction ψ obtained from piezoelectric vibrating gyroscope
The pair position (Δx, Δy) can be obtained. FIG. 8 is a flowchart of the navigation equation 3 routine.
It is. Here, first, at step 106,
It is determined whether demodulation has been completed for stars or more. Demodulate one or more satellites
If so, step 107 is performed. Step
In step 107, it is determined whether there is altitude data. Altitude
If there is data, the equation 6 in Table 1 is
And set σ in step 113_m ^TwoExecute the calculation routine
And σ for one satellite₁ ^TwoAnd step 11
Σδ at 5_x ^Two, Σδ_y ^TwoExecute the calculation routine to calculate σδ
_x ^Two, Σδ_y ^Two(Σδ_x ^Two, Σδ_y ^TwoIs the vehicle speed sensor and pressure
X-coordinate of two-dimensional position obtained from output of electric vibratory gyro and
And the prediction error of the Y coordinate. ), And step 119
At σ_a ^TwoExecute the calculation routine to calculate σ_a ^TwoAfter asking
In step 121, the measured values are calculated according to the determinant 6 in Table 2.
Matrix Σε indicating prediction error_{i + 1}And define this routine
finish. Here, since one satellite has been demodulated,
_m= Vector required from one satellite to the previous position
The matrix A is represented using primes. The matrix L is [Δ
L1 δ_x  δ_y  Δh]^TIt is represented by Δ_xPassing
And δ_yIs calculated by the following equation.
Required in [0044] (6)_x= STEP · cos (180-ψ) δ_y= STEP · sin (180-ψ) Solving navigation equation 6 using equation 6 gives one
Information obtained from satellites of Japan, azimuth obtained from piezoelectric vibrating gyroscope
ψ, the integrated distance STEP obtained from the vehicle speed sensor, and the altitude sensor
The three-dimensional relative position (Δx, Δ
y, Δz) can be obtained. If there is no altitude data in step 107
In step 110, equation 7 in Table 1 is set at step 110,
In step 114, σ_m ^TwoExecute the calculation routine and get one
Σ for satellite₁ ^Two, And in step 116 σδ
_x ^Two, Σδ_y ^TwoExecute the calculation routine to calculate σδ_x ^Two, Σδ
_y ^Two, The determinant 7 in Table 2 is obtained in step 122.
A matrix Σε indicating the prediction error of the measured value according to_{i + 1}Determined
Then, this routine ends. Here, one satellite is demodulated
Because it is finished,_m= 1 satellite to previous position
The matrix A is represented using the elements of the heading vector.
The matrix L is [ΔL1 δ_x  δ_y  0]^TRepresented by
You. It is assumed that the difference Δz in the height direction is zero. Solving the navigation equation 7 using the equation (6)
Therefore, information obtained from one satellite, piezoelectric vibration gyro
Distance obtained from the direction ψ obtained from the vehicle speed sensor
From the two-dimensional relative position (Δx, Δy)
You. Number of satellites demodulated in step 106
If not, step 108 is performed. Step 1
At 08, it is determined whether or not there is altitude data. Altitude data
In step 111, equation 8 in Table 1 is set.
At step 117_x ^Two, Σδ_y ^TwoCalculation roux
Run chin and σδ_x ^Two, Σδ_y ^Two, Step 1
20 at σ_a ^TwoExecute the calculation routine to calculate σ_a ^TwoSought
Then, in step 123, measurement is performed according to determinant 8 in Table 2.
Matrix Σε indicating the prediction error of the value_{i + 1}Establishing this root
End Here, no satellite has been demodulated
Therefore, the matrix A is represented by a unit matrix. Also, the matrix L
Is [δ_x  δ_y  Δh 0]^TIt is represented by Use Equation 6
By solving the navigation equation 8, the piezoelectric vibration gy
B) azimuth obtained from b, integrated distance STE obtained from vehicle speed sensor
3D relative from P and altitude difference Δh obtained from altitude sensor
The position (Δx, Δy, Δz) can be obtained. If there is no altitude data in step 108
In step 112, equation 9 in Table 1 is set in step 112,
In step 118, σδ_x ^Two, Σδ_y ^TwoCalculation routine
Run and σδ_x ^Two, Σδ_y ^Two, And step 12
4 shows the prediction error of the measured value according to determinant 9 in Table 2.
Matrix Σε_{i + 1}Is determined, and this routine ends. This
Here, since no satellite has been demodulated, the matrix A is simply
It is represented by a rank matrix. The matrix L is [δ_x  δ_y  0 0]^T
It is represented by It is assumed that the difference Δz in the height direction is zero.
By solving the navigation equation 9 using the equation 6, the piezoelectric
Azimuth か ら obtained from vibrating gyroscope and product obtained from vehicle speed sensor
Find two-dimensional relative position (Δx, Δy) from arithmetic distance STEP
Can be FIG._m ^TwoCalculation Routine Flowchart
It is. Here, first, at step 125, UER
Execute the E calculation routine, and among the i = 1 to 4th satellites,
URE of each satellite that has finished demodulation (UERE is
Indicates the 1σ error amount regardless of the arrangement of stars. ). Next
In step 126, the following equation is obtained for each satellite.
Based on_m ^TwoAsk for. [0051] [Equation 7]_m ^Two= UERE^Two After this calculation, this routine ends. FIG._a ^TwoCalculation routine flow chart
It is. Here, first, at step 127, the error
URE and VDOP (VDOP is an error for altitude
Shows the difference index. ) Is determined. U for error
If ERE and VDOP are present, at step 128 (V
DOP × UERE)^TwoTo σ_a ^TwoAnd assign this to
End ERE and VDOP for error must be present
In step 129, a predetermined constant const^TwoTo σ_a
^TwoAnd terminates this routine. FIG._G ^TwoCalculation routine flow chart
It is. Here, first, in step 130,
The distance STEP is read from the memory. Then, step
The azimuth calculated by the azimuth estimation error calculation routine at 131
The deviation estimation error Δθerror is extracted, and the
Σ_G ^TwoIs calculated. [0054] (Equation 8) σ_G ^Two= (N × STEP × sinΔθerror)^Two Here, n is a constant. After doing the above calculations,
End FIG._x ^Two, Σδ_y ^TwoCalculation routine
It is a flowchart of FIG. Here, first, Step 1
E @ 33_x, Eψ_yBy executing the calculation routine, Eψ
_xAnd Eψ_y(Eψ_xAnd Eψ_yIs a piezoelectric vibrating gyroscope
The error of the X coordinate and the Y coordinate is shown. ). Next
In step 134, the accumulated distance estimation error STEP
Find r. The accumulated distance estimation error STEPerror is
Pal within a certain period of time captured in the integrated distance calculation routine
Number of rotations and the rotation error ROLL obtained by the numerical correction routine described later.
Ask for error. Next, step 135 and step
136 based on the following equation._x ^TwoAnd σδ_y ^TwoTotal
Calculate. [0056] ## EQU9 ## σδ_x ^Two= (Eψ_x+ STEPerror · cos
(180 ° -ψ)^Two σδ_y ^Two= (Eψ_y+ STEP error · sin (180
° -ψ))^Two Where ψ is the azimuth obtained in the gyro azimuth calculation routine.
You. After performing the above calculation, this routine ends. FIG. 13 shows Eψ_x, Eψ_yCalculation routine
It is a low chart. Here, first, step 137
Takes out the bearing ψ obtained by the gyro bearing calculation routine at
You. Next, at step 138, the orientation estimation error calculation routine
The azimuth estimation error で error obtained by the Next,
Integrated distance S obtained by the integrated distance calculation routine in step 139
Take out the TEP. Next, in step 140, the formula 10
By_TAsk for. Next, steps 141 and 142
Eψ based on Equation 11_xAnd Eψ_yIs calculated. [0058] [Equation 10]_T= Ψ-ψerror [0059] [Equation 11]_x= STEP [cos (180 ° -ψ)
-Cos (180 ° -ψ_T)] Eψ_y= STEP [sin (180 ° -ψ) -sin
(180 ° -ψ_T)] After performing the above calculation, this routine ends. FIG. 14 is a flowchart of a positioning estimation error calculation routine.
It is a chart. Here, first, step 143
To determine whether there is a reference heading.
The process proceeds to step 145, and if not, the process proceeds to step 144. Stay
In step 144, it is determined whether or not it is the first positioning. First time
Step 145 is executed if the positioning is
If the positioning is performed, step 146 and subsequent steps are executed. Steps
At 145, based on the following equation, ΣX₁Is calculated. [0061] 数 X₁= (A₁ ^TΣε₁ ^-1  A₁)^-1 Where A₁Is the navigation equation at the time of the first positioning
Is the matrix A on the left side of Σε₁Table 2 at the time of the first positioning
Matrix Σε indicating the prediction error of the measured value obtained from the determinant of
_{i + 1}(I = 0). ΣX_{i + 1}Is the following equation
Is a matrix representing the error defined by₁Is once
It is at the time of eye positioning. In step 156, the number
Σ found from Equation 12 and Equation 13_x ^TwoAnd σ_y ^TwoFor
Then, the positioning estimation error is calculated in accordance with Equation 14, and
End the routine. [0063] (Equation 13) [0064] ## EQU14 ## Positioning estimation error = (σ_x ^Two+ Σ_y ^Two)^1/2 In the second and subsequent positioning, first, in step 146,
To read the total distance STEP, and
Read the position. Next, in step 148, the formula 6
Therefore δ_xAnd δ_yAsk for. Next, in step 149
Matrix Y according to equation 15_{i + 1}Is defined. [0065] [Equation 15] Y_{i + 1}= [Δ_x  δ_y  0 0]^T Next, at step 150, the aforementioned σδ_x ^Two, Σδ_y ^Two
Execute the calculation routine and calculate σδ_x ^TwoAnd σδ_y ^TwoGet.
Next, in step 151, σ_a1 ^TwoTo the constant const^TwoSet to
You. Next, in Step 152, as shown in Expression 16,
A matrix ΣUi indicating an expected error model is defined. [0066] (Equation 16) Next, at step 153, one time before (i times
Eye) estimated error matrix ΣX_i, Matrix ΣU_i
From the matrix ΣY indicating the estimated value of the estimation error_{i + 1}Equation 17
Calculate according to [0068] [Expression 17]_{i + 1}= ΣX_i+ ΣU_i Next, at step 154, a matrix indicating the Kalman gain
Kg is calculated according to equation (18). Where A_{i + 1}Is a table
Set in each of the navigation equations 1 to 3 routine based on 1.
Is the matrix on the left side of Equations 1 to 9,_{i + 1}Is Table 2
Are the matrices respectively defined based on. [0069] [Expression 18] Kg = ΣY_{i + 1}A_{i + 1} ^T[Σε_{i + 1}+ A_{i + 1}
ΣY_{i + 1}A_{i + 1} ^T]^-1 Next, in step 155, the current estimation is performed based on equation (19).
Constant error matrix ΣX_{i + 1}Ask for. Where I is a 4 × 4 unit
It is a rank matrix. [0070] [Equation 19] ΣX_{i + 1}= 2 · [I-Kg · A_{i + 1}] ΣY_{i + 1} Thereafter, in step 156, equation (19) is calculated and equation (13) is obtained.
Σ obtained by applying the formula_x ^TwoAnd σ_y ^TwoUsing the number
Calculate the positioning estimation error according to equation (14)
To end. The estimated error matrix ΣX in this routine_{i + 1}of
The calculation method is an optimal filter called the Kalman filter
The Kalman filter for which the positioning solution is
Luther's error variance / covariance matrix (ΣX_{i + 1}) 1
One component (σ_x ^Two) And 22 components (σ_y ^Two) And the square root
did. If necessary, 33 components (σ_z ^Two) Plus square
It may be the root. FIG. 15 is a flowchart of a positioning routine.
is there. Here, first, in step 157, the second and subsequent times
It is determined whether or not it is positioning. If it is the first positioning,
Step 161 is executed, and if the positioning is the second or subsequent positioning, the step is performed.
158 to 160 are executed. In step 161, X is calculated based on the following equation.₁Total
Calculate. [0074] (Equation 20)₁= A₁ ^-1L₁ Where A₁Is the navigation equation at the time of the first positioning
Is the matrix A on the left side of L₁Is the navigation method at the time of the first positioning
This is the matrix L on the right side of the equation. X₁Is the time of the first positioning
Shows the solution sought in. In step 162
Transforms the obtained solution (XYZ coordinates) into latitude, longitude and altitude.
Then, this routine ends. At step 157, the second and subsequent positioning
In this case, in step 158, the positioning estimation error calculation route
Matrix Y indicating the predicted value of the solution_{i + 1}Take out.
Next, in step 159, the matrix D indicating the amount of improvement
Calculate based on Equation 1. [0077] D = L_{i + 1}-A_{i + 1}Y_{i + 1} Where A_{i + 1}Is the left side of the navigation equation in this positioning
Is a matrix A. L_{i + 1}Is the navigation method for this positioning
This is the matrix L on the right side of the equation. Next, at step 160
Matrix X indicating the positioning solution of the first positioning_{i + 1}Based on Equation 22
To calculate. [0078] (Equation 22)_{i + 1}= Y_{i + 1}+ Kg · D Here, Kg is a number in the above-described positioning estimation error calculation routine.
Is a matrix showing Kalman gain calculated according to equation 18.
You. Thereafter, in step 162, the obtained solution (XYZ
) To latitude, longitude and altitude and terminate this routine.
Complete. The calculation method of this routine is based on the Kalman filter.
This is an application of an optimal filter called
It suppresses abnormal solution changes. Piezoelectric vibration gai
(B) Using the output of the vehicle speed sensor and the solution predicts the solution of the equation
Is provided, so that the positioning solution
Will stop when the value fluctuates greatly. In particular, vehicle speed = 0
In the case of, there is no movement as the solution prediction, and the prediction error is also 0
Has the effect of suppressing the fluctuation of the solution while stopped.
You. FIG. 16 is a flowchart of the altitude correction routine.
It is. Here, first, at step 163, VDO
It is determined whether P is lower than the upper limit of VDOP. VDOP
Is lower than the VDOP upper limit, the routine jumps to step 164, where V
If DOP is higher than VDOP upper limit, do nothing
End In step 164, the position
The difference between the measured altitude and the altitude obtained by the altitude sensor
Get the quantity. Here, this altitude correction routine is more than 3 satellites
Demodulation is completed and positioning is performed by four satellites.
Altitude obtained by position is obtained only by GPS
Becomes GPS positioning is usually obtained a few seconds later
Therefore, there is a time difference from the altitude obtained by the altitude sensor.
Has occurred. So, according to the delay of GPS, Jai
B is updated in step 69 in the azimuth calculation routine.
Use past altitude data. For example, GPS delay is 3
In the case of seconds, the altitude H [-3] from the altitude obtained by positioning
Is used as a pull correction amount. Next, in step 165, the past
The correction is performed by adding the correction amount to the altitude data. Then, step
At step 166, the UERE calculation routine is executed, and the UERE is calculated.
obtain. Next, in step 167, VDOP and UERE are
The program is memorized and this routine ends. VDOP is σ_z
^TwoTake the square root of FIG. 17 is a flowchart of the UERE calculation routine.
It is a chart. Here, first, from the information of each satellite
SVACC (SVaccu) is an index that does not depend on the satellite arrangement.
racy: One satellite is located based on the internal state of each satellite
The index obtained by calculating the effect on the
It is determined whether SVACC is 0 or more and less than 6.
You. If SVACC is 0 or more and less than 6, step 17
0 for User Range Accuracy URA (User Range
 Accuracy) is calculated by Expression 23. SVACC is 7 or more
If yes, at step 169 the user range accu
The rasy URA is obtained by equation (24). [0082] (Equation 23) URA = 2^{(1 + SVACC / 2)}      (0 ≦ AVACC <7) [0083] (Equation 24) URA = 2^(SVACC-2)        (7 ≦ AVACC) Next, in step 171, Uere is calculated based on equation (25).
Ask. [0084] (Equation 25)   UERE = (UEA^Two+ Σ (value other than intentional deterioration coefficient)^Two)^1/2           = (UEA^Two+119.7)^1/2 Of these, the intentional degradation coefficient is based on the satellite clock error and the satellite
This is due to the prediction error of the orbit. In addition, intentional deterioration
Values other than the number are the perturbation prediction errors of the satellite orbit (value of 1σ =
1.0 m), ionospheric propagation delay correction error (1σ value = 7.
5m), tropospheric propagation delay correction error (1σ value = 2.0)
m), receiver noise (1σ value = 7.5 m), multipath
(1σ value = 1.2m), other errors based on satellite
(1σ value = 0.5 m), other errors based on control
(1σ value = 0.5m), other errors based on the user
(1σ = 0.5 m). The magnitude of these errors
If the value has changed, change the numerical value of Equation 25.
No. Thereafter, this routine ends. FIG. 18 is a flowchart of the gyro correction routine.
It is a chart. The correction mode is a flag that indicates the status of the correction.
It is. Here, first, determine which correction mode is used.
Steps 173, 174, 17 according to the correction mode
5,176 mode-1, mode 0, mode 1, mode
Execute any one of the routines and terminate this routine
I do. Here, the correction is made for the first passing through the vehicle during running.
Gyro and GP between the second, third and third points
This is performed using the output data of S. In mode-1, the correction is opened.
Judge the beginning and collect data of the first point in mode 0
In mode 1, data of the second point is collected,
In the second, data of the third point is collected, and these data are collected.
The correction is performed on the basis of the correction. FIG. 19 is a flowchart of the mode-1 routine.
It is. When the correction mode is mode-1, this
Chin runs. The correction mode is determined by other routines.
Mode-1 is set in the initial state and after completion of correction.
You. Therefore, in the initial state, the mode-1 routine is selected.
It is. First, at step 177, the vehicle speed is set to a predetermined lower limit value.
It is determined whether it is greater. The vehicle speed is lower limit (for example, 10K
m / h), in step 183, the correction mode
Is left in mode-1, and the processing ends. Vehicle speed is below the lower limit
If it is higher, the gyrode for the delay time is determined in step 178.
Check whether data is saved. Where the delay time
Step 1 when saving gyro data for minutes
Go to 79, but save the gyro data for the delay time
If not, the flow advances to step 183 to change the correction mode.
Then, the process is terminated. In step 179,
The positive mode is set to mode 0, and the azimuth is φ in step 180.
m1 [0] and the delay amount of GPS to φm1 [1]
substitute. Next, at step 181, the timer Tφ is set to 0.
And the counter count0 is set to 0 in step 182.
Set and end this routine. Timer Tφ is
Thereafter, the elapsed time is counted. As described above, in the mode-1 routine,
Indicates that the vehicle speed is equal to or higher than the lower limit and the GPS outputs
Gyro data for the data delay time is stored
In this case, gyro correction is started. FIG. 20 is a flowchart of the mode 0 routine.
It is. When the correction mode is mode 0, this routine
Is executed. First, in step 184, the counter coun
t0 is incremented by one. Next, step 18
It is determined at 5 whether the vehicle speed has become greater than a predetermined lower limit value.
I do. In step 186, the gyro data for the delay time
It is determined whether or not is stored. In step 187
It is determined whether or not the mode is continuous with mode-1. Steps
In 188, is the direction at this point the same as mode-1?
Determine whether or not. Any of steps 185 to 188
If not, set the correction mode in step 189.
Then, the routine is terminated. Step 18
If all of the conditions 5 to 188 hold, step 19
At 0, it is determined whether the azimuth stabilization is OK. Orientation is wrong
If stable, this routine ends. The direction is stable
If yes, execute the numerical correction routine in step 191
Then, in step 192, the correction mode is set to mode 1.
The details of the numerical correction routine will be described later, but the GPS azimuth and
The difference between the gyro directions is used as the correction amount, and the
This corrects the azimuth direction and the like. Next, step 19
At 3, the azimuth, delay, and error measured by GPS are respectively
Are substituted into φ0 [0], φ0 [1], and φ0 [2]. Next
In step 194, the timer Tφ is set to 0.
You. Next, in step 195, the gyro corresponding to the delay amount
The direction gldelay-raw [delay amount] is substituted for φ0raw. Next
In step 196, the counters count0 and co
Set unt1 to 0 and end this routine. As described above, in the mode 0 routine, the mode
Start condition at C-1 (the vehicle speed is equal to or higher than the lower limit, and
Gyro data for the delay time of data output by GPS
Data is saved) and continues from mode-1.
And the direction is changed from mode-1 to azimuth.
If there is no gyro azimuth, φ0 raw, GPS azimuth
Is stored such as φ0 [0]. This is the first
It becomes the point of. FIG. 21 is a flowchart of the mode 1 routine.
It is. When the correction mode is mode 1, this routine
Is executed. First, at step 197, the counter coun
t1 is incremented by one. Next, step 19
At 8, it is determined whether the vehicle speed has become greater than a predetermined lower limit.
I do. In step 199, the gyro data for the delay time
It is determined whether or not is stored. Step 198, 1
If any of 99 is not satisfied, in step 203
The counter count1 is set to 0, and this routine ends. Stay
When both steps 198 and 199 are established,
In mode 200, determine whether mode 1 is the second or later
You. If it is the first time, the azimuth measured in step 201,
Substituting the delays into φ1 [0] and φ1 [1], respectively,
End the routine. Mode 1 is 2 in step 200
If it is the first time or later, in step 202, the previous mode 1
It is determined whether or not the orientation at this time is the same. Bearing
Are not continuous, the counter co
unt1 is set to 0, and this routine ends. In the same direction
If so, it is determined in step 204 whether the azimuth stabilization is OK or not.
Is determined. If the bearing is unstable, end this routine
I do. If the bearing is stable, the numerical value is
Execute the correction routine, and use GPS at step 206.
Positioned bearing and delay are φ1 [0] and φ1 [1], respectively.
Substitute for Next, at step 207, the error is φ0
[2] It is determined whether or not it is greater than. If it is big,
In step 208, the error is substituted into φ1 [2].
At step 209, φ0 [2] is substituted for φ1 [2].
Next, in step 210, the gyro direction corresponding to the delay amount
gldelay-raw [delay amount] is substituted into φ1raw. next,
In step 211, φ0 [1] is added to the value of the timer Tφ.
Then, φ1 [1] is subtracted to T10. Then, step
At step 212, φ0 [0] is subtracted from φ1 [0] to obtain the variable co.
l1. Next, this angle difference is adopted in step 213.
It is determined whether or not it is OK. If you are not OK
End If adoption is OK, go to step 214
Subtract φ0raw from φ1raw to obtain raw1. next,
In step 215, each of col1 and raw1 is divided by T10.
Update. Next, in step 216, the correction mode is
Mode 2 and the timer Tφ is set to 0 in step 217.
, And at step 218, the counters count1 and
Set counter count2 to 0 and end this routine
You. As described above, in the mode 1 routine, the mode
Start condition at C-1 (the vehicle speed is equal to or higher than the lower limit, and
Gyro data for the delay time of data output by GPS
Mode 1 has been saved, and mode 1 has been
Running continuously, the same bearing is continuously stable
Gyro direction φ1 raw, GPS method
Data such as φ1 [0]. This point
It will be point 2. Also, the time between the first point and the second point
Difference T10, GPS azimuth difference φ1 [0] -φ0 [0]
(Col1 in step 212), and the difference φ between the gyro directions
Calculate 1raw -φ0raw (raw1 in step 214)
Then, at step 215, the GPS direction col1,
And gyro direction raw1 per time is calculated. FIG. 22 is a flowchart of the mode 2 routine.
It is. When the correction mode is mode 2, this routine
Is executed. First, in step 219, the counter coun
t2 is incremented by one. Next, step 22
Determines whether the vehicle speed has become greater than the predetermined lower limit at 0
I do. In step 221, gyro data for the delay time
It is determined whether or not is stored. Steps 220 and 2
21 is not satisfied, a supplement is made at step 225.
Leave the normal mode in mode 2 and
The counter count2 is set to 0, and this routine ends. Stay
When both steps 220 and 221 are established,
In step 222, it is determined whether or not the mode 2 is the second or later.
You. If it is the first time, the azimuth measured in step 223,
Substituting the delays into φ2 [0] and φ2 [1],
End the routine. Mode 1 is 2 in step 222
If it is the second time or later, in step 224, the previous mode 2
It is determined whether or not the orientation at this time is the same. Bearing
If is not continuous, step 2025 sets the correction mode
In the mode 2 and the counter co in step 238.
unt2 is set to 0, and this routine ends. In the same direction
If so, in step 226, it is determined whether the azimuth stabilization is OK.
Is determined. If the bearing is unstable, end this routine
I do. If the bearing is stable, enter the value
Execute the correction routine and perform positioning in step 228.
And delay to φ2 [0] and φ2 [1], respectively.
You. Next, in step 229, the error is larger than φ1 [2].
Determine if it works. If it is larger, go to step 231
An error is substituted for φ2 [2].
Substitute φ1 [2] for φ2 [2]. Next,
Gyro heading gldelay-raw according to delay amount in step 232
[Delay amount] is substituted for φ2raw and the value of timer Tφ is φ
1 [1] is added and φ2 [1] is subtracted to obtain T21.
Subtract φ1 [0] from [0] to obtain col2. Next,
In step 233, it is determined whether or not this angle difference is OK.
If the adoption is not OK, at step 234 the counter coun
t2 is set to 0, and col2 is set to 999 (max.
Value) and the routine ends. Recruitment is OK
Then, in step 236, φ1 raw is subtracted from φ2 raw.
Raw2 and update by dividing col2 and raw2 by T21 respectively
I do. Next, in step 237, the difference between col1 and col2 is determined.
Whether the logarithmic value is greater than a value obtained by dividing a predetermined lower limit value by T21
Determine whether or not. If smaller, count at step 238
The count2 is set to 0, and this routine ends. Be big
If so, step 239 is executed. In step 239, co
Absolute value of difference between l1 and col2 divided predetermined upper limit value by T21
Greater than the value and the absolute value of the difference between raw1 and raw2 is specified
Is greater than or equal to the value obtained by dividing the upper limit value of T21 by T21
I do. If not, jump to step 246,
For example, step 240 is executed. In step 240 raw1
The absolute value of the difference between と and raw2 is the value obtained by dividing the predetermined lower limit by T21
It is determined whether it is greater than. If it is small, this root
If the value is larger, step 241 is executed. S
In step 241, is the sum of the correction amounts larger than a predetermined lower limit value?
Determine whether or not. If larger, AD conversion in step 242
Initialize the coefficient correction value, and in step 243, the AD conversion coefficient
Execute the correction routine. If smaller, go to step 244
To execute the successive approximation routine. Next, step 245
To set the correction mode to mode-1 and go to step 246.
move on. In step 246, φ2 [0] is changed to φ1 [0].
Substituting φ2 [1] into φ1 [1] and substituting φ2 [2]
Is substituted for φ1 [2], col2 is substituted for col1, and raw2 is ra
Substitute w1. Next, in step 247, the timer Tφ
Is reset to 0, and this routine ends. As described above, in the mode 2 routine, the mode
Start condition at C-1 (the vehicle speed is equal to or higher than the lower limit, and
Gyro data for the delay time of data output by GPS
Data is saved), and mode 2 is
Running continuously, the same bearing is continuously stable
Gyro azimuth φ2 raw, GPS method
Data such as φ2 [0]. This point
It will be point 3. Also, the time between the third point and the second point
Difference T21, GPS azimuth difference φ2 [0] -φ1 [0]
(Col2 in step 232), and the difference φ between the gyro directions
2raw-φ1raw is calculated, and in step 235,
GPS direction col2 and gyro direction per hour
Find raw2. FIG. 23 is a flowchart of a numerical correction routine.
It is. First, in step 248, the vehicle is delayed from the GPS direction.
The correction amount is obtained by subtracting the azimuth ψ [delay] taking into account the shift amount.
Next, at step 249, this correction amount is used as the GPS azimuth error.
N times or more and half of the minimum unit of GPS azimuth output
It is determined whether or not this is the case.
Step 250 adds the correction amount to the correction amount sum and multiplies the correction amount.
Find the calculated value. Here, the GPS azimuth error is calculated based on the following equation.
I'm asking. Ve is a GPS speed error constant. [0096] (Equation 26) GPS bearing error = arctan (Ve / (vehicle speed + Ve)) Next, in step 251, a correction amount is added to the gyro direction,
Correct the gyro heading. Next, in step 252,
遅 延 [-3], ψ [-2], ψ [-
1] and ψ [0] are corrected by adding a correction amount. Next
In step 253, the reference direction is made effective.
At 54, the azimuth estimation error ψerror is set to zero. Next, at step 255, the GPS vehicle speed is predicted.
Vehicle speed correction lower limit (for example, 10 km / h) or more
The vehicle speed correction lower limit.
If the vehicle speed is lower than the vehicle speed correction lower limit, step 25
At 6, the GPS vehicle speed is adjusted to the pulse number P [
To obtain the variable ROLLnew. Also, GPS vehicle speed
The error is divided by the number of pulses P (delay) with delay
Find the number ROLLerrornew. Next, in step 257,
Correct the variable ROLL based on equation 27, and in step 258
The variable ROLLerror is corrected based on the equation (28). [0098] [Equation 27] [0099] [Equation 28] Next, at step 259, the pulse number is delayed.
P [-3], P [-2], P [-1], P
The correction amount is added to each of [0] and the correction is performed.
To end. FIG. 24 shows an AD conversion coefficient correction routine and
It is a flowchart of a next approximation routine. First, here
Is the GPS orientation col1, col2, and the gyro orientation ra
Using w1 and raw2, matrix A_i ^GAnd L_i ^GAs follows
Define. [0102] (Equation 29) [0103] [Equation 30] L_i ^G= [Col1 col2]^-T Further, the correction coefficients α and θ₀And the matrix X_i ^GThe following
As follows. [0104] [Formula 31] X_i ^G= [Α θ₀]^-T Here, referring to FIG. 26, correction coefficients α and θ₀The relationship
explain. Here, while the vehicle is traveling on the road R, the first
Mode 1 is set between the point a1 and the second point a2,
Mode 2 between the second point a2 and the third point a3
It shall be. The actual vehicle of the first point a1
The direction is d1, and the actual direction 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, the time from the first point a1 to the second point a2 is
T10 is applied, and from the second point a2 to the third point a3
It takes time T21. Where the mode
The azimuth change amount of GPS at 1 is col1 · T10 and time
The direction change amount per hit is col1. In mode 2,
GPS azimuth change amount is col2 · T21, per hour
Is azimuth change amount of col2. Gyro in mode 2
The position change amount is raw1 · T10, and the direction change per time
The amount will be raw1. Gyro direction change in mode 2
The raw material amount is raw2 · T21, and the azimuth change amount per hour is
It becomes raw2. Where A_i ^G・ X_i ^G= L_i ^G(I is
Number), A_i ^GAnd L_i ^GCan be calculated, so X
_i ^GCorrection coefficients α and θ₀Is required. In the AD conversion coefficient correction routine,
First, at step 260, a variable i is set to 0, and then
In step 261, the G used when calculating col1 and col2
1 / σθ from φ2 2 indicating the maximum value of PS azimuth error^TwoTo
The value is obtained by Expression 32. [0106] (32) 1 / σθ^Two= 1 / φ22^Two Next, at step 262, the matrix Σε₁ ^{G -1}Based on the following equation
Calculate [0107] [Equation 33] Next, at step 263, the matrix ΣX₁ ^GTo
It is calculated based on the following equation. [0109] ３４X₁ ^G= (A₁ ^GTΣε₁ ^{G -1}A₁ ^G)^-1 Next, at step 264, the matrix X₁ ^GIs calculated based on the following equation.
Put out. [0110] [Expression 35] X₁ ^G= A₁ ^{G -1}L₁ ^G If there is no inverse matrix during correction, the correction is canceled.
You. This gives the matrix X₁ ^GIs obtained, and the correction coefficient α and
θ₀Is found. Next, at step 265, the piezoelectric vibrating
Gyro 15 conversion coefficient ADA multiplied by correction coefficient α for conversion
Update the coefficient ADA. In step 266, the matrix
ΣX_{i + 1} ^GTake the square root of the 11 components of
It is assumed that the error coefficient ADAerror of the block 15 is ADAerror. Where i =
0, so here the matrix ΣX₁ ^GSquare of 11 components of
Take root. Next, at step 267, θ₀The sample
The value obtained by dividing the number of samples and the conversion coefficient ADA is the conversion coefficient A
Subtract DC and update transform coefficient ADC. Next,
Matrix ΣX at step 268_{i + 1} ^GSquare root of 21 components of
Divided by the number of samples and the conversion factor ADA
The error coefficient ADCerror of the piezoelectric vibrating gyroscope 15 is defined as ADCerror.
Since i = 0 here, the matrix ΣX₁ ^G2
Take the square root of one component. Next, delay in step 269
Specify re-collection of data.
The sum is set to zero and this routine ends. In the successive approximation routine, first, at step 27
At 1, it is determined whether or not the coefficient correction is for the second shift. First time
If the coefficient correction is
The same processing as the conversion coefficient correction routine is performed. From the second time
If it is, the process proceeds to step 272, where
Add 1. Next, at step 273, col1 and col2 are
Φ2 indicating the maximum value of the GPS azimuth error used at the time of calculation
2 to σθ according to Equation 36^TwoAsk for. [0112] (36)^Two= Φ2 2^Two Next, in step 274, the matrix Σε_{i + 1} ^{G -1}To
Calculate based on [0113] (37) Next, step 275 and step 276
Matrix at X_i ^GIs a matrix ΣU_i ^GAnd matrix ΣY
_i ^GSubstitute for Next, at step 277, the matrix Y
_{i + 1} ^GIs defined based on the following equation. [0115] [Equation 38] Y_{i + 1} ^G= [10]^T Next, at step 278, the Kalman gain is calculated based on the following equation.
Matrix Kg^GAsk for. [0116] [Equation 39] Kg^G= ΣY_{i + 1} ^GA_{i + 1} ^GT[Σε_{i + 1} ^G
+ A_{i + 1} ^GΣY_{i + 1} ^GA_{i + 1} ^GT]^-1 Next, at step 279, the estimation error matrix Σ
_{i + 1} ^GAsk for. Here, I is a 2 × 2 unit matrix.
You. [0117] (Equation 40) ΣX_{i + 1} ^G= 2 [I-Kg^GA_{i + 1} ^G] ΣY_{i + 1} ^G Next, at step 280, a matrix D indicating the improvement amount^GIs
Based on [0118] [Formula 41] D^G= L_{i + 1} ^G-A_{i + 1} ^GY_{i + 1} ^G Next, at step 281, the matrix Y_{i + 1} ^GIs based on
And add a value obtained by multiplying the Kalman gain by the amount of improvement.
You. [0119] (Equation 42) Y_{i + 1} ^G= Y_{i + 1} ^G+ Kg^GD^G Thereafter, the process proceeds to step 265. At step 265,
The conversion coefficient ADA of the piezoelectric vibrating gyroscope 15 is changed by multiplying by α.
The conversion coefficient ADA is updated. In step 266,
Column ΣX_{i + 1} ^GTake the square root of the 11 components of
The error coefficient of the gyro 15 is ADAerror. Next,
In step 267, θ₀The number of samples and the converter
Subtract the value divided by the number ADA from the conversion coefficient ADC to convert
Update the coefficient ADC. Next, at step 268, the matrix
ΣX_{i + 1} ^GTake the square root of the 21 components of
les and the conversion coefficient ADA to divide the piezoelectric vibratory gyroscope 15
Is an error coefficient ADCerror. Next, step 269
Specify delay data re-collection in step 270
Thus, the routine is ended with the sum of the correction amounts set to zero. By the above-mentioned AD conversion coefficient correction routine,
Positive coefficients α and θ₀Gyro conversion coefficient AD
A, ADC and error coefficient ADAerror, ADCerror are G
Corrected by PS and gyro readings for more accurate
Be able to perform positioning. FIG. 25 is a flowchart of a gyro zero point correction routine.
It is a chart. First, here at step 282
Whether the vehicle speed = 0 and the sum of the correction amounts is equal to or less than a predetermined lower limit value
Is determined, and if not, this routine is terminated.
If so, the process proceeds to the next step 283. In step 283, the power
Immediately after the source has been turned on, or
It is determined whether or not. If no, go to step 285
An average of speed = 0 is taken for one second, and if not, step 28
At 4, take the average of all the delay data. Next, step 286
The maximum value and the minimum value of the sum for one second are obtained with. Step
In step 287, the difference between the maximum value and the minimum value is equal to a predetermined upper limit value.
(E.g., 20 deg / s)
If it is larger than the upper limit, it is determined that it cannot be adopted and this routine
And if it is smaller than the upper limit value, it is determined in step 288.
Change the average value to zero. Thereafter, at step 289, the delay data is re-
Specify collection and set the correction mode in step 290.
-1 and set the correction amount sum to zero in step 291.
The lever routine ends. In this gyro zero point correction routine,
Measure the gyro zero point so that the drift amount becomes zero
Has been corrected. As described above, this embodiment is designed for a vehicle.
The vehicle speed sensor 14, which is a position sensor for detecting the position, and the pressure
From the electric vibratory gyro 15 and the signals emitted by multiple satellites
Satellite information that detects the relative position between each satellite and the vehicle
A GPS which is a detecting means or a relative speed detecting means,
From at least one of the output of the
Computer as positioning means for positioning the absolute position of the vehicle
19 positioning routines 35 and 47, based on information from satellites
Sensor output correction means for correcting the detection position of the
Gyro correction routine 37 of the computer 19 which is a stage
It is provided with. The positioning means is a vehicle determined from the output of the gyro.
Both azimuths, integrated distance obtained from the output of the vehicle speed sensor, G
Position using any of the relative speeds of the PS satellites
I have. In addition, the gyro correction routine uses information from the satellite.
Azimuth correction to correct the calculation constant of the azimuth calculation means based on
It is also a correct means. Also executed during the gyro correction routine
In the numerical correction routine, the
Distance correction to correct the calculation constant of the total distance calculation means
(Steps 255 to 259). Therefore, the gyro or the like which is likely to cause drift
Accurate positioning is possible even when using a vehicle speed sensor with a large error.
it can. Note that the gyro correction routine of the above embodiment is
As shown in the figure, the gyro heading change per hour (raw
1, raw2) and azimuth change obtained from satellite information per hour
Jai which is the azimuth angle calculation means from the amount of conversion (col1, col2)
Complement the calculation constants (ADA, ADC) of the azimuth calculation routine.
Correct it. Further, the number in the gyro correction routine
Gyro heading and satellite information as shown in the value correction routine
The azimuth difference obtained from the report is used as a correction amount (step 24).
8) Add gyro azimuth correction amount to correct (step)
251). Furthermore, the number during the gyro correction routine
Gyro heading and satellite information as shown in the value correction routine
Of the azimuth difference obtained from the information
(Step 250), and if the sum of the correction amounts is small,
As shown in the AD conversion coefficient correction routine,
Gyro heading change (raw1, raw2) and per hour
Of the azimuth change (col1, col2) obtained from satellite information
To determine the run and correct the calculation constant of the azimuth angle calculation means.
When the sum of the correction amounts is large, as shown in the successive approximation routine
And gyro direction change per hour (raw1, raw2)
And the azimuth change amount (col1,
col2) using Kalman filter
The calculation constant of the angle calculation means may be corrected. The numerical correction routine of the above embodiment is shown in FIG.
Vehicle speed (GPS speed) and vehicle obtained from satellite information
Integrated distance calculation routine based on speed error (GPS vehicle speed error)
Calculate the correction amount (ROLL, ROLLerror) of the distance, and calculate the total distance and total
It is preferable to correct the distance error. [0131] Claims of the InventionOneAccording to the invention, the drift
A rotational angular velocity sensor that is easy to
Even if used, accurate positioning can be performed. In addition, the amount of correction
The azimuth is increased because the error that increases due to addition is predicted and corrected.
The output of the calculation means will be accurate. [0132] [0133] [0134] [0135]

BRIEF DESCRIPTION OF THE 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 the present invention. FIG. 3 is a flowchart of a gyro azimuth calculation routine according to the embodiment of the present invention. FIG. 4 is a flowchart of a direction estimation error calculation routine according to the embodiment of the present invention. FIG. 5 is a flowchart of an integrated distance calculation routine according to the embodiment of the present invention. FIG. 6 is a flowchart of a navigation equation 1 routine according to the embodiment of the present invention. FIG. 7 is a flowchart of a navigation equation 2 routine according to the embodiment of the present invention. FIG. 8 is a flowchart of a navigation equation 3 routine according to the embodiment of the present invention. FIG. 9 is a flowchart of a σi ² calculation routine according to the embodiment of the present invention. FIG. 10 is a flowchart of a σa ² calculation routine according to the embodiment of the present invention. FIG. 11 is a flowchart of a σG ² calculation routine according to the embodiment of the present invention. [12] Shigumaderutax ² embodiment of the present invention, a flow chart of Shigumaderutawai ² calculation routine. FIG. 13 is a flowchart of an EψxEψy calculation routine according to the embodiment of the present invention. FIG. 14 is a flowchart of a positioning estimation error calculation routine according to the embodiment of the present invention. FIG. 15 is a flowchart of a positioning routine according to the embodiment of the present 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 UERE calculation routine according to the embodiment of the present 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 the present invention. FIG. 20 is a flowchart of a mode 0 routine according to the embodiment of the present invention. FIG. 21 is a flowchart of a mode 1 routine according to the embodiment of the present invention. FIG. 22 is a flowchart of a mode 2 routine according to the embodiment of the present invention. FIG. 23 is a flowchart of a numerical value 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 the present 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 embodiment of the present invention. FIG. 27 is an explanatory diagram of an embodiment of the present invention. [Description of Signs] 10 Receiving antenna 11 GPS receiver 12 GPS demodulator 13 Display device 14 Vehicle speed sensor 15 Piezoelectric vibration gyro 16 Advanced sensor 17 Electronic control unit 18 Input / output interface 19 Microcomputer 20 Analog / digital converter

──────────────────────────────────────────────────続 き Continued on the front page (72) Inventor Tomio Yasuda 2-1-1 Asahicho, Kariya-shi, Aichi Aisin Seiki Co., Ltd. (56) References JP-A-5-157828 (JP, A) JP-A-2-189414 (JP, A) JP-A-2-134587 (JP, A) JP-A-63-5213 (JP, A) JP-A-63-302317 (JP, A) (58) Fields investigated (Int) .Cl. ⁷ , DB name) G01C 21/00 G01S 5/00-5/14

Claims (1)

(57) [Claims] [Claim 1] A gyro for detecting an azimuth deviation of a vehicle; a vehicle speed sensor for detecting a speed of the vehicle; a relative speed between each satellite and the vehicle based on signals emitted by a plurality of satellites. Relative speed detecting means for detecting; azimuth angle calculating means for obtaining an azimuth angle of the vehicle from the output of the gyro; integrated distance calculating means for obtaining an integrated distance of the vehicle from the output of the vehicle speed sensor; Positioning means for performing positioning using any of the relative velocities; azimuth correction means for correcting the calculation constant of the azimuth calculation means based on information from a satellite; calculation of the integrated distance calculation means based on information from a satellite. An in-vehicle positioning device comprising : an integrated distance correcting means for correcting the integrated distance.
Compensates for the difference between the gyro direction and the direction obtained from satellite information.
A correction amount sum is obtained by integrating the correction amount, and the correction amount is calculated.
Gyro heading change per unit time when the sum is small
Between the azimuth and the direction change obtained from satellite information per hour
Is calculated, and the calculation constant of the azimuth angle calculation means is corrected.
When the sum of the correction amounts is large, the gyro
Azimuth change obtained from azimuth change amount of satellite and time per satellite information
The relationship with the amount of conversion was determined using a Kalman filter,
It is characterized in that the calculation constant of the azimuth angle calculation means is corrected.
In-vehicle positioning device.