JPH0868652A - Current position detector for vehicle - Google Patents

Abstract

PURPOSE: To perform compensation with means other than GPS when the GPS cannot measure a position for a long time in cases where the estimation navigation is to be compensated by the GPS using Karman filter. CONSTITUTION: An offset error, a distance coefficient error, an absolute azimuth error, and an absolute position error are obtained by a Karman filter 6 according to the information of the position, azimuth, and vehicle speed obtained from the estimation navigation and the information of the position, azimuth, and vehicle speed of a vehicle outputted from a GPS 3 to perform each compensation in the estimation navigation for the estimation navigation according to a vehicle speed sensor 1, a gyro 2, a relative trace operation part 4, and an absolute trace operation part 5. When the position of the GPS 3 cannot be measured for a long time and the accuracy of the azimuth of the vehicle decreases, the absolute azimuth of the estimation navigation is matched to the azimuth of geomagnetism which is detected by a geomagnetism sensor 7.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a vehicle current position detecting device for detecting the current position of a vehicle based on the direction and the moving distance of the vehicle.

[0002]

2. Description of the Related Art Conventionally, in this type of vehicle-mounted navigation device, a relative direction sensor (gyro, steering sensor, wheel sensor, etc.) for detecting the amount of change in the direction of the vehicle.
And the output of a distance sensor (vehicle speed sensor, wheel sensor, etc.) that detects the speed (distance) of the vehicle,
Dead reckoning which detects vehicle speed etc. is used.

The dead reckoning navigation output (position, direction, vehicle speed, etc.) includes an error of the sensor, and thus an error occurs. In particular, the position / azimuth is obtained in an integrated manner, so the error gradually increases. On the other hand, since GPS can determine the absolute position, direction, and vehicle speed, GP
The correction can be made by matching the dead reckoning output with the GPS output when S is positioned. For example, when the difference between the position obtained by dead reckoning and the position obtained by aligning the road position on the road map by map matching and the position obtained by GPS becomes larger than a predetermined value, The position may be corrected to the position obtained by GPS.

[0004]

However, the position obtained by dead reckoning can be corrected by the output of GPS, but not by the sensor. Therefore, there is a problem that the dead-reckoning navigation output accuracy is poor due to an error in the sensor output when GPS is not received. A first object of the present invention is to correct an error in sensor output so as to obtain a current position.

In order to achieve the above object, the present inventors have
As will be described later, a Kalman filter is used to obtain the sensor error amount based on the difference between the information on the vehicle direction obtained from dead-reckoning and the information on the vehicle direction output from GPS, and the error correction of the sensor output is performed. Thought what to do. Positioning data from GPS is necessary for the error correction by the Kalman filter. However, when the GPS cannot perform positioning for a long time, the sensor error becomes large, so it is necessary to correct the sensor error by some method.

Therefore, a second object of the present invention is to perform correction using data other than GPS when GPS cannot be positioned for a long time.

[0007]

In order to achieve the above object, the present invention provides a relative azimuth sensor (2) for outputting a signal according to an amount of azimuth change of a vehicle.
And a moving distance detecting means (1 and 103) for detecting a moving distance of the vehicle, and based on a signal from the relative azimuth sensor, an offset correction amount is offset-corrected to obtain an azimuth change amount. Of the vehicle and the position detecting means (4, 5 and arithmetic processing therefor) for detecting the position of the vehicle based on the direction of the vehicle and the moving distance of the vehicle detected by the distance detecting means. Dead reckoning means (1, 2, 4, 5 and arithmetic processing therefor), and GPS (3) that receives satellite radio waves from GPS satellites and outputs information regarding the direction of the vehicle
And at least the offset error and the heading error of the vehicle as the state quantity X (t), and the state quantity X (t + 1) is given by the process matrix φ that gives a temporal change of this error and the noise ω generated in the signal generation process. Signal generation process related by X (t) + ω, state quantity X (t) and observed value Y (t)
The vehicle in the dead reckoning means is based on a model that forms an observation process in which the observation value Y (t) is related by H · X (t) + v by the observation matrix H and noise v generated in the observation process. The observed value Y (t) is calculated from the difference between the information about the direction of the vehicle and the information about the direction of the vehicle output from the GPS, and the state quantity X (t) is calculated from the model based on the calculated value. State quantity calculation means (6
And the processing of 401 to 408, 410, 411 corresponding thereto) and the state quantity X obtained by the state quantity calculation means.
A correction unit (40) for correcting the offset correction amount and the vehicle direction in the position detection unit based on the offset error and the vehicle direction error due to (t).
9), an absolute azimuth sensor (7) for detecting the absolute azimuth of the vehicle, and the position detection means specifies when a state in which the accuracy of the azimuth of the vehicle specified by the position detection means is lowered is detected. And a change means (601 to 603) for changing the direction of the vehicle to the direction detected by the absolute direction sensor.

According to a second aspect of the present invention, in the first aspect of the invention, the state quantity computing means estimates at least the magnitude of the offset error and the heading error of the vehicle and the cross-correlation value of those errors. Which comprises means (405, 411) for calculating an error covariance matrix composed of
The changing means detects a state in which the accuracy of the heading of the vehicle is reduced by estimating the magnitude of the heading error of the vehicle.

According to a third aspect of the present invention, in the second aspect of the invention, the changing means sets the direction of the vehicle specified by the position detecting means to the direction detected by the absolute direction sensor. It is characterized by having means (603) for setting to 0 a cross-correlation value having a correlation with the heading error of the vehicle in the error covariance matrix.

According to another aspect of the present invention, a relative azimuth sensor (2) for outputting a signal corresponding to the amount of change in the azimuth of the vehicle, and moving distance detecting means (1 and 103) for detecting the moving distance of the vehicle. Based on a signal from the relative azimuth sensor, offset correction is performed by an offset correction amount to obtain an amount of azimuth change, and the azimuth of the vehicle is specified by the amount of azimuth change, and the azimuth of the vehicle and the distance detection means detect the Dead reckoning means (1, 2, 4, 5 and arithmetic processing for it), which is composed of position detection means (4,5, and arithmetic processing for it) that detects the position of the vehicle based on the moving distance of the vehicle; GP that receives satellite radio waves from GPS satellites and outputs information about the direction of the vehicle
S (3) and at least the offset error and the heading error of the vehicle are the state quantities X (t), and the state quantity X (t + 1) is obtained by the process matrix φ that gives a temporal change of these errors and the noise ω generated in the signal generation process. ) By φ · X (t) + ω, the state quantity X (t) and the observed value Y (t), the observed value Y by the observation matrix H and the noise v generated in the observed process. The difference between the information about the vehicle direction in the dead reckoning means and the information about the vehicle direction output from the GPS based on a model forming an observation process in which (t) is related by H · X (t) + v According to the state value calculation means (6 and 401 to 408, 410, corresponding thereto) for calculating the observation value Y (t) and calculating the state quantity X (t) from the model based on the calculated value.
411) and the offset error and the vehicle heading error due to the state quantity X (t) obtained by the state quantity computing means, the offset correction amount is corrected and the vehicle heading in the position detecting means is corrected. A correction unit (409) and a change unit (701 to 703) for changing the offset correction amount based on the output from the relative direction sensor when the vehicle is stopped when the state in which the accuracy of the offset correction is lowered is detected. It is characterized by having.

According to a fifth aspect of the present invention, in the fourth aspect of the present invention, the state quantity computing means estimates at least the magnitude of the offset error and the heading error of the vehicle and the cross-correlation value of those errors. Which comprises means (405, 411) for calculating an error covariance matrix composed of
The changing means detects a state in which the accuracy of offset correction is reduced by estimating the magnitude of the offset error.

According to a sixth aspect of the present invention, in the fifth aspect of the present invention, the changing means changes the offset correction amount, and the cross-correlation having a correlation with the offset error in the error covariance matrix. It is characterized by having means (703) for setting the value to 0. It should be noted that the above-mentioned information about the azimuth of the vehicle is a concept that includes not only the azimuth itself but also information related to the azimuth, and also includes the absolute position and the like correlated with the azimuth as described later.

Further, the reference numerals in parentheses of the above-mentioned means indicate the correspondence with the concrete means described in the embodiments described later. Further, each step in the flowcharts described later constitutes a function realizing means for realizing each function.

[0014]

According to the first and fourth aspects of the invention, the offset error is obtained from the difference between the information on the vehicle direction obtained from dead-reckoning and the information on the vehicle direction output from GPS. Since the offset of the relative azimuth sensor is corrected, the error in the sensor output from the relative azimuth sensor can be corrected.

Further, in the invention described in claim 1, when the GPS cannot perform positioning for a long time and the accuracy of the orientation of the vehicle deteriorates, the orientation of the vehicle is determined by the output from the absolute orientation sensor. Therefore, the azimuth of the vehicle in such a case can be corrected. further,
According to the third aspect of the invention, when setting the vehicle azimuth using the absolute azimuth sensor, the cross-correlation value correlated with the vehicle azimuth error in the error covariance matrix is set to 0. Therefore, it is possible to improve the error correction after the GPS is positioned again.

Further, in the invention described in claim 4,
The offset correction amount is set by the output from the relative azimuth sensor when the vehicle is stopped when the accuracy of the offset correction deteriorates when the GPS cannot be positioned for a long time. Therefore, the offset correction in such a case is performed. The amount can be corrected. Further, in the invention according to claim 6, when the offset correction amount is set when the vehicle is stopped as described above, a cross-correlation value having a correlation with an offset error in the error covariance matrix is set to 0. Therefore, it is possible to improve the error correction after the GPS is positioned again.

[0017]

Embodiments of the present invention will be described below with reference to the drawings. This embodiment uses a Kalman filter in order to combine dead reckoning and GPS. The outline of this Kalman filter will be described. This Kalman filter is divided into a signal generation process and an observation process, as shown in FIG. In the figure, if there is a linear system (φ) and a part of X (t) related by the observation matrix H can be observed for the state X (t) of the system, the filter of X (t) Gives an optimal estimate. Here, ω is noise generated in the signal generation process, and v is noise generated in the observation process. The input of this filter is Y (t) and the output is the optimal estimate of X (t).

The optimum estimated value of the state X using the information up to the time t, that is, the state quantity X (t | t) is obtained by the equation 1.

[0019]

## EQU1 ## X (t | t) = X (t | t-1) + K (t)
{Y (t) -HX (t | t-1)} where X (t | t-1) is a pre-estimated value and K (t) is a Kalman gain, which are represented by Equations 2 and 3, respectively. It

[0020]

## EQU00002 ## X (t | t-1) =. Phi.X (t-1 | t-1)

[0021]

[Number 3] K (t) = P (t | t-1) H T (HP (t |
t−1) H ^T + V) ⁻¹ where P is the error covariance of the state quantity X, and P (t | t
-1) is the predicted value of the error covariance, and P (t-1 | t-1) is the error covariance, which are represented by equations 4 and 5, respectively.

[0022]

[Number 4] P (t | t-1) = φP (t-1 | t-1) φ T + W

[0023]

## EQU5 ## P (t-1 | t-1) = (I-K (t-1)
H) P (t-1 | t-2) V is the variance of noise v generated in the observation process, and W is the variance of noise ω generated in the signal process. In addition, A (i |
j) represents the estimated value of A at time i based on the information up to time j. The subscript ^T means a transposed matrix, and ^-1 means an inverse matrix. I is a unit matrix.

Further, V and W are white Gaussian noises having an average of 0 and are uncorrelated with each other. In the Kalman filter as described above, the accuracy of the state quantity X is improved by giving an appropriate error to the initial values of the state quantity X and the error covariance P and repeating the above calculation each time a new observation is made. To do. This embodiment applies such a Kalman filter to dead reckoning.

First, the definition of the above signal generation process will be described. The Kalman filter in dead-reckoning aims to correct the error in dead-reckoning, so the state quantity X defines the following five error values. It is the process matrix φ that gives a temporal change of this error value. Offset error (εG)

[0026]

[Equation 6] εG _t = εG _t-1 + ω _{0 There} is no definite change, and noise is added to the previous error. Absolute heading error (εA)

[0027]

In Equation 7] _{εA t = T × εG t-} 1 + εA t-1 + ω 1 last error, the azimuth error and noise to obtain over the elapsed time from the previous to the offset error is added. Distance coefficient error (εK)

[0028]

[Equation 8] εK _t = εK _t-1 + ω _{2 There} is no definite change, and noise is added to the previous error. Absolute position north direction error (εY)

[0029]

## EQU9 ## εY _t = sin (A _T + εA _t-1 + εG _t-1 ×
T / 2) × L × (1 + εK _t-1 ) −sin (A _T ) × L
+ ΕY _t-1 The error generated by the azimuth error / distance error is added to the previous error. Absolute position east error (εX)

[0030]

ΕX _t = cos (A _T + εA _t-1 + εG _t-1
× T / 2) × L × (1 + εK _t-1 ) -cos (A _T ) ×
L + εX _t-1 The error caused by the azimuth error / distance error is added to the previous error. In the above definition, _AT is the true absolute azimuth, L is the moving distance from the previous time, and T is the elapsed time from the previous time.

When the above equations are partially differentiated by the state quantity and linearized, the signal generation process is defined as follows.

[0032]

[Equation 11]

The above A is the absolute azimuth A _T + εA _t-1 + εG
_It means _t-1 × T / 2. This value is obtained by adding the sensor error to the true absolute azimuth A _T , and is the absolute azimuth A obtained from the amount of azimuth change, as will be described later. Also, ω ₀
Indicates offset noise (change in offset due to temperature drift etc.), ω ₁ indicates absolute direction noise (gyro gain error), and ω ₂ indicates distance coefficient noise (aging).

Next, the definition of the above observation process will be described. The observation value is obtained from the difference between the dead reckoning output and the GPS output. Since each output includes an error, the sum of the dead reckoning error and the GPS error is obtained in the observed value. The observed value Y and the state quantity X are related to each other and are defined as in Expression 12.

[0035]

[Equation 12]

However, noise v generated in the observation process is GPS
Noise, which is defined as

[0037]

[Equation 13]

Dead-reckoning navigation using a Kalman filter will be described based on the above definitions. FIG. 1 shows a schematic configuration of this embodiment. As shown in this figure, based on the signals from the vehicle speed sensor 1 and the gyro 2, the relative trajectory calculation unit 4,
The absolute position calculation unit 5 performs the calculation, and the vehicle speed, the relative trajectory, the absolute position, and the absolute azimuth are output by the calculation (dead-reckoning navigation calculation). In addition, from GPS3
Output of vehicle speed can be obtained. The Kalman filter 6 corrects the distance coefficient of the vehicle speed sensor 1, the offset correction of the gyro 2, and the absolute position based on the vehicle speed, the absolute position / absolute direction information and the vehicle speed / position / direction information from the GPS 3 obtained by dead reckoning. Correction, absolute azimuth correction.

When the Kalman filter is applied to such a vehicle-mounted navigation device, the pre-estimation X (t shown in Formula 2 is obtained by the distance coefficient correction of the vehicle speed sensor 1, the offset correction of the gyro 2, the absolute azimuth correction, and the absolute position correction.
| T−1) becomes 0. Therefore, equation 1 becomes as shown in equation 14.

[0040]

[Mathematical formula-see original document] X (t | t) = K (t) Y (t) Therefore, the state quantity X by the five error values defined in the signal generation process is the Kalman gain obtained by the equations 3 to 5. It is determined by K (t) and the observed value Y (t).

Here, the error covariance P in Equation 3 is defined by Equation 15.

[0042]

(Equation 15)

In this error covariance P, σ _GG ² represents an estimate of the magnitude of the offset error, σ _AA ² represents an estimate of the magnitude of the absolute heading error, and σ _KK ² is an estimate of the magnitude of the distance coefficient error. , Σ _YY ² represents an estimate of the absolute position northward error magnitude, and σ _XX ² represents an absolute position eastward error magnitude estimate. Other than that, σ _ij ² represents the cross-correlation value of the i-th row and the j-th column. For example, σ _AG ² represents the cross-correlation value between the offset error and the absolute azimuth error.

The value of this error covariance P is updated by the calculation of equation (4). In addition, in the initial value, σ _GG ² , σ
_The values of _AA ² , σ _KK ² , σ _YY ² , and σ _XX ² are set to values that maximize the error, and the cross-correlation values are all 0.
Set to. In addition, the matrix shown in Expression 12 is used for H in Expression 3, and the matrix shown in Expression 13 is used for V. Further, W in Expression 4 uses the variance of ω shown in Expression 11.

As the observed value Y in the observation process,
As shown in FIG. 2, εA _DRt −εA _{GP St} , εK _DRt −εK
_GPSt , εY _DRt −εY _GPSt , εX _DRt −εX _GPSt are used. Here, the subscript _DRt means a value obtained by dead-reckoning navigation based on signals from the vehicle speed sensor 1 and the gyro 2 at time t, and GPSt means a value output from _{GPS 3} at time t.

ΕA _DRt −εA _GPSt is the difference between the absolute azimuth obtained by dead reckoning and the azimuth output from GPS3,
That is, the absolute azimuth obtained by dead-reckoning contains the true absolute azimuth and its error εA _DRt.
The true absolute azimuth and its error εA
_{Since GPSt} is included, εA _DRt −εA _GPSt can be obtained by taking the difference between them.

Similarly, εK _DRt −εK _GPSt is a distance coefficient error obtained from the difference between the speed calculated by dead reckoning and the speed output from GPS3. ) / (Speed by dead reckoning). Also, εY _DRt −εY _GPSt
Is the Y component of absolute position and GP obtained by dead reckoning
It is the difference in error of the Y component of the position output from S3, and ε
X _DRt −εX _GPSt is the difference between the error between the X component of the absolute position obtained by dead reckoning and the X component of the position output from the GPS 3.

Further, the noise v generated in the observation process shown in the equation 13 is the noise of GPS3 and can be obtained as follows. Pseudo-range measurement error in GPS3 (UER
E) and HDOP (Horizontal Dilution of Presision)
Therefore, the positioning accuracy is calculated by UARE × HDOP, and by squaring the positioning accuracy, v _2t and v _3t are calculated. In addition, Doppler frequency measurement error and HD
The speed accuracy is calculated from the relationship of OP by the measurement error of the Doppler frequency x HDOP, and the distance coefficient measurement error is calculated by this speed accuracy / vehicle speed.
_1t is required. Further, the azimuth accuracy is calculated as tan ⁻¹ (speed accuracy / Vc) from the vehicle speed Vc and the speed accuracy,
The square of this azimuth accuracy gives v _0t .

Therefore, εA _DRt −εA in the observation process
_{GPSt, εK DRt -εK GPSt, εY} DR t -εY GPSt, εX
_DRt- εX _GPSt and the noise V are input, and by executing the _equations 3 to 5 and the equation 1, the state quantity X based on the five error values defined in the signal generation process is obtained. The distance coefficient correction of 1, the offset correction of the gyro 2, the absolute position correction, and the absolute azimuth correction are performed.

The above-mentioned relative locus calculation, absolute position calculation, and Kalman filter are carried out by the calculation processing by the microcomputer, which will be described below. FIG. 2 shows the arithmetic processing of the main routine of dead reckoning. Step 1
At 00, the azimuth change amount / movement distance is calculated. Details of this processing are shown in FIG. First, in step 101, the output angular velocity of the gyro 2 is multiplied by the start cycle T _M of the main routine to calculate the bearing change amount. In the next step 102,
The offset correction amount (the correction amount will be described later) multiplied by the start cycle T _M of the main routine is subtracted from the direction change amount to perform the offset correction of the direction change amount. In the next step 103, the moving distance is calculated by multiplying the number of vehicle speed pulses from the vehicle speed sensor 1 by a distance coefficient (this distance coefficient will also be described later).

After step 100, step 20
A relative locus calculation process of 0 is performed. Details of this processing are shown in FIG. First, in step 201, the relative azimuth is updated based on the azimuth change amount (obtained in step 102).
The relative position coordinates are updated in step 202 based on the updated relative azimuth and the moving distance obtained in step 103. This update is based on the relative azimuth X,
This is performed by adding the Y component to the relative position coordinates up to that point. This relative position coordinate is used to determine the relative trajectory, and so-called map matching is performed based on the relationship between the relative trajectory and the road shape.

After step 200, step 30
The absolute azimuth and absolute position of 0 are calculated. Details of this processing are shown in FIG. First, in step 301, the absolute azimuth is updated based on the azimuth change amount (obtained in step 102). This updated absolute orientation and step 10
In step 202, the absolute position coordinates are updated based on the movement distance obtained in 3. The absolute azimuth A and the absolute position updated in the processing of this step 200 are used in the composite processing with GPS described later.

FIG. 6 shows the details of step 400 for performing the processing for combining with the GPS. First, in step 401, it is determined whether or not T1 seconds have elapsed since the last positioning or prediction calculation. This performs the process of correcting the dead reckoning error in steps 403 to 409 every time the GPS3 positioning is performed. However, when the GPS3 positioning cannot be performed, the error becomes large. Therefore, the prediction calculation of the error corresponding thereto is performed. Step 4
It is provided to perform regularly at 10 and 411.

If the determination in step 401 is NO, the scan
Whether there is positioning data from GPS3 at step 402
Do that. If there is positioning data from GPS3,
The processing proceeds to the Kalman filter calculation processing after step 403. Well
First, in step 403, the observation value Y is calculated. this
Is the speed, position, and direction data output from GPS3.
And obtained in the process of step 300 in dead reckoning
Absolute azimuth, absolute position and speed calculation processing not shown
Based on the vehicle speed pulse from the vehicle speed sensor 1,
Therefore, εA shown in Equation 12_DRt-ΕA _GPSt, ΕK_DRt
-ΕK_GPSt, ΕY_DRt-ΕY_GPSt, ΕX_DRt-ΕX_GPSt
As well as calculating
Noise v is calculated based on GPS3 positioning data and the like.

In step 404, the process matrix φ is calculated. This is the process shown in Formula 11 according to the moving distance L from the time when the previous process matrix was calculated, the elapsed time T (these are separately obtained by the measurement process not shown), and the absolute azimuth A obtained in step 301. Find the matrix φ. Based on the observation value Y and the process matrix φ calculated in this way, the above-described calculations of the expressions 3 to 5 are performed to obtain the state quantity X shown in the expression 14. That is, in step 405, the prediction calculation of the error covariance P is performed by the equation 3. In step 406, the Kalman gain K is calculated by the equation 4.
Calculate. In step 407, the error covariance P is calculated by the equation 5. After that, based on the Kalman gain K and the observed value Y, in step 408, the state quantity X is obtained by the calculation of Expression 14. This state quantity X is, as shown on the left side of the equation 11, offset error (εG), absolute heading error (εA), distance coefficient error (εK), absolute position north direction error (εY), absolute position east direction error ( εX) is represented.

Due to these errors, in step 409, the dead reckoning error is corrected by the calculation shown in the figure, that is, the gyro 2 offset correction, the vehicle speed sensor 1 distance coefficient correction, the absolute azimuth correction, and the absolute position correction are performed. . The offset correction amount used in step 102 is corrected by the offset correction of the gyro 2, the distance coefficient used in step 103 is corrected by the distance coefficient correction of the vehicle speed sensor 1, and the absolute direction correction is performed in step 301 by the absolute direction correction. The absolute orientation A used is modified, and the absolute position correction modifies the absolute position used in step 302.

The above process is repeated every time there is positioning data from the GPS 3 to correct the above error, and more accurate dead reckoning data can be obtained. Also, G
If positioning of PS3 is not possible for a long time, step 401
If the determination is YES, the process proceeds to steps 410 and 411 to calculate the process matrix φ and predict the error covariance P. This makes it possible to perform an error covariance prediction calculation corresponding to an error when the GPS 3 cannot be positioned, and to accurately perform the Kalman filter process performed when the GPS 3 is subsequently positioned.

Position correction data from the GPS 3 is required for error correction by the Kalman filter described above. However, when the GPS 3 cannot perform positioning for a long time, the sensor error becomes large, so it is necessary to correct the sensor error by some method. Therefore, the absolute direction is corrected using the geomagnetic sensor 7. In this case, it is possible to compare the absolute direction of dead reckoning and the geomagnetic direction, and to adjust the absolute direction of dead reckoning to the geomagnetic direction when the difference is a certain value or more.

By such a correction, the error of the azimuth is reduced, but the predicted value of the azimuth accuracy remains large and the data becomes unreliable. Therefore, when the absolute azimuth is corrected to the geomagnetic azimuth, it is possible to reduce the accuracy prediction value of the absolute azimuth to a value that can be expected by the geomagnetic sensor. However, in this case, if only the absolute azimuth dispersion portion is reduced, the relationship between the absolute azimuth error and the magnitude of other state quantity covariance becomes abnormal, and when the GPS 3 is positioned next and the Kalman filter operates, incorrect correction is performed. Furthermore, the error covariance becomes a negative number and the calculation becomes impossible.

Therefore, when the absolute azimuth is corrected using the geomagnetic azimuth, the correction does not correlate the absolute azimuth error with the absolute position error, offset error, and distance coefficient error. Therefore, the cross-correlation value of the covariance term between the absolute orientation in the error covariance matrix and the other state quantities is cleared (set to 0). Specifically, the processing shown in FIG. 8 is performed.

In FIG. 8, in step 601, it is determined whether or not the absolute azimuth accuracy is lower than the accuracy expected by the geomagnetic sensor. Here, the absolute orientation accuracy can be grasped by σ _AA ² in the error covariance matrix. In addition, the accuracy that can be expected with the geomagnetic sensor can be obtained by experiments, etc., and the value obtained by experiments etc. can be used for comparison judgment, but if the vehicle is magnetized, that value can be You may make it change according to it. Therefore, in this step 601, σ _AA ² and its comparison value are compared.

The determination in step 601 is YES.
Then, in step 602, the absolute direction A is determined by the geomagnetic sensor 7
Correct to the direction detected in. After this, step 603
Of the error covariance matrix shown in Equation 15_AA ²Geomagnetic
When setting the dispersion value of the azimuth accuracy that can be expected from the sensor,
, Σ_AG ², Σ_AK ², Σ_AY ², Σ_AX ², Σ_GA ², Σ _KA
², Σ_YA ², Σ_XA ²Each value of is set to 0.

By the control as described above, the geomagnetic sensor 7
It is possible to reflect the effect of the correction by the accuracy prediction value, and it is possible to perform the normal correction even when the GPS 3 is positioned next time and the correction is performed. Next, the correction of the gyro offset will be described. When the GPS 3 cannot perform positioning for a long time, the offset correction accuracy of dead reckoning decreases, and the accuracy is lower than the offset correction accuracy expected by the gyro offset correction performed when the vehicle is stopped, when the offset correction by the vehicle stop becomes possible. , Update dead reckoning offset correction amount.

At this time, the offset correction error dispersion portion of the error covariance matrix is reduced to a value that can be expected by the correction when the vehicle is stopped. Also, since offset correction is performed when the vehicle is stopped, there is no correlation between the offset correction error and the error of the other state quantities, so the offset correction error of the error covariance matrix and the other state quantities are not correlated. Clear (set to 0) the cross-correlation value of the dispersion term.

Specifically, the processing shown in FIG. 9 is performed. Figure 9
In step 701, it is determined whether the offset correction accuracy is lower than the accuracy that can be expected in the offset correction when the vehicle is stopped. Here, the offset correction accuracy can be grasped by σ _GG ² in the error covariance matrix.
Further, since the accuracy that can be expected in the offset correction when the vehicle is stopped can be a value obtained by an experiment or the like, σ _GG ² and its comparison value are compared.

When the determination in step 701 is YES, it is determined in step 702 when the vehicle is stopped, that is, the vehicle is stopped based on the signal from the vehicle speed sensor 1, and the output of the gyro 3 at this time is obtained and the value is determined. The offset correction amount is corrected to a value such that After this, step 70
Of the error covariance matrix shown in Equation 3, σ _GG ² is set to the variance value of the offset correction accuracy that can be expected in the offset correction when the vehicle is stopped, and σ _GA ² , σ _GK ² , and σ _GY
² , each value of σ _GX ² , σ _AG ² , σ _KG ² , σ _YG ² , and σ _XG ² is set to 0.

By the control as described above, the effect of the offset correction when the vehicle is stopped can be accurately reflected in the value, and the normal correction can be performed even when the GPS 3 can be positioned next and the correction is performed. Become.

[Brief description of drawings]

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

FIG. 2 is a flowchart showing a calculation process of a main routine of dead reckoning.

FIG. 3 is a flowchart showing a calculation process of a direction change amount / movement distance.

FIG. 4 is a flowchart showing a relative locus calculation process.

FIG. 5 is a flowchart showing a calculation process of absolute azimuth and absolute position.

FIG. 6 is a flowchart showing a combination process with GPS.

FIG. 7 is a configuration diagram showing a model of a Kalman filter.

FIG. 8 is a flowchart showing a process of correcting an absolute azimuth using a geomagnetic azimuth.

FIG. 9 is a flowchart showing a process of performing offset correction when the vehicle is stopped.

[Explanation of symbols]

 1 Distance Sensor 2 Relative Direction Sensor 3 GPS 4 Relative Trajectory Calculation Section 5 Absolute Position Calculation Section 6 Kalman Filter 7 Geomagnetic Sensor

Claims (6)

[Claims] 1. A relative azimuth sensor that outputs a signal according to the amount of change in the azimuth of the vehicle, a moving distance detecting unit that detects a moving distance of the vehicle, and an offset correction amount based on a signal from the relative azimuth sensor. A position detecting means for correcting the azimuth change amount, identifying the azimuth of the vehicle based on the azimuth change amount, and detecting the position of the vehicle based on the azimuth of the vehicle and the moving distance of the vehicle detected by the distance detecting means. Dead reckoning means composed of, a GPS that receives satellite radio waves from GPS satellites, and outputs information regarding the heading of the vehicle, and at least an offset error and a heading error of the vehicle are defined as state quantities X (t), and this error By the process matrix φ that gives a temporal change of and the noise ω generated in the signal generation process,
A signal generation process relating the state quantity X (t + 1) with φ · X (t) + ω, and the state quantity X (t) and the observed value Y (t) are observed matrix H and noise v generated in the observation process. Thus, based on a model forming an observation process in which the observed value Y (t) is related by H · X (t) + v, information on the vehicle direction in the dead reckoning means and the vehicle direction output from the GPS are obtained. And a state quantity calculating means for calculating the state quantity X (t) from the model based on the calculated value and the observed value Y (t). Correction means for correcting the offset correction amount and the vehicle orientation in the position detecting means, and an absolute orientation for detecting the absolute orientation of the vehicle, based on the offset error and the vehicle orientation error based on the obtained state quantity X (t). Sen And changing the azimuth of the vehicle specified by the position detecting means to the azimuth detected by the absolute azimuth sensor when detecting a state where the accuracy of the azimuth of the vehicle specified by the position detecting means is lowered. And a vehicle current position detection device. 2. The state quantity calculating means has means for calculating an error covariance matrix composed of at least the magnitudes of the offset error and the heading error of the vehicle and cross-correlation values of those errors. , The state quantity X
The calculation of (t) is performed, and the changing unit detects a state in which the accuracy of the vehicle azimuth is lowered by estimating the magnitude of the azimuth error of the vehicle. Current position detection device for vehicle. 3. The change means sets an azimuth error of the vehicle in the error covariance matrix when the azimuth of the vehicle specified by the position detection means is set to the azimuth detected by the absolute azimuth sensor. 3. The vehicle current position detection device according to claim 2, further comprising means for setting a cross-correlation value having a correlation to zero. 4. A relative direction sensor that outputs a signal according to the amount of change in the direction of the vehicle, a moving distance detecting unit that detects a moving distance of the vehicle, and an offset correction amount based on a signal from the relative direction sensor. A position detecting means for correcting the azimuth change amount, identifying the azimuth of the vehicle based on the azimuth change amount, and detecting the position of the vehicle based on the azimuth of the vehicle and the moving distance of the vehicle detected by the distance detecting means. Dead reckoning means composed of, a GPS that receives satellite radio waves from GPS satellites, and outputs information regarding the heading of the vehicle, and at least an offset error and a heading error of the vehicle are defined as state quantities X (t), and this error By the process matrix φ that gives a temporal change of and the noise ω generated in the signal generation process,
A signal generation process relating the state quantity X (t + 1) with φ · X (t) + ω, and the state quantity X (t) and the observed value Y (t) are observed matrix H and noise v generated in the observation process. Thus, based on a model forming an observation process in which the observed value Y (t) is related by H · X (t) + v, information on the vehicle direction in the dead reckoning means and the vehicle direction output from the GPS are obtained. And a state quantity calculating means for calculating the state quantity X (t) from the model based on the calculated value and the observed value Y (t). A correction unit that corrects the offset correction amount and the vehicle direction in the position detection unit based on the offset error and the vehicle direction error based on the obtained state quantity X (t), and the accuracy of the offset correction is reduced. The upon detecting, based on an output from the relative direction sensor when the vehicle stops, the vehicle current position detecting device characterized by comprising a changing means for changing the offset correction amount. 5. The state quantity calculating means has means for calculating an error covariance matrix composed of at least the magnitudes of the offset error and the heading error of the vehicle and cross-correlation values of those errors. , The state quantity X
5. The vehicle according to claim 4, wherein the changing means detects the state in which the accuracy of the offset correction is lowered by performing the calculation of (t), and estimating the magnitude of the offset error. Current position detection device. 6. The changing means has means for setting to 0 a cross-correlation value having a correlation with the offset error in the error covariance matrix when changing the offset correction amount. 5. The vehicle current position detection device according to item 5.