JP5569276B2 - Current position detection device for vehicle - Google Patents

Description

  The present invention relates to a vehicle current position detection device that detects a current position of a vehicle based on a traveling direction and a travel distance of the vehicle.

  With regard to a technique for correcting output information of an angular velocity sensor (orientation sensor) such as a gyroscope, for example, Patent Document 1 discloses a technique relating to the following vehicle current position detection device. The vehicle current position detection device includes a gyroscope (hereinafter referred to as a gyroscope) that outputs a signal corresponding to the rotational angular velocity of the vehicle, a speed sensor that outputs a pulse signal corresponding to the traveling speed of the vehicle, a GPS (Global Positioning System) A GPS receiver that receives radio waves transmitted from satellites and outputs the position, direction (traveling direction), speed, etc. of the vehicle, and the current position of the vehicle based on outputs from the gyro, speed sensor, and GPS receiver And a current position detection unit that detects data such as a traveling direction (data for performing dead reckoning navigation).

The current position detecting unit calculates a moving distance of the vehicle based on the pulse signal from the speed sensor, and calculates a moving amount of the vehicle based on the detection signal from the gyro. A relative trajectory calculation unit that calculates the relative trajectory and vehicle speed of the vehicle based on the calculated azimuth change amount and travel distance, and similarly calculates the absolute azimuth and absolute position of the vehicle based on the azimuth change amount and travel distance. The absolute position calculation unit to be calculated, and the difference between the calculated value in the relative locus calculation unit and the absolute position calculation unit and the detection value in the GPS receiver are calculated as observation values, and used for calculation of vehicle speed, direction, and position. An error estimator composed of a Kalman filter that obtains an estimated value of the state quantity using the error of the calculation parameter or calculation result as a state quantity, and an estimated value of the state quantity (that is, error) calculated by the error estimator I, and a correction unit for correcting the calculated parameters and calculated values at each operation unit. The error estimation unit estimates a conversion gain error (gain error) indicating a conversion ratio from the gyro output to the rotation angular velocity as one of the state quantities. Then, the correction unit corrects the azimuth change amount calculated using the gyro output and the conversion gain, based on the estimated gain error value obtained by the error estimation unit.
By the way, in this type of device, when the vehicle continues to travel straight (when the vehicle does not turn), the rotational angular velocity does not occur in the vehicle, so the error in the conversion gain cannot be estimated appropriately. As a result, the conversion gain may be corrected more than necessary and accuracy may be reduced.

JP 2000-55678 A

  Accordingly, an object of the present invention is to provide a vehicle current position detection device capable of accurately correcting a conversion gain indicating a ratio for converting an output value of an angular velocity sensor into a rotation angular velocity of a vehicle.

According to the vehicle current position detection device of the first aspect, the angular velocity sensor outputs a signal corresponding to the rotational angular velocity of the vehicle. The speed sensor outputs a signal corresponding to the traveling speed of the vehicle. The azimuth change amount calculation means calculates a azimuth change amount of the vehicle based on the output value of the angular velocity sensor using a conversion gain indicating a ratio for converting the output value of the angular velocity sensor to the rotational angular velocity as a calculation parameter. The moving distance calculating means calculates the moving distance of the vehicle based on the output value of the speed sensor. The current position calculation means calculates the current position and the traveling direction of the vehicle based on the azimuth change amount calculated by the azimuth change amount calculation means and the movement distance calculated by the movement distance calculation means. The GPS receiver receives radio waves from GPS satellites and outputs the current position and traveling direction of the vehicle. The error estimation means is provided in the current position calculation means, calculates a difference between an output value from the GPS receiver and a calculation value in the current position calculation means as an observation value, and calculates the observation value and the conversion gain. Based on the error covariance value , the error of the conversion gain is estimated as a state quantity. The correcting unit corrects the conversion gain based on the state quantity estimated by the error estimating unit. When the predicted value of the error covariance value of the conversion gain is larger than a predetermined value, the error covariance value of the conversion gain is replaced with the predetermined value.
Thereby, the conversion gain is not corrected more than necessary, and the conversion gain can be corrected with high accuracy.

According to the vehicle current position detection device of the second aspect, the error generation factor determination means determines the generation factor of the error of the conversion gain. The predetermined value is set based on the generation factor determined by the error generation factor determination means.
Accordingly, the degree of correction amount of the conversion gain is set for each error generation factor based on the generation factor of the conversion gain error, so that the conversion gain can be corrected with higher accuracy.

The block diagram which concerns on one Embodiment of this invention and shows the structure of the present position detection apparatus for vehicles. Diagram showing an overview of the Kalman filter Flowchart showing the contents of dead reckoning processing Flow chart showing the contents of calculation processing of azimuth change / movement distance Flow chart showing the contents of relative locus calculation processing Flow chart showing the details of absolute azimuth and absolute position calculation processing Flow chart showing the contents of the composite processing with GPS Flow chart showing the content of learning level update processing Diagram showing the relationship between system noise and error covariance The figure which shows the determinant equivalent to (12) Formula The figure which shows the determinant equivalent to (13) Formula Diagram showing the determinant that defines the noise generated during the observation process Diagram showing the determinant that defines the error covariance Explanatory drawing showing the relationship between the output value of the gyroscope and the rotational angular velocity and the error correction procedure

  Hereinafter, an embodiment of the present invention will be described with reference to the drawings. As shown in FIG. 1, a navigation device 10 (corresponding to a vehicle current position detection device) is applied to a vehicle with a vehicle speed sensor 11 (corresponding to a speed sensor) that outputs a pulse signal at intervals according to the traveling speed of the vehicle. A gyroscope 12 (corresponding to an angular velocity sensor, hereinafter simply referred to as a gyro) that outputs a detection signal corresponding to the angular velocity of the rotational motion and a radio wave (satellite radio wave) transmitted from a GPS (Global Positioning System) artificial satellite. A GPS receiver 13 which receives via a GPS antenna (not shown) and outputs the absolute position, direction (traveling direction), speed, etc. of the vehicle, a vehicle speed sensor 11, a gyro 12 and a GPS receiver 13 A current position detection unit 14 (corresponding to current position calculation means) for detecting data such as the current position and traveling direction of the vehicle (data for performing dead reckoning navigation) based on the output; A navigation execution unit 15 that displays the position of the host vehicle on a map on a display screen (not shown) based on the detection result of the current position detection unit 14 and provides route guidance to a set destination; It has. The current position detection unit 14 and the navigation execution unit 15 are realized as processing of an electronic control unit (ECU) that is configured with a known microcomputer as the center.

  The current position detecting unit 14 calculates a moving distance of the vehicle based on a pulse signal from the vehicle speed sensor 11 (corresponding to a moving distance calculating unit) and a detection signal from the gyro 12 An azimuth change amount calculation unit 22 (which corresponds to an azimuth change amount calculation unit) that calculates an azimuth change amount, and a relative trajectory calculation unit 23 that calculates a relative locus and a vehicle speed of the vehicle based on the calculated azimuth change amount and movement distance. Similarly, the absolute position calculation unit 24 that calculates the absolute azimuth and absolute position of the vehicle based on the azimuth change amount and the movement distance, the calculated values in the relative locus calculation unit 23 and the absolute position calculation unit 24, and the GPS receiver 13 The difference between the detected value and the detected value at is calculated as an observed value, and the calculation parameters used for calculating the vehicle speed, heading, and position, and errors in the calculation results are used as the state quantity. In accordance with the estimated value of the state quantity (that is, error) calculated by the error estimation unit 25 (corresponding to the error estimation unit) composed of a filter and the error estimation unit 25, the calculation parameters and the calculation values in the respective calculation units 21 to 24 are corrected. And a correction unit 26 (corresponding to correction means).

  Here, an outline of the Kalman filter constituting the error estimation unit 25 will be described. As shown in FIG. 2, the Kalman filter includes a signal generation process and an observation process. That is, the Kalman filter can observe a part of the state X (t) related by the observation matrix H with respect to the state X (t) of the linear system (signal generation process) defined by the process matrix φ (observation). In the case of (process), the observed value Y (t) is used as an input, and an optimum estimated value of the state X (t) is given. Note that ω is noise generated in the signal generation process, and ν is noise generated in the observation process.

And the optimal estimated value of the state X using the information until the time t, that is, the state quantity X (t | t) can be obtained by the following equations (1) to (5).
[Formula (1)]
X (t | t) = X (t | t−1) + K (t) [Y (t) −HX (t | t−1)]
[Formula (2)]
X (t | t−1) = φX (t−1 | t−1)
[Formula (3)]
K (t) = P (t | t−1) H ^T [HP (t | t−1) H ^T + V] ⁻¹
[Formula (4)]
P (t | t−1) = φP (t−1 | t−1) φ ^T + W
[Formula (5)]
P (t-1 | t-1) = [I-K (t-1) H] P (t-1 | t-2)
K is the Kalman gain, P is the covariance of the state quantity X (hereinafter referred to as error covariance), V is the variance of noise ν generated in the observation process, and W is generated in the signal generation process. The variance of the noise ω. A (i | j) is an estimated value of A at time i based on information up to time j. The noises ω and ν are both white Gaussian noises with an average of 0 and are not correlated with each other. Also, I means a unit matrix, the subscript “T” means a transposed matrix, and the subscript “−1” means an inverse matrix. W means system noise in this system.

  In the Kalman filter defined in this way, the state quantity X is defined as an appropriate initial state, an appropriate initial value is given to the error covariance P, and then the above calculation is performed based on the observed value Y. An optimum estimated value X (t | t) of the state quantity is obtained. Then, by repeating the calculation each time observation is performed and the observed value Y is obtained, the accuracy of the optimum estimated value X (t | t) of the state quantity is improved.

In this embodiment, as the state quantity X, offset error (εG), gain error (εA), distance coefficient error (εK), absolute azimuth error (εθ), absolute position north direction error (εY), absolute position east Six error values of direction error (εX) are used.
The offset error (εG) is an error that is added evenly in the entire region of the output value regardless of the magnitude of the output value of the gyro 12. The gain error (εA) is an error of a conversion gain set as a ratio for converting (converting) the output value of the gyro 12 into the rotational angular velocity. The distance coefficient error (εK) is an error of the distance coefficient that is multiplied by the number of pulses per unit time output from the vehicle speed sensor 11 when calculating the travel distance. The absolute azimuth error (εθ) is an error given based on the above-described offset error (εG), gain error (εA), distance coefficient error (εK), and the like when calculating the absolute azimuth. The absolute position north direction error (εY) and the absolute position east direction error (εX) are errors similarly given when calculating the absolute position. These error values εG, εA, εK, εθ, εY, εX are defined by the following equations (6) to (11).
[Formula (6)]
εG _t = εG _t-1 + ω ₀
[Formula (7)]
εA _t = εA _t-1 + ω ₁
[Formula (8)]
εK _t = εK _t-1 + ω ₂
[Formula (9)]
εθ _t = T _T · εG _t-1 + D _T · εA _t-1 + εθ _t-1 + ω ₃
[Formula (10)]
εY _t = sin (θ) · L _T · (1 + εK _t-1 ) −sin (θ _T ) · L _T + εY _t-1
[Formula (11)]
εX _t = cos (θ) · L _T · (1 + εK _t-1 ) −cos (θ _T ) · L _T + εX _t-1

Is defined by the following equation.
θ = θ _T + εθ _t-1 + T _T · εG _t-1 / 2 + D _T · εA _t-1 / 2
Also, T _T is the elapsed time from the previous time (previous observation), D _T is the direction change from the previous time, L _T is the movement distance from the previous time, and θ _T is the true absolute direction. .
Further, ω ₀ is an offset variation due to temperature drift or the like (referred to as offset noise), ω ₁ is a gyro gain variation due to temperature drift or the like (referred to as gain noise), and ω ₂ is the vehicle speed sensor 11. A variation due to secular change (referred to as distance coefficient noise), and ω ₃ represents a variation due to cross coupling of the gyro 12 or the like (referred to as absolute azimuth noise).

As shown in equations (6) to (8), there is no definite change in the offset error (εG), gain error (εA), and distance coefficient error (εK). _{0 to} ω ₂ are added. As shown in the equation (9), with respect to the absolute azimuth error (εθ), the previous error includes the azimuth error based on the offset error (εG), the azimuth error based on the gain error (εA), and the noise ω _3. Is added. Further, as shown in the equations (10) and (11), for the absolute position errors (εY) and (εX), the azimuth error based on the offset error (εG) and the absolute azimuth error (εθ) is added to the previous error. And an error caused by a distance error based on the distance coefficient error (εK) is added. Further, theta is meant absolute direction calculated by the absolute position computing section 24, which is what was added an error based on the sensor error to the true absolute direction theta _T.

The process matrix φ defines the temporal change of these error values εG, εA, εK, εθ, εY, εX, and the above equations (6) to (11) are partially differentiated with respect to each error value. It is obtained by linearization. As a result, the signal generation process is defined by the following equation (12), that is, the determinant shown in FIG.
[Formula (12)]
X (t + 1) = φ · X (t) + W

The observed value Y (t) includes a calculated value (hereinafter referred to as dead reckoning data) in the relative locus calculating unit 23 and the absolute position calculating unit 24 and a detected value (hereinafter referred to as GPS positioning data) in the GPS receiver 13. It is calculated | required from the difference with this. Since each output includes an error, the sum of the error of dead reckoning data and the error of GPS positioning data is obtained in the observed value Y (t). By relating the observed value Y (t) and the state quantity X (t), the observation process is defined by the following equation (13), that is, the determinant shown in FIG.
[Formula (13)]
Y (t) = H · X (t) + V
Note that the noise ν generated in the observation process is noise included in the detection value from the GPS receiver 13, and is defined by a determinant shown in FIG.

In the current position detecting unit 14 having such a Kalman filter, the moving distance calculating unit 21 calculates the moving distance of the vehicle based on the signal from the vehicle speed sensor 11, and the azimuth change calculating unit 22 is based on the signal from the gyro 12. To calculate the direction change amount of the vehicle. Then, based on the calculated moving distance and azimuth change amount, the relative trajectory calculation unit 23 and the absolute position calculation unit 24 calculate vehicle speed, relative trajectory, absolute position, and absolute azimuth as dead reckoning navigation data. On the other hand, the GPS receiver 13 outputs the position, direction, and vehicle speed as needed as GPS positioning data.
Then, the error estimation unit 25 (Kalman filter) calculates an estimated value of each error εG, εA, εK, εθ, εY, εX based on the dead reckoning navigation data and the GPS positioning data. Based on this estimated value, the correction unit 26 corrects the calculation parameters and calculation values in the respective calculation units 11 to 14.

By the way, when the distance coefficient correction of the vehicle speed sensor 11, the gyro 12 offset correction, the gain correction, the absolute azimuth correction, and the absolute position correction are performed based on the estimation error as described above, the state quantity shown in the equation (2) The estimated value X (t | t−1) is zero. That is, the equation (1) is as shown in the following equation (1a), and the six error values defined as the state quantity X as described above are obtained by the equation (1a) and the above-described equations (3) to (3). 5) is calculated by the equation.
[Formula (1a)]
X (t | t) = X (t | t−1) + K (t) Y (t)

Here, the error covariance P used in the equations (3) to (5) is defined as shown in FIG.
Note that σ _GG ² , σ _AA ² , σ _KK ² , σθθ ² , σ _YY ² , and σ _XX ² shown in FIG. 13 are estimates of the sizes of the errors εG, εA, εK, εθ, εY, and εX, respectively. It represents. In addition, σ _ij ² other than these is a cross-correlation value between i rows and j columns. For example, σ _AG ² represents a cross-correlation value between a gain error (εA) and an offset error (εG). is there.
This error covariance P is updated by the above equation (4). However, in the initial value P (0 | 0) of the error covariance, the diagonal element σ _ij ² (i = j) is set to a value that maximizes the error, and the other elements σ _ij ² (i ≠ j ) Are all set to 0. Further, the matrix H in the equation (3) uses the one defined in the equation (13). Similarly, the matrix V in equation (3) is defined by the determinant shown in FIG. Further, the matrix W in the equation (4) uses the one defined in the equation (12).

Next, as the observed value Y (t) in the observation process, as shown in FIG. 11, εK _DRt −εK _GPSt , εθ _DRt −εθ _GPSt , εY _DRt −εY _GPSt , and εX _DRt −εX _GPSt are used. Here, the subscript “DRt” means a value based on dead reckoning data at time t, and the subscript “GPSt” means a value based on GPS positioning data at time t.
εK _DRt −εK _GPSt is a distance coefficient error obtained from the difference between the speed of dead reckoning data and the speed of GPS positioning data. Specifically, {(speed of dead reckoning data) − (speed of GPS positioning data) } / (Speed of dead reckoning data).
εθ _DRt −εθ _GPSt is obtained by taking the difference between the absolute azimuth of dead reckoning navigation data and the azimuth of GPS positioning data. That is, the absolute azimuth of dead reckoning data includes the true absolute azimuth θ _T and its error εθ _DRt , and the azimuth of GPS positioning data includes the true absolute azimuth θ _T and its error εθ _GPSt. ing. Therefore, εθ _DRt −εθ _GPSt is obtained by taking the difference between them.
εY _DRt −εY _GPSt is the difference in error between the Y component of the absolute position of dead reckoning navigation data and the Y component of the position of GPS positioning data.
εX _DRt −εX _GPSt is a difference in error between the X component of the absolute position of dead reckoning navigation data and the X component of GPS positioning data.

Furthermore, the noise ν generated in the observation process defined by the determinant shown in FIG. 12 is the noise in the GPS receiver 13 and is obtained as follows. That is, the positioning accuracy is obtained by UERE × HDOP based on the relationship between the pseudorange measurement error (UERE) and the HDOP (Horizontal Dilution of Presision) in the GPS receiver 13. Then, ν _2t and ν _3t are obtained by squaring the positioning accuracy. Further, the speed accuracy is obtained from the relationship between the measurement error of the Doppler frequency and HDOP by the measurement error of Doppler frequency × HDOP. Then, a distance coefficient measurement error is obtained from this speed accuracy / vehicle speed, and ν _1t is obtained by squaring this. Furthermore, azimuth | direction precision is calculated | required from vehicle speed Vc and speed accuracy. That is, the azimuth accuracy is obtained by tan ⁻¹ (speed accuracy / Vc). Then, ν _0t is obtained by squaring this azimuth accuracy.

Therefore, εK _DRt −εK _GPSt , εθ _DRt −εθ _GPSt , εY _DRt −εY _GPSt , εX _DRt −εX _GPSt and the noise V in the observation process are input, and the above-described equations (3) to (5) and (1) By executing the equation, a state quantity X consisting of six error values εG, εA, εK, εθ, εY, εX defined in the signal generation process is obtained. As a result, the distance coefficient correction of the vehicle speed sensor 11, the offset correction and gain correction of the gyro 12, and the absolute position correction and absolute azimuth correction are performed.

As described above, the current position detection unit 14 is realized as a microcomputer process. Hereinafter, dead-reckoning processing executed by the current position detection unit 14 will be described with reference to flowcharts shown in FIGS. This process is repeatedly executed at a constant cycle TM.
As shown in FIG. 3, first, in step A <b> 1, the current position detection unit 14 performs an azimuth change amount / movement distance calculation process by the movement distance calculation unit 21 and the azimuth change amount calculation unit 22. Details of this calculation processing will be described with reference to FIG. That is, in step B1, the azimuth change amount is calculated by multiplying the gyro output angular velocity detected by the gyro 12 by the activation cycle TM of the main routine. As is apparent from the graph of FIG. 14A, the gyro output angular velocity is obtained by subtracting the offset value (in this case, 2.5 V) from the value obtained by sampling the output of the gyro 12, and the conversion gain (in the graph). (Equivalent to the slope).
Next, in step B2, offset correction of the azimuth change amount is performed by subtracting an offset correction amount, which will be described later, from the azimuth change amount obtained in step B1 by multiplying the main routine startup period TM. That is, as shown in FIG. 14B, the error based on the fluctuation of the output voltage (zero point) of the gyro 12 corresponding to the angular velocity 0 [deg / sec] is corrected.
Next, in step B3, gain correction of the azimuth change amount is performed by multiplying the azimuth change amount offset-corrected in step B2 by a gain correction amount described later. That is, as shown in FIG. 14C, the error based on the fluctuation of the slope (converted gain) of the graph is corrected.
Next, in step B4, the moving distance is calculated by multiplying the number of output pulses from the vehicle speed sensor 11 detected during the period from when this process was last activated until this time, by a distance coefficient described later. Thus, the azimuth change amount / movement distance calculation processing is completed.

When the calculation processing of the azimuth change amount / movement distance is completed, the current position detection unit 14 proceeds to step A2 in FIG. 3 and the relative trajectory calculation unit 23 executes the relative trajectory calculation processing.
Details of this calculation processing will be described with reference to FIG. That is, in step C1, the relative orientation is updated by adding the amount of change in orientation calculated in the previous step B3 to the relative orientation obtained so far. Next, in Step C2, the relative position coordinates are updated based on the updated relative orientation and the movement distance calculated in the previous Step B4. Specifically, the relative coordinates rel. x is updated according to the following equation (16), and the relative coordinates rel. y is updated according to the following equation (17). Here, θ is the relative orientation calculated in step C1.
[Formula (16)]
rel. x ← rel. x + travel distance × cos θ
[Formula (17)]
rel. y ← rel. y + travel distance × sin θ

That is, this update is performed by adding the x and y components of the relative azimuth to the movement distance to the relative position coordinates so far. The relative position coordinates are used to obtain a relative locus, and so-called map matching is performed based on the relationship between the relative locus and the road shape.
Next, in step C3, the vehicle speed is calculated by dividing the movement distance calculated in step B4 by the activation cycle TM of the main routine. The relative locus calculation process is thus completed.

When the relative locus calculation process is completed, the current position detection unit 14 proceeds to step A3 in FIG. 3, and the absolute position calculation unit 24 performs absolute azimuth / absolute position calculation processing.
Details of this calculation processing will be described with reference to FIG. That is, in Step D1, the absolute azimuth is updated by adding the azimuth change amount calculated in the previous Step B3 to the absolute azimuth obtained so far. Next, in step D2, the absolute position coordinates are updated based on the updated absolute azimuth and the movement distance calculated in the previous step B4. Specifically, absolute coordinates abs. x is updated according to the following equation (18), and the absolute coordinates abs. y is updated according to the following equation (19). However, (theta) is an absolute azimuth | direction calculated in step D1.
[Formula (18)]
abs. x ← abs. x + travel distance × cos θ
[Formula (19)]
abs. y ← abs. y + travel distance × sin θ

  That is, this update is performed by adding the x and y components of the absolute direction with respect to the moving distance to the absolute position coordinates so far. The absolute azimuth and absolute position updated in this way are used in a combination process with GPS described later.

When the absolute azimuth / absolute position calculation process is completed, the current position detection unit 14 proceeds to step A4 in FIG. 3, and the error estimation unit 25 and the correction unit 26 perform a GPS combination process. Details of the composite processing will be described with reference to FIG. First, in step E1, has the current position detection unit 14 passed T1 seconds since the Kalman filter process (steps E3 to E12) or the prediction calculation process (steps E13 to E17) based on GPS positioning data described later is executed last time? Determine whether or not. If the current position detection unit 14 makes a negative determination (NO), it proceeds to step E2.
In step E2, the current position detection unit 14 determines whether or not GPS positioning data is input from the GPS receiver 13 when the GPS receiver 13 performs positioning. If the current position detection unit 14 makes an affirmative determination (YES), the current position detection unit 14 proceeds to step E3 and starts processing by the error estimation unit 25 (Kalman filter). On the other hand, if the current position detection unit 14 makes a negative determination (NO), the present process ends.

In step E3, the current position detection unit 14 receives GPS positioning data (speed, position, direction) from the GPS receiver 13, and dead reckoning data calculated by the previous dead reckoning process (vehicle speed calculated in step C3, Based on the absolute position updated in step D2 and the absolute orientation updated in step D1, the observed value Y (εK _DRt −εK _GPSt , εθ _DRt −εθ _GPSt , εY _DRt −εY _GPSt , εX _DRt −εX _GPSt ) is calculated. Do. At the same time, the noise ν generated in the observation process is calculated based on the GPS positioning data of the GPS receiver 13 and the like.
Next, the current position detection unit 14 proceeds to Step E4 and calculates the process matrix φ. In this step E4, the movement distance L from the time of calculation of the previous process matrix, the elapsed time T (these are separately obtained by measurement processing not shown), and the absolute azimuth θ obtained in step D1, The process matrix φ shown in the equation (12) is obtained.

  Based on the observed value Y and the process matrix φ calculated in this way, the following equations (3) to (5) are calculated to obtain the state quantity X shown in equation (1a). First, the current position detection unit 14 proceeds to Step E5 and executes a learning degree update process. Details of the update processing will be described with reference to FIG. That is, in step F1, the current position detection unit 14 determines whether or not the rotational angular velocity detected by the gyro 12 is equal to or greater than the threshold value W1. If the current position detection unit 14 makes an affirmative determination (YES), the current position detection unit 14 proceeds to step F2 and updates the cumulative number of turns. The cumulative number of turns is the cumulative number of turns of the vehicle after the gyro 12 is attached to the vehicle (for example, after shipment of the vehicle) (the number of times that the rotational angular velocity detected by the gyro 12 is equal to or greater than the threshold value W1). A value is stored. Then, when the current position detection unit 14 updates the cumulative number of turns, the current position detection unit 14 proceeds to Step F3. If the current position detection unit 14 makes a negative determination in step F1 (NO), the current position detection unit 14 proceeds to step F3 without updating the cumulative number of turns.

In step F3, the current position detection unit 14 determines whether or not the cumulative number of turns is equal to or greater than the threshold value N1 (corresponding to error generation factor determination means). If the current position detection unit 14 makes a negative determination (NO), the process proceeds to step F4, where the system noise W is set to Qs1, and the upper limit value Pmax of the error covariance P is set to Ps1.
On the other hand, if an affirmative determination is made in step F3 (YES), the current position detection unit 14 proceeds to step F5 and determines whether or not T2 time has elapsed since the navigation device 10 was started (error occurrence). Factor determination means). If the current position detection unit 14 makes a negative determination (NO), the process proceeds to step F6, the system noise W is set to Qs2, and the upper limit value Pmax of the error covariance P is set to Ps2.
On the other hand, if an affirmative determination is made in step F5 (YES), the current position detection unit 14 proceeds to step F7, sets the system noise W to Qs3, and sets the upper limit value Pmax of the error covariance P to Ps3. Set.

  Here, as shown in FIG. 9, Qs1, Qs2, and Qs3 set as the system noise W are set so as to satisfy the relationship of Qs1> Qs2> Qs3. Further, Ps1, Ps2, and Ps3 set as the upper limit value Pmax of the error covariance P are set so as to satisfy the relationship of Ps1> Ps2> Ps3. Note that the error covariance P increases more rapidly as the value of the system noise W increases.

  If the gyro 12 has just been attached to the vehicle, that is, if the cumulative number of turns of the vehicle is small (NO in step F3), the cause of the conversion gain error is the gyro 12 attachment angle. The main error is an error (an error based on the fact that the detection axis of the gyro 12 is installed in an inclined state) or an error of manufacturing variation of the gyro 12 (an error inherent to each product in the process of manufacturing the gyro 12). It becomes a factor. The error caused by such factors (offset error) does not fluctuate, and if a correction is not made, a constant error will continue to occur. However, if a correction is made, it can be eliminated and an error will continue to occur. There is nothing. Therefore, it is desirable to correct such errors as soon as possible. Therefore, the current position detection unit 14 sets the system noise W to be large (system noise W = Qs1) and the upper limit of the error covariance P when the cumulative number of turns of the vehicle is small (NO in step F3). A large value Pmax is set (upper limit value Pmax of error covariance P = Ps1). Thereby, the correction amount of the conversion gain error is increased, and the conversion gain can be corrected quickly.

  On the other hand, if a certain amount of time has elapsed since the gyro 12 was attached to the vehicle and the correction of the conversion gain has been repeated to some extent (YES in step F3), the cause of the conversion gain error is a temperature change. The main factor is an error associated with (an error caused by a temperature change of the gyro 12 itself). The error that occurs due to such factors fluctuates according to the temperature, and even if it is corrected, the error may occur again due to subsequent temperature changes. Therefore, it is desirable to correct such an error as needed according to the actual situation (temperature situation). Therefore, the current position detection unit 14 sets the system noise W to be small (system noise W = Qs2, Qs3) after the correction of the conversion gain is repeated to some extent, and sets the upper limit value Pmax of the error covariance P. A small value is set (upper limit value Pmax of error covariance P = Ps2, Ps3). Thereby, although the correction amount of the error of the conversion gain becomes small, the conversion gain can be finely corrected according to the actual temperature condition.

  Note that the temperature of the gyro 12 starts to gradually increase after the navigation device 10 is activated, and tends to converge almost constant at a certain temperature. Therefore, when a certain amount of time (in this case, T2 time) has not elapsed since the activation (NO in step F5), the current position detection unit 14 is in the process of raising the temperature of the gyro 12 (during temperature change). The system noise W is determined to be larger than Qs3 (system noise W = Qs2), and the upper limit value Pmax of the error covariance P is set larger than Ps3 (the upper limit value of the error covariance P). Pmax = Ps2). Thereby, the correction amount of the error of the conversion gain becomes relatively large, and the conversion gain can be corrected while following the temperature change.

  On the other hand, when a certain amount of time (T2 time) has elapsed since the start of the navigation device 10 (YES in step F5), the current position detection unit 14 determines that the temperature of the gyro 12 has converged substantially constant, System noise W is set smaller than Qs2 (system noise W = Qs3), and upper limit value Pmax of error covariance P is set smaller than Ps2 (upper limit value of error covariance P = Ps3). Thereby, the correction amount of the error of the conversion gain becomes relatively small, and the conversion gain can be corrected with high accuracy according to the converged constant temperature. In addition, when the temperature of the gyroscope 12 converges substantially constant and there is no need to increase the correction amount, the correction amount does not increase more than necessary.

  As described above, when the system noise W and the upper limit value Pmax of the error covariance P are set based on the generation factor of the error of the conversion gain, the current position detection unit 14 ends the learning degree update process. Then, the process proceeds to step E6 in FIG. 7, and the prediction calculation of the error covariance P is performed by the equation (4). Then, the current position detection unit 14 proceeds to step E7, where the calculated error covariance P value is the value of the upper limit value Pmax of the error covariance P set in any of the above steps F4, F6, F7. It is judged whether it is larger than. If an affirmative determination is made (YES), the current position detection unit 14 proceeds to step E8 and replaces the value of the error covariance P with the upper limit value Pmax. Then, the process proceeds to step E9. On the other hand, if the current position detection unit 14 makes a negative determination in step E7 (NO), the current position detection unit 14 proceeds to step E9 without replacing the value of the error co-distribution difference P with the upper limit value Pmax.

  In step E9, the current position detection unit 14 calculates the Kalman gain K according to equation (3). In the subsequent step E10, the current position detection unit 14 calculates the error covariance P using equation (5). In subsequent step E11, the current position detection unit 14 obtains the state quantity X by the equation (1a) based on the calculated Kalman gain K and the observed value Y. This state quantity X includes the offset error (εG), gain error (εA), absolute azimuth error (εθ), distance coefficient error (εK), absolute position northward error (εY), absolute The position east direction error (εX) is shown.

In step E12, correction of dead reckoning error (offset correction of gyro 12, gain correction, vehicle speed sensor 11) based on these error values εG, εA, εK, εθ, εY, εX calculated as state quantity X. (Distance coefficient correction, absolute azimuth correction, and absolute position correction) are performed according to the following equations (20) to (25).
[Formula (20)]
Offset correction amount = Offset correction amount−εG
[Formula (21)]
Gain correction amount = Gain correction amount × (1−εA)
[Formula (22)]
Distance coefficient = Distance coefficient × (1−εK)
[Formula (23)]
Absolute orientation = Absolute orientation-εθ
[Formula (24)]
abs. y (absolute position) = abs. y-εY
[Formula (25)]
abs. x (absolute position) = abs. x-εX

That is, the offset correction amount and gain correction amount used in steps B2 and B3 are corrected by the offset correction and gain correction of the gyro 12. By the distance coefficient correction of the vehicle speed sensor 11, the distance coefficient used in step B4 is corrected. The absolute azimuth θ used in step D1 is corrected by the absolute azimuth correction. The absolute position used in step D2 is corrected by the absolute position correction.
And the said error is corrected at any time by repeating the process of the said steps E3-E12 whenever there exists GPS positioning data from the GPS receiver 13. FIG. Thereby, more accurate dead reckoning navigation data is obtained.

If the GPS positioning data from the GPS receiver 13 cannot be obtained for a long time (T1 seconds or more) and the determination in step E1 is affirmative (YES), the current position detection unit 14 Performs calculation of the process matrix φ (step E13), learning level update processing (step E14), prediction calculation of the error covariance P (step E15), and the error covariance set by the calculated error covariance P If it is larger than the upper limit value Pmax of P, the value of the error covariance P is replaced with the upper limit value Pmax (steps E16, E17). The process in step E14 has the same contents as the learning degree update process in step E5 described above (see FIG. 8).
If GPS positioning data cannot be obtained from the GPS receiver 13, the error will increase unless any correction is made. For this reason, by executing the processing of the above steps E13 to E17, the prediction calculation of the error covariance P, that is, the estimation of the error and the setting of the upper limit value Pmax of the system noise W and the error covariance P are periodically performed. Go to. Thereby, the Kalman filter process performed when the input of GPS positioning data from the GPS receiver 13 is subsequently resumed can be performed accurately.

As described above, according to this embodiment, when the upper limit value Pmax is set for the error covariance P and the calculated value of the error covariance P is larger than the set upper limit value Pmax, the error covariance is set. The value of the difference P is replaced with the upper limit value Pmax. Thereby, the conversion gain indicating the ratio for converting the output value of the gyro 12 into the rotational angular velocity of the vehicle is not corrected more than necessary, and the conversion gain can be corrected with high accuracy.
Further, the upper limit value Pmax of the error covariance P based on the error generation factor of the conversion gain, in other words, the degree of correction amount of the conversion gain (upper limit value) is set to a different value for each error generation factor. . Thereby, the correction amount of the conversion gain can be set to an amount corresponding to the cause of the error, and the conversion gain can be corrected with higher accuracy.

In addition, this invention is not limited only to one Embodiment mentioned above, For example, it can deform | transform or expand as follows.
Specific values of Qs1, Qs2, and Qs3 set as the system noise W and specific values of Ps1, Ps2, and Ps3 set as the upper limit value Pmax of the error covariance P are, for example, the navigation device 10 and the gyroscope. It can be appropriately changed and set according to the performance of 12 or the like. Further, values such as time T1, T2 and threshold values W1, N1 in dead reckoning processing can be appropriately changed and set.
For example, when the gyro 12 is reattached, that is, when the attachment angle of the gyro 12 changes, the processing of steps F3 and F4 shown in FIG. 8 is performed by resetting the stored cumulative number of turns. That is, the process for correcting the offset error may be executed again.

  In the drawings, 10 is a navigation device (vehicle current position detection device), 11 is a vehicle speed sensor (speed sensor), 12 is a gyroscope (angular velocity sensor), 13 is a GPS receiver, and 14 is a current position detector (current position calculation). Means, error generation factor determination means), 21 is a movement distance calculation section (movement distance calculation means), 22 is an azimuth change amount calculation section (direction change amount calculation means), 25 is an error estimation section (error estimation means), and 26 is A correction part (correction means) is shown.

Claims (2)

An angular velocity sensor that outputs a signal corresponding to the rotational angular velocity of the vehicle;
A speed sensor that outputs a signal corresponding to the traveling speed of the vehicle;
Azimuth change amount calculation means for calculating a azimuth change amount of the vehicle based on the output value of the angular velocity sensor, using a conversion gain indicating a ratio for converting the output value of the angular velocity sensor into the rotation angular velocity as a calculation parameter;
A moving distance calculating means for calculating a moving distance of the vehicle based on an output value of the speed sensor;
Current position calculation means for calculating a current position and a traveling direction of the vehicle based on the azimuth change amount calculated by the azimuth change amount calculation means and the movement distance calculated by the movement distance calculation means;
A GPS receiver that receives radio waves from GPS satellites and outputs the current position and traveling direction of the vehicle;
Provided in the current position calculation means, the difference between the output value from the GPS receiver and the calculated value in the current position calculation means is calculated as an observed value, and the error covariance value of the observed value and the conversion gain is calculated. An error estimating means for estimating an error of the conversion gain as a state quantity based on;
Correction means for correcting the conversion gain based on the state quantity estimated by the error estimation means;
With
When the predicted value of the error covariance value of the conversion gain is larger than a predetermined value, the vehicle current position detection device is configured to replace the error covariance value of the conversion gain with the predetermined value.
An error generation factor determination means for determining an error generation factor of the conversion gain;
2. The vehicle current position detection device according to claim 1, wherein the predetermined value is set based on the generation factor determined by the error generation factor determination means.

Images