JP3321096B2 - Vehicle position measuring device and speed measuring device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a GPS (Glo
bal Positioning System) using the observation information of the receiver and the measurement information of the onboard sensor,
The present invention relates to a vehicle position measuring device and a speed measuring device provided with a compound navigation for calculating a traveling direction of a vehicle, a current position, a speed, and the like.

[0002]

2. Description of the Related Art A conventional vehicle position measuring device will be described. FIG. 11 is a block diagram showing a configuration of a conventional vehicle position measuring device disclosed in, for example, JP-A-8-68655. In the figure, 1 is a distance sensor, 2 is a GPS receiver, 3 is a relative azimuth sensor, 5 is a relative trajectory calculation unit for calculating a relative trajectory based on output signals of the distance sensor 1 and the relative azimuth sensor 3, 6 Is a distance sensor 1 and a relative direction sensor 3
An absolute trajectory calculation unit 7 that calculates an absolute trajectory based on the output signal of the above-mentioned is a Kalman filter that performs each correction of the distance coefficient of the distance sensor 1, the offset of the relative azimuth sensor 3, the absolute position, and the absolute azimuth. The relative trajectory calculation unit 5, the absolute trajectory calculation unit 6, and the Kalman filter 7 are realized by calculation processing by a microcomputer.

The processing procedure of the relative trajectory calculator 5, the absolute trajectory calculator 6, and the Kalman filter 7 will be described below. Here, FIG. 12 is a flowchart showing the calculation processing of the main routine, FIG. 13 is a flowchart showing the calculation processing of the azimuth change and the moving distance, FIG. 6 is a flowchart showing a calculation process of an absolute azimuth and an absolute position in an absolute trajectory calculation unit 6; FIG. 16 is a flowchart showing a calculation process of the Kalman filter 7 for combining dead reckoning navigation and GPS navigation.

First, in step ST1201 of the main routine shown in FIG. 12, a change in the direction of the vehicle and a calculation of a moving distance are performed. FIG. 13 shows details of the processing. In this azimuth change and movement distance calculation processing, step ST13
At 01, first, a change in the direction of the vehicle is obtained from the relative direction sensor 3. Next, in step ST1302, the offset correction of the azimuth change is performed by subtracting the product of the offset correction amount and the moving distance L from the previous time from the azimuth change. Next, in step ST1303, a movement distance is calculated by multiplying a signal (wheel sensor pulse) from the distance sensor 1 by a distance coefficient.

Next, in step ST1202 of the main routine shown in FIG. 12, a relative trajectory calculation process is performed. FIG. 14 shows the details of the processing. In the relative trajectory calculation processing, first, in step ST1401, the relative azimuth is updated based on the azimuth change. This update is performed by adding the azimuth change to the previous relative azimuth. Next, in step ST1402, the relative position coordinates are updated using the updated relative orientation and the moving distance obtained in step ST1303 in FIG. This update is performed by adding the X and Y components of the relative azimuth with respect to the moving distance to the relative position coordinates up to that time.

Next, in step ST1203 of the main routine shown in FIG. 12, arithmetic processing of absolute azimuth and absolute position is performed. FIG. 15 shows details of the processing. In this absolute azimuth / absolute position calculation processing, step ST150
In step 1, first, the absolute azimuth is updated based on the azimuth change. This update is performed by adding the azimuth change to the previous absolute azimuth. Next, in step ST1502, the absolute position coordinates are updated based on the updated absolute azimuth and the moving distance obtained in step ST1303 in FIG. The updating of the absolute position coordinates is performed by adding the X and Y components of the absolute azimuth with respect to the moving distance to the absolute position coordinates up to that time. The absolute azimuth and the absolute position updated in step ST1203 of the main routine shown in FIG. 12 are used in a process of combining with the observation information of the GPS receiver 2 in step ST1204 following the main routine.

FIG. 16 shows the details of the processing in step ST1204 of the main routine for performing the compounding processing with the observation information of the GPS receiver 2. FIG. 16 shows the calculation procedure of the Kalman filter 7. Here, the Kalman filter for combining dead reckoning navigation and GPS navigation will be described first.

The Kalman filter is divided into a signal generation process and an observation process. When a part of the state X (t) of the system related by the observation matrix H can be observed with respect to the state X (t) of the system represented by the linear system, the filter determines the state X (t) of the system. Give the best estimate.
Here, system noise generated in the signal generation process is denoted by ω, and observation noise generated in the observation process is denoted by v. The input of this Kalman filter is the observed value Y (t) and the output is the optimal estimate of the state X (t) of the system. At the time t, the estimated amount X (t | t) of the state vector using the information up to the time t is obtained by the following equation (1).

X (t | t) = X (t | t-1) + K (t) {Y (t) -HX (t | t-1)} (1)

Here, in the above equation (1), X (t |
t-1) is the predicted amount of the state vector at time t-1 at time t-1, and K (t) is the Kalman gain, which are expressed by the following equations (2) and (3), respectively.

X (t | t−1) = FX (t | t−1) (2) K (t) = Σ (t | t−1) H^T{HΣ (t | t-1) H^T+ Σ_V}^-1  ... (3)

In the above equations (2) and (3),
Σ (t−1 | t−1) is the error covariance of the state X of the system at time t−1, and Σ (t | t−1) is the time t
The amount of error covariance prediction at time t at −1. Then, sigma _V is the variance of the measurement noise v generated in the observation process, the prediction of error covariance Σ (t | t-1) , and the state X of the system error covariance Σ (t-1 | t- 1) is represented by the following equations (4) and (5).

Σ (t | t−1) = FX (t−1 | t−1) F^T+ Σ_W  (4) {(t-1 | t-1) = {I−K (t−1) H}} (t−1 | t−2) (5)

Here, in the above equation (4), _{Ｗ W} is the variance of the system noise ω generated in the signal process, and in the equation (5), I is a unit matrix. Further, in Expressions (3) and (4), the suffix ^T indicates a transposed matrix, and ^-1 indicates an inverse matrix. Note that the system noise ω and the observation noise v are white Gaussian noise with an average of 0, and are uncorrelated with each other.

Such a Kalman filter first gives an appropriate error to the state X of the system and the initial value of the error covariance 、, and repeats the above calculation every time a new observation is performed. Here, the definition of the signal generation process will be described first. Since the Kalman filter aims to correct errors in dead reckoning, the matrix elements of the estimated amount X of the state vector include five elements of an offset error, an absolute azimuth error, a distance coefficient error, an absolute position northward error, and an absolute position eastward error. Define one. A state transition matrix F gives a temporal change of these error values.

The offset error εG has no definite change, and is expressed by the following equation (6) assuming that noise is added to the previous error.

ΕG _t = εG _t−1 + ω ₀ (6)

The absolute azimuth error εA is represented by the following equation (7) assuming that the azimuth error and noise obtained by multiplying the previous offset error by the elapsed time from the previous time are added.

ΕA _t = T × εG _t−1 + εA _t−1 + ω ₁ (7)

The distance coefficient error εK has no definite change,
Assuming that noise is added to the previous error, it is expressed by the following equation (8).

ΕK _t = εK _t−1 + ω ₂ (8)

The absolute position northward error εY and the absolute position eastward error εX are obtained by adding an error caused by an azimuth error and a distance error to the previous error, as follows:
And by equation (10). In these equations (9) and (10), 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.

ΕY_t= L × sin (A_t+ ΕA_t-1+ ΕG_t-1× t / 2) (1 + εK_t-1) −L × sin (A_t) + ΕY_t-1  ... (9) εX_t= L × cos (A_t+ ΕA_t-1+ ΕG_t-1× t / 2) (1 + εK_t-1) -L × cos (A_t) + ΕX_t-1  ... (10)

When the equations (9) and (10) are partially differentiated by the state quantity and linearized, the signal generation process is represented by the following equations (11) to (15).

[0025]

[0026] In the above formula (13), A are those applied is sensor error to the true absolute direction A _t, it is determined from the orientation change as described below. Equation (15)
, Ω ₀ is offset noise (offset variation due to temperature drift or the like), ω ₁ is absolute azimuth noise (gyro scale factor error), ω ₂ is distance coefficient noise (aging), ω ₃ and ω ₄ are absolute positions. Means noise.

Next, the observation process will be described. Observed values are obtained from the difference between each output of dead reckoning navigation and GPS navigation. Since each output includes an error, the sum of the error in dead reckoning and the error in GPS observation information is obtained as an observation value. The observed value Y is associated with the system state X, and defined as in the following equations (16) to (19).

[0028]

Incidentally, in the elements of the matrices of the above formulas (17) and (19), the subscript _DRt was obtained by dead reckoning at time t based on the output signals from the distance sensor 1 and the relative azimuth sensor 3. Subscript _GPSt indicates time t
Means a value output from the GPS receiver 2.

The observed value Y shown in the above equation (17)
_t matrix elements (εA _DRt -εA _{G PSt)} is the difference between the azimuth output absolute obtained by dead reckoning heading and from the GPS receiver 2. That is, the absolute azimuth obtained by dead reckoning navigation includes the true absolute azimuth and its error εA.
_DRt is included, and since the azimuth output from the GPS receiver 2 also includes the true absolute azimuth and the error εA _GPSt thereof, the azimuth can be obtained by taking the difference between them.

Further, the observed value Y shown in the above equation (17) is obtained.
The matrix element of _t (εK _DRt −εK _{G PSt} ) is a distance coefficient error obtained from a difference between the speed obtained by dead reckoning navigation and the speed output from the GPS receiver 2, and is expressed as (speed by dead reckoning navigation−speed by GPS receiver). ) / (Speed by dead reckoning). Furthermore, the matrix element of the observed value _{Y t (εY DRt -εY GPSt)} is the difference of the errors of the Y component of the position output from the Y component and the GPS receiver 2 of the absolute position obtained by dead reckoning, matrix elements (ΕX
_{DRt- [} epsilon] _XGPSt ) is the difference between the errors in the X component of the position.

Further, the noise v generated in the observation process is GP
Each matrix element of the observation noise v, which is the observation noise of the S receiver 2 and is shown in the above equation (19), is obtained as follows. First, the measurement error of the Doppler frequency and the HDOP (H
origination Dilution of Pre
The speed accuracy is obtained by the Doppler frequency measurement error × HDOP, and the azimuth accuracy is tan ⁻¹ (speed accuracy /
Vehicle speed). Then, the azimuth accuracy and the speed accuracy are each squared to obtain a matrix element (−εA
_GPSt ) and a matrix element (−εK _GPSt ) are obtained. In addition, the calculation error EURE (User Equivalent R) of the pseudorange in the GPS receiver 2 is performed.
aging error) and HDOP, U
The positioning accuracy is obtained by ERE × HDOP, and the matrix element (−
The εY _GPSt ) and the matrix element (−εX _GPSt ) are obtained by squaring this positioning accuracy.

Next, the processing procedure of the Kalman filter 7 will be described with reference to the flowchart of FIG. First, in step ST1601, it is determined whether or not T1 seconds have elapsed since the last positioning or prediction calculation. Where GP
Every time two-dimensional or three-dimensional positioning is performed by the S receiver 2, a process for correcting an error in dead reckoning is performed in steps ST1603 to ST1609.
If two-dimensional or three-dimensional positioning cannot be performed by the receiver 2, the error increases. This step ST1601 is provided for performing the prediction calculation of the error corresponding thereto periodically in steps ST1610 and ST1611.

That is, if the determination result in step ST1601 is Yes, the process branches to step ST1610 to calculate the state transition matrix F by the above equation (14). Next, in step ST1611, a prediction calculation of the error covariance matrix Σ is performed based on the obtained state transition matrix F, and the processing shown in FIG. 16 is temporarily terminated. If the determination result in step ST1601 is No, it is determined in step ST1602 whether or not there is GPS observation information from the GPS receiver 2. As a result, if there is no GPS observation information from the GPS receiver 2, the processing shown in FIG.

On the other hand, when there is GPS observation information from the GPS receiver 2, the process proceeds to the Kalman filter calculation process after step ST1603. In the Kalman filter calculation process, first, in step ST1603, the observation value Y is calculated. Observed value Y in step ST1603
Is calculated from the GPS observation information on the speed, position, and direction output from the GPS receiver 2, the absolute direction and absolute position obtained in step ST1203 of the main routine in dead reckoning, and the speed not shown. The observation value Y is calculated from the above equation (16) using the speed of the vehicle obtained from the output signal of the distance sensor 1 by the arithmetic processing, and the observation noise v generated in the observation process is also received by GPS. The calculation is performed based on the observation information of the aircraft 2.

Next, in step ST1604, a state transition matrix F is calculated. This calculation processing includes a movement distance L, an elapsed time t,
A state transition matrix F is obtained from the absolute azimuth A obtained in step ST1501 of FIG. 15 by using the above equation (14).

Hereinafter, based on the observation value Y and the state transition matrix F calculated in steps ST1603 and ST1604, the equations (1), (2), (3), (4), and ( The calculation of 5) is performed to obtain the estimated amount X of the state vector. That is, first, in step ST1605, the prediction calculation of the error covariance matrix 行 い is performed, and then, in step ST1606, the Kalman gain K is calculated. Then, in step ST1607, the error covariance matrix 計算 is calculated. In step ST1608, the estimated amount X of the state vector is obtained based on the Kalman gain K and the observed value Y. The estimated amount X of the state vector represents an offset error, an absolute azimuth error, a distance coefficient error, an absolute position northward error, and an absolute position eastward error.

Then, in step ST1609,
These calculations correct the dead reckoning error, that is,
After performing the offset correction of the relative direction sensor 3, the distance coefficient correction of the distance sensor 1, the absolute direction correction, and the absolute position correction,
This series of processing ends once. The offset correction amount of the relative azimuth sensor 3 is obtained by subtracting the offset error εG from the offset correction amount up to that time.
The distance coefficient of the distance sensor 1 is corrected by multiplying the distance coefficient by subtracting the distance coefficient error εK from 1 and multiplying the obtained distance coefficient. In addition, the absolute azimuth is obtained by subtracting the absolute azimuth error εA from the previous absolute azimuth, and the absolute position is obtained by subtracting the absolute position eastward error εX or the absolute position northward from the X component or Y component of the previous absolute azimuth. It is corrected by subtracting the error εY.

The above processing is repeated each time there is GPS observation information from the GPS receiver 2 to correct the error.

Each element of the matrix of the system noise ω generated in the signal generation process represented by the above equation (15), ie, offset noise ω ₀ , absolute azimuth noise ω ₁ , distance coefficient noise ω ₂ , absolute position The noises ω ₃ and ω ₄ are respectively obtained as follows. First, specified values are set for the offset noise ω ₀ and the absolute azimuth noise ω ₁ , and thereafter, the absolute azimuth noise ω ₁ is calculated as a sum of error variances for each predetermined error generation factor generated according to the behavior state of the vehicle. Recalculate. For the absolute position noises ω ₃ and ω ₄ , a value obtained by dividing the magnitude of the position error generation amount due to the absolute azimuth error into the X and Y axes only when the element of the error covariance matrix の is equal to or smaller than a predetermined value Is calculated based on

As described above, in the conventional vehicle position measuring device disclosed in Japanese Patent Laid-Open No. 8-68655, dead-reckoning navigation is performed by obtaining the offset error, the distance coefficient error, the absolute azimuth error, and the absolute position error using the Kalman filter 7. Is performed for each correction. And the Kalman filter 7
The accuracy of the offset error, distance coefficient error, absolute azimuth error, and absolute position error determined by the Kalman filter 7 is determined in accordance with the accuracy of the system error and the observation error in.

[0042]

Since the conventional vehicle position measuring device is constructed as described above, the observation error is high.
Although it is calculated based on the OP and the UERE, since these are indexes for estimating the positioning accuracy by the user, there is a problem that the error at that time cannot be directly expressed and the observation error cannot be set with high accuracy. . For the addition system errors, set the specified value to the absolute azimuth noise omega ₁ and offset noise omega _0, the absolute azimuth noise as the sum of error variance for each predetermined error generation factor generated according to the behavior state of the vehicle omega ₁ the to recalculate not follow the offset error, also there is a problem that can not be set absolute azimuth noise omega ₁ accurately. For the absolute position noises ω ₃ and ω ₄ , the position error is calculated as a value obtained by dividing the absolute azimuth error with low precision into the X and Y axes only when the element relating to the azimuth error of the error covariance matrix is equal to or less than a predetermined value. Therefore, there is a problem that the position error cannot be detected with high accuracy.

In addition, since the offset error, the distance coefficient error, the absolute azimuth error, and the absolute position error are simultaneously calculated as the matrix elements of the state vector, if any one of the matrix errors of the system error and the observation error is incorrect, Has a problem that the Kalman filter 7 cannot perform detection and setting in accordance with the characteristics of each error, because other matrix elements of the state vector are reflected in the value by an amount determined by the Kalman gain K, and an error occurs. .

Further, the accuracy of the speed information observed by the GPS receiver 2 may be reduced when the vehicle is traveling at a low speed. However, no countermeasure for this is shown in the calculation of the distance coefficient error. Since the variance of the speed information observed in 2 is larger than the variance of the speed calculated from the output signal of the distance sensor 1, it is considered that the variance of the calculated distance coefficient error increases. As for the relative direction sensor 3,
Although only the offset error is to be corrected, when a gyro is used as the relative direction sensor 3, it is necessary to consider a scale factor that can vary up to about ± 20% with respect to a standard value. Since the vehicle position measuring device does not take this into account, there has been a problem that an azimuth error caused by a scale factor error is erroneously corrected as an offset error.

The present invention has been made to solve the above-described problems. When the GPS navigation and the autonomous navigation are combined, a single calculation method is used to measure the observation information of the GPS receiver and the measurement of the sensor mounted on the vehicle. Vehicle position measurement that can simultaneously calculate the current position and heading of the vehicle from the information, and can calculate the current position and heading of the vehicle with high accuracy even if errors in the observation information of the GPS receiver cannot be detected with high accuracy. The aim is to obtain a device.

Another object of the present invention is to provide a vehicle position measuring device capable of reliably correcting the azimuth error when the azimuth error of the traveling azimuth of the vehicle is detected.

Further, the present invention uses a relative direction sensor as a direction measuring means of a vehicle-mounted sensor, and detects the influence of the drift on the traveling direction of the vehicle even when it detects that a drift has occurred in the offset of the relative direction sensor. It is an object of the present invention to obtain a vehicle position measuring device that does not cause an azimuth error.

Further, the present invention uses a distance sensor as a means for measuring the speed of the vehicle, and when there is an error in the speed of the vehicle calculated from the output signal of the distance sensor, the error is quickly detected during the running of the vehicle. It is an object of the present invention to obtain a speed measuring device capable of performing correction, and further to obtain a speed measuring device capable of performing more stable correction.

Further, the present invention uses a relative direction sensor as a direction measuring means of a vehicle-mounted sensor, and when there is an error in the offset and scale factor of the relative direction sensor, the error is quickly corrected by one calculation method. Another object of the present invention is to provide a vehicle position measuring device capable of performing more accurate correction.

Further, according to the present invention, the azimuth error caused by the offset error of the relative azimuth sensor increases with time, but the azimuth error caused by the scale factor error occurs according to the turning angle. An object is to obtain a vehicle position measuring device.

[0051]

In a vehicle position measuring apparatus according to the present invention, in a position / azimuth calculating means, a azimuth change calculating means calculates a vehicle calculated from a signal output from a relative azimuth sensor according to a azimuth change of the vehicle. And the distance calculating means calculates the position information and the direction information of the vehicle from the moving distance per unit time of the vehicle calculated from the signal output from the distance sensor according to the moving distance of the vehicle. Based on the system model and an observation model representing the relationship between the position information of the vehicle observed by the GPS receiver using the satellite and the position information in the system model, the position information of the system model is added to the matrix element of the state vector. The azimuth information is set, and a Kalman filter in which the position information observed by the GPS receiver is set is used as the observation value. Positive amount is obtained to perform the calculation of the state vector by adjusting the Kalman gain to be equal to or less than the predetermined value.

In the vehicle position measuring apparatus according to the present invention, the position / azimuth calculating means sets at least an azimuth error in a matrix element of a system error in the Kalman filter and, based on the azimuth error, a traveling azimuth which is an element of a state vector. The calculation of a predetermined value for limiting the amount of correction is performed.

In the vehicle position measuring device according to the present invention, the azimuth change calculating means multiplies the offset of the output signal detected from the output signal of the relative azimuth sensor by the scale factor after subtracting the offset from the output signal, and In addition to calculating the azimuth change for each unit time, the drift is calculated from the history when the offset is updated, and the position / azimuth calculation means corrects the traveling azimuth which is a matrix element of the state vector based on the drift of the offset. The calculation of a predetermined value for limiting the amount is also performed.

In the speed measuring device according to the present invention, in the speed calculating means, a system model for calculating the speed from the output signal of the distance sensor, the scale factor and the acceleration,
Based on the observation model showing the relationship between the speed information observed by the GPS receiver and the speed information in the system model, the speed and scale factor of the system model are set in the matrix element of the state vector, and the observed value is set by the GPS receiver. Using a Kalman filter that sets the observed velocity information, the state vector is calculated only when the difference between the rate of change of the output signal of the distance sensor and the velocity information observed by the GPS receiver at a predetermined time is less than a predetermined value. Is performed.

The speed measuring device according to the present invention is characterized in that when the standard deviation of the scale factor of the distance sensor calculated by the Kalman filter a predetermined number of times becomes a ratio equal to or less than a predetermined value with respect to the average value, the distance calculating means uses the distance sensor. Is corrected.

[0056]

[0057] The vehicle position measuring apparatus according to the present invention, orientation
In the change calculation means, from the output signal of the relative direction sensor
The offset detected by the offset detection means has been subtracted.
Multiply the value by the scale factor to calculate the vehicle
Calculate the position change, and then add the azimuth change
The system model to be calculated and the
Between the azimuth information and the azimuth information observed by the GPS receiver.
Based on the represented observation model,
The offset and scale factor of the system model
Set the azimuth obtained by integrating the azimuth and azimuth change.
Kalman fill with azimuth information observed by S receiver
To calculate the state vector using
Observed by GPS receiver in position / azimuth calculation means
Position information, direction change calculation means and distance / speed calculation
Based on the vehicle's travel distance and azimuth change calculated in steps,
Vehicle position measurement device that calculates the current position and heading of the vehicle
When the offset of the relative azimuth sensor is detected by the offset detecting means , the offset of the relative azimuth sensor and the traveling azimuth calculated by the position / azimuth calculating means are used as initial values, and matrix elements other than the scale factor of the state vector in the Kalman filter are used. Is initialized by the azimuth change calculating means.

In the vehicle position measuring device according to the present invention, the azimuth change calculating means uses the output signal of the relative azimuth sensor as a signal.
The offset detected by the offset detection means has been subtracted.
Multiply the value by the scale factor to calculate the vehicle
Calculate the position change, and then add the azimuth change
The system model to be calculated and the
Between the azimuth information and the azimuth information observed by the GPS receiver.
Based on the represented observation model,
The offset and scale factor of the system model
Set the azimuth obtained by integrating the azimuth and azimuth change.
Kalman fill with azimuth information observed by S receiver
To calculate the state vector using
Observed by GPS receiver in position / azimuth calculation means
Position information, direction change calculation means and distance / speed calculation
Based on the vehicle's travel distance and azimuth change calculated in steps,
Vehicle position measurement device that calculates the current position and heading of the vehicle
Wherein the azimuth change calculating means adjusts the Kalman gain to calculate the state vector so that the scale factor is not corrected by the predetermined value or more by the Kalman filter when the azimuth change is equal to or less than the predetermined value. is there.

In the vehicle position measuring device according to the present invention, the azimuth change calculating means uses the output signal of the relative azimuth sensor as the
The offset detected by the offset detection means has been subtracted.
Multiply the value by the scale factor to calculate the vehicle
Calculate the position change, and then add the azimuth change
The system model to be calculated and the
Between the azimuth information and the azimuth information observed by the GPS receiver.
Based on the represented observation model,
The offset and scale factor of the system model
Set the azimuth obtained by integrating the azimuth and azimuth change.
Kalman fill with azimuth information observed by S receiver
To calculate the state vector using
Observed by GPS receiver in position / azimuth calculation means
Position information, direction change calculation means and distance / speed calculation
Based on the vehicle's travel distance and azimuth change calculated in steps,
Vehicle position measurement device that calculates the current position and heading of the vehicle
, The heading of the vehicle changes by a predetermined value or more within a predetermined time.
Immediately after the conversion , the azimuth change calculation means adjusts the Kalman gain to calculate the state vector so that the offset is not corrected by the Kalman filter by a predetermined value or more until a predetermined time elapses thereafter. It is.

[0060]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS One embodiment of the present invention will be described below. Embodiment 1 FIG. FIG. 1 is a block diagram showing a configuration of a vehicle position measuring device according to Embodiment 1 of the present invention. In the figure, reference numeral 1 denotes a distance sensor that detects a moving distance of a vehicle and outputs a pulse signal according to the moving distance. Reference numeral 2 denotes a GPS receiver to which a GPS antenna for receiving a radio wave from a satellite is connected and which observes the current position, speed, heading and the like of the vehicle from the radio wave from the satellite received by the antenna. In No. 1, a device that measures at least the current position of the vehicle and outputs position data is used. 3
Is a relative azimuth sensor that detects a change in the azimuth of the vehicle and outputs a signal corresponding to the change. In the first embodiment, a sensor that outputs an angular velocity signal according to the yaw rate of the vehicle using a gyro is used. Reference numeral 4 denotes a signal processor including a computer for calculating the traveling direction and the current position of the vehicle according to a control program stored in a memory in advance.

In the signal processor 4, reference numeral 41 denotes distance / speed calculating means for calculating the moving distance of the vehicle per unit time from the output signal of the distance sensor 1. Distance / speed calculating means for calculating a moving distance and a speed for each time is used. Reference numeral 42 denotes an offset detecting means for detecting an offset of the output signal of the relative azimuth sensor 3, and reference numeral 43 denotes an azimuth change calculating means for calculating a azimuth change of the vehicle per unit time from the output signal of the relative azimuth sensor 3.

Reference numeral 44 denotes position / direction calculation means for calculating the position and direction of the vehicle. In the first embodiment, the position / azimuth calculating means 44 is based on the moving distance of the vehicle per unit time and the azimuth change calculated by the distance / speed calculating means 41 and the azimuth change calculating means 43. From a system model for calculating information and azimuth information, and an observation model representing the relationship between position information observed by the GPS receiver 2 and position information in this system model, the position information of the system model is added to the matrix element of the state vector. The azimuth information is set, and a Kalman filter in which the position information observed by the GPS receiver 2 is set is used as the observation value, and the Kalman gain is adjusted so that the correction amount of the traveling azimuth which is an element of the state vector is equal to or less than a predetermined value. One that adjusts to calculate a state vector is used.

Next, the operation of each means of the signal processor 4 will be described. Here, FIG. 2 is a flowchart showing the processing contents of the main routine, and FIG. 3 is a flowchart showing the contents of the first interrupt processing executed every predetermined time Δt.
FIG. 4 is a flowchart showing the content of a second interrupt process executed when a signal is output from the distance sensor 1, and FIG.
Is a flowchart showing the contents of a third interrupt process for receiving GPS observation information output from the GPS receiver 2 in each observation cycle. FIG. 6 is a flowchart showing the contents of the arithmetic processing in the position / azimuth calculating means 44.

The main routine shown in FIG. 2 is executed by all means such as the distance / speed calculating means 41, offset detecting means 42, azimuth change calculating means 43, and position / azimuth calculating means 44 which constitute the signal processor 4. Manage behavior. First, in step ST201, all processes are initialized. Next, the process proceeds to step ST202, and it is determined whether or not it is the processing start timing with reference to the processing start flag. If the processing start flag has been set, it is determined that the processing start timing has been reached, and the process branches to step ST203. If the flag has been cleared, it is determined that the processing has not been started, and the process waits until this processing start flag is set.

In step ST203, the processing start flag is cleared for the next processing. Next, step S
Proceeds to T204, the distance and speed calculating means 41, from the count value .DELTA.N _i of the second output signal of the distance sensor 1 which is counted for each predetermined time Δt in the interrupt process shown in FIG. 4, the moving distance [Delta] d _i of the vehicle And the speed Vel _DRi are calculated by the following equations (20) and (21), and the count value ΔN _i of the output signal is set to 0 for the next processing. It should be noted that SF _(in formulas (20) and (21))
_{N → d) i} is a scale factor for converting an output signal of the distance sensor 1 into a distance.

Δd _i = Δd _i−1 + ΔN _i × SF _{(N → d) i} (20) Vel _DRi = | ΔN _i × SF _{(N → d) i} | / Δt (21)

Next, in step ST205, the speed Vel _DRi calculated in step ST204 becomes 0.
When the vehicle is stopped, the average of the output signals ω _sg of the relative azimuth sensor 3 for a predetermined time or more while the vehicle is stopped is calculated by the offset detection means 42 as the offset ω of the output signal.
_of . Next, in step ST206, the azimuth change calculating means 43 calculates the predetermined time Δ
The azimuth change Δθ _i is calculated from the output signal ω _sg of the relative azimuth sensor 2 measured every t. Note that ω _offi and SF _{(ω → φ) i} in the equation (22) are an offset of an output signal and a scale factor for converting the output signal into an azimuth.

Δθ _i = Δθ _i−1 + (ω _sgi −ω _offi ) × SF _{(ω → φ) i} × Δt (22)

Next, in step ST207, the GPS
It is determined whether or not the reception of the GPS observation information from the receiver 2 has been completed with reference to the GPS observation information reception flag. As a result, if the GPS observation information reception flag is set, it is determined that the reception of the GPS observation information from the GPS receiver 2 has been completed, and the process branches to step ST208. If the flag is cleared, the process proceeds to step ST202. return. In step ST208, the GPS observation information reception flag is cleared for the next processing.

Next, proceeding to step ST209, the position / azimuth calculating means 44 executes a position / azimuth measuring process,
The current position (λ _i , φ _i ) and traveling direction θ _i of the vehicle are calculated using a calculation formula (Kalman filter) described later. Next, in step ST210, this step ST209
Current position (λ _i , φ _i ) and traveling direction θ _i calculated in step ST204, and the velocity Ve calculated in step ST204.
1 _DRi . In step ST 211, after the moving distance [Delta] d _i and the azimuth change [Delta] [theta] _i between receiving GPS observation information from the GPS receiver 2 to 0, the process returns to step ST 202.

Next, the first interrupt processing will be described.
The first interrupt process shown in FIG. 3 is started at predetermined time intervals Δt, and in step ST301, a process start flag indicating a process start timing is set, and the first interrupt process is terminated.

Next, the second interrupt processing will be described.
The second interrupt processing shown in FIG. 4 is started each time a signal is output from the distance sensor 1, and in step ST401, the counter value of the distance sensor signal counter counting the output signal of the distance sensor 1 is changed. The value is incremented, and the second interrupt processing ends.

Next, the third interrupt processing will be described.
In the third interrupt processing shown in FIG.
At T501, after receiving the GPS observation information output from the GPS receiver 2 for each observation cycle, the GPS observation information is stored. Next, in step ST502, all GPS
It is determined whether or not the reception of the observation information has been completed. If the reception has been completed, the process branches to step ST503, and if not, the third interrupt processing ends. Step ST5
At 03, it is determined that all the GPS observation information has been received, the GPS observation signal reception flag is set, and the third interrupt processing ends.

Next, the operation of the position / azimuth calculation means 44 will be described. Position and orientation calculation unit 44, distance and speed calculation means 41 moving distance of the vehicle calculated for each unit time Δt by [Delta] d _i, of the vehicle calculated for each azimuthal variation calculation means 43 the unit time Δt from the azimuth change [Delta] [theta] _i A system model for calculating the current position (λ _i , φ _i ) and the traveling direction θ _i of the vehicle is calculated using the following equations (23) to (28).

Λ_i= Λ_i-1+ Δd_i× sin {θ_i-1+ Δθ_i} × SF_{d → λ}+ Δλ_i  ... (23) φ_i= Φ_i-1+ Δd_i× cos {θ_i-1+ Δθ_i} × SF_{d → φ}+ Δφ_i  ... (24) δλ_i= {ΔΔd_i× sin θ_i+ ΔΔθ_i× Δd_i× cos θ_i} × SF_{d → λ} ... (25) δφ_i= {ΔΔd_i× cos θ_i−δΔθ_i× Δd_i× sin θ_i} × SF_{d → φ} ... (26) sin {θ_i-1+ Δθ_i} = Cosθ_i-1× sinΔθ_i+ Sin θ_i-1× cosΔθ_i  ... (27) cos {θ_i-1+ Δθ_i} = Cosθ_i-1× cosΔθ_i−sin θ_i-1× sinΔθ_i  ... (28)

The relationship between the position information (λ _Gi , φ _Gi ) observed by the GPS receiver 2 and the vehicle position (λ _i , φ _i ) of the system model is _expressed by the following equations (29) and (3).
Obtain an observation model represented by 0).

Λ _Gi = λ _i + δλ _Gi (29) φ _Gi = φ _i + δφ _Gi (30)

In the above equations (23) to (29), (29) and (30), i is a discrete time, λ
_i-1 and φ _i-1 are the longitude and latitude of the vehicle position at discrete time i-1, θ _i-1 is the heading of the vehicle at discrete time i-1, and SF _{d → λ} and SF _{d → φ} are This is a coefficient for converting the unit of the moving amount (distance) in the longitude or latitude direction into the unit of longitude or latitude. [delta] [lambda] _i and .delta..phi _i longitude error and latitude errors in the vehicle position, δΔθ _i and Derutaderutad _i is the error of the moving distance [Delta] d _i and orientation change [Delta] [theta] _i, the position information [delta] [lambda] _Gi and .delta..phi _Gi is observed by the GPS receiver ( λ _Gi ,
φ _Gi ).

Next, based on the system model and the observation model, a state equation based on the following equations (31) to (35), an observation equation based on the equations (36) to (39), and an equation (40) -The current position (λ _i , φ _i ) and the heading θ _i of the vehicle are calculated by the Kalman filter equation according to equation (44).

[0080]

[0081]

X_{i | i}= X_{i | i-1}+ K_i{y_i− (Hx_{i | i-1}+ V_i)} ・ ・ ・ (40) x_{i + 1 | i}= F_ix_{i | i}+ G_iw_i ... (41) K_i= Σ_{i | i-1}H^T[HΣ_{i | i-1}H^T+ Σ_vi]^-1  ... (42) Σ_{i | i}= Σ_{i | i-1}-K_iHΣ_{i | i-1}  ... (43) Σ_{i + 1 | i}= F_iΣ_{i | i}F_i ^T+ G_iΣ_wiG_i ^T  ... (44)

Note that the above equations (31) to (35) and
In (36) to (39), x _iAt discrete time i
Opening vector, F_iIs also the state at discrete time i
State transition matrix, G_iIs also the driving matrix, w_iIs also a cis
System noise, y_iIs also the observed value, v_iIs also the observation noise
And H is the observation matrix. In addition, the above equations (40) to
In equation (44), x_{i | i}Is the state at discrete time i
State vector estimator, x_{i + 1 | i}Is at discrete time i
Of the state vector at discrete time i + 1_i
Is Kalman gain, Σ_{i | i}Is the state at discrete time i
Estimator of the covariance matrix of the vector, Σ_{i + 1 | i}Is discrete time
The predicted amount of the covariance matrix at discrete time i + 1 at time i, Σ
_viIs the covariance matrix of the observation error, Σ_wiIs the system error
Is a covariance matrix, but each element of the matrix is
Since it can be found, the description is omitted.

Next, the contents of the arithmetic processing of the position / azimuth calculating means 44 will be described with reference to the flowchart of FIG. First, in step ST601, the vehicle speed Vel _DRi calculated by the distance / speed calculation means 41 is 0.
It is determined whether it is greater than. As a result, the speed Vel
_{DR i} is completed and the process is determined to 0, the vehicle is stopped, the process proceeds to determine the traveling greater than 0 to step ST 602. In step ST602, two-dimensional or three-dimensional positioning is performed by the GPS receiver 2, and the current position (λ _i , φ _i ) of the vehicle is determined by the previous Kalman filter.
And the traveling azimuth θ _i are calculated, and it is determined whether or not the vehicle has moved a predetermined distance or more. If the determination result in step ST602 is Yes, the process branches to step ST603, and if the determination result is No, the process branches to step ST605.

At step ST603, an observation error is evaluated. That is, the GPS receiver 2 over a predetermined section
In the observation position information (λ _Gi, φ _Gi) by calculating the variance and standard deviation of, as well as the standard deviation of latitude lambda _Gi and longitude phi _Gi and matrix elements of the observation error v _i, latitude lambda _Gi and longitude phi A covariance matrix 観_{測 vi} of the observation error is calculated from the _Gi standard deviation. Next, in step ST604, the system error is evaluated. That is, to calculate the distance error Derutaderutad _i from the distance minimum interval of the output signal of the sensor 1 (axial resolution),
Moreover, calculating the difference in the direction of movement of both the travel locus of the GPS track and the vehicle position at a given interval as a direction error δΔθ _i. When the process of step ST604 ends, the process proceeds to step ST606. On the other hand, step ST
In 605, after the Kalman gain _{K i} to 0, the process proceeds to step ST606.

In step ST606, the GPS receiver 2
Using the position information (λ _Gi , φ _{G i} ) observed in the above as the observed value y _i , the above equations (31) to (35), (36) to (39), and (40) to (44) ) Is calculated according to the state vector estimator _{xi | i} . When the correction amount of the traveling azimuth θ _i which is a matrix element of the state vector is larger than a predetermined value, all the elements of the matrix of the Kalman gain K _i are adjusted to a small value until the correction amount becomes equal to or smaller than the predetermined value. , The state vector estimator x _{i | i} is recalculated. Next, in step ST607, equations (31) to (3)
The state transition matrix F _i is calculated using 5), and in step ST608, the driving matrix G _i is calculated using Expressions (31) to (35). Next, step ST6
In 09, the predicted amount x _{i + 1 | i} of the state vector is calculated according to the equations (40) to (44).

Next, proceeding to step ST610, as in the case of step ST602, two-dimensional or three-dimensional positioning is performed by the GPS receiver 2, and the current position (λ _i , φ _i ) is determined by the previous Kalman filter. After calculating the traveling azimuth θ _i , it is determined whether or not the vehicle has moved a predetermined distance or more. If the determination result in step ST610 is Yes
If so, the process branches to step ST611, and if the determination result is No, the calculation processing by the position / azimuth calculation means 44 ends.

In step ST611, the above equation (43) is obtained.
Is used to calculate the estimated amount 誤差_{i | i} of the error covariance, and further proceeds to step ST612 to calculate the estimated amount 誤差_{i + 1 | i} of the error covariance using equation (44). In step ST 613, after calculating the Kalman gain _{K i} using equation (42), the position and orientation calculation unit 44
The arithmetic processing according to is terminated.

As described above, according to the first embodiment, the azimuth change of the vehicle per unit time calculated by the azimuth change calculating means 43 from the output signal of the relative azimuth sensor 3 and the output signal of the distance sensor 1 A system model for calculating position information and azimuth information of the vehicle from the moving distance of the vehicle per unit time calculated by the distance / speed calculating means 41, and a GPS receiver 2
Sets the position information and azimuth information of the system model in the matrix element of the state vector based on the observation model representing the relationship between the position information of the observed vehicle and the position information in the system model. The Kalman gain is adjusted so that the correction amount of the traveling direction, which is a matrix element of the state vector, is equal to or less than a predetermined value by using a Kalman filter in which the position information observed by the aircraft 2 is set. Since the calculation of the vector is performed, when the GPS navigation and the autonomous navigation are combined, the current position of the vehicle is obtained from the observation information of the GPS receiver 2 and the measurement information of the onboard sensors 1 and 3 by one calculation method. And the heading can be calculated at the same time, and even if the observation error in the Kalman filter temporarily becomes inaccurate, The accuracy of the azimuth does not immediately decrease, the reliability of the current position and the traveling azimuth of the vehicle is improved, and even if the error of the observation information of the GPS receiver 2 cannot be detected with high accuracy, An effect is obtained that a vehicle position measuring device capable of accurately calculating the current position and the traveling direction can be realized.

Embodiment 2 The position / azimuth calculating means 44 shown in the first embodiment is configured to set at least an azimuth error in a matrix element of a system error in the Kalman filter, and to determine a predetermined value for limiting the correction amount in the traveling direction based on the azimuth error. May be calculated. The second embodiment of the present invention relates to such a vehicle position measuring device, and its configuration is the same as that of the first embodiment shown in FIG. 1, and therefore, its illustration and description are omitted.

The vehicle position measuring apparatus according to the second embodiment of the present invention is different from the first embodiment only in the state vector estimating operation in step ST606 of the measuring operation by position / azimuth calculating means 44 shown in the flowchart of FIG. Since the other points are the same as those of the first embodiment, only the operation of step ST606 in the second embodiment will be described below.

In the step ST606 of the second embodiment, similarly to the first embodiment, the position information (λ _Gi , φ _Gi ) observed by the GPS receiver 2 is set as the observed value y _i , and 31) to Equation (35), Equations (36) to Equation (3)
9) and calculation of the estimated amount x _{i | i} of the state vector according to the equations (40) to (44), and when the correction amount of the traveling direction θ _i which is a matrix element of the state vector is larger than a predetermined value. Adjusts all the elements of the matrix of Kalman gain K _i to a small value until the correction amount becomes equal to or less than a predetermined value,
Recalculate the estimated amount x _{i | i} of the state vector. At this time, the predetermined value for limiting the correction amount of the traveling azimuth θ _i is obtained by multiplying the azimuth error δΔθ _i obtained in step ST604 by a predetermined coefficient.

As described above, according to the second embodiment, at least the azimuth error is set to the matrix element of the system error in the Kalman filter of the position / azimuth calculation means 44 of the first embodiment, and the azimuth error is set. Therefore, since the calculation of a predetermined value for limiting the correction amount of the heading direction, which is an element of the state vector, is performed, when the heading error of the heading direction of the vehicle is detected, the heading error can be corrected reliably. Thus, an effect is obtained that a vehicle position measuring device that can accurately determine the current position and the traveling direction of the vehicle can be realized.

Embodiment 3 The position / azimuth calculating means 44 shown in the first embodiment may calculate a predetermined value for limiting the correction amount in the traveling direction based on the magnitude of the drift of the offset of the relative azimuth sensor 3. .
The third embodiment of the present invention relates to such a vehicle position measuring device, and its configuration is the same as that of the first embodiment shown in FIG. 1, and therefore, its illustration and description are omitted.

The vehicle position measuring device according to the third embodiment of the present invention includes the calculation processing of step ST205 by offset detecting means 42 of the main routine shown in FIG. 2 and the measurement by position / azimuth calculating means 44 shown in FIG. Only the estimation operation of step ST606 of the operation is the same as that of the first embodiment except that it is different from that of the first embodiment. Hereinafter, the calculation process of step ST205 and the estimation operation of step ST606 in the third embodiment will be described below. I will explain only.

Step ST2 in the third embodiment
Similarly to the first embodiment, when the speed Vel _DRi calculated in step ST204 is 0 (stopped), the offset detection unit 42 detects the relative azimuth sensor for a predetermined time or more during stoppage. 3 output signal ω
an average of the _sg and offset _ω ofi of the output signal. Incidentally, to calculate those by dividing the amount of change in the time offset omega _OFI as time for updating the offset omega _OFI as a drift of the offset.

Also, in step ST606 in the third embodiment, the GPS
The position information (λ _Gi , φ _Gi ) observed by the receiver 2 is defined as the observed value y _i , and the above equations (31) to (35) and (3)
6) to (39) and (40) to (44), the state vector estimator x _{i | i} is calculated,
When the correction amount of the traveling azimuth θ _i , which is a matrix element of the state vector, is larger than a predetermined value, all the elements of the matrix of the Kalman gain K _i are adjusted to a small value until the correction amount becomes equal to or smaller than the predetermined value. , The state vector estimator x _{i | i} is recalculated. The predetermined value for limiting the correction amount of heading theta _i that time is obtained by multiplying a predetermined coefficient to drift obtained in step ST205.

As described above, according to the third embodiment, the azimuth change calculating means 43 multiplies the scale factor by subtracting the offset of the output signal of the relative azimuth sensor 3 from the output signal, and multiplies by the scale factor. In addition to calculating the azimuth change for each unit time, the drift is calculated from the history when the offset is updated.
Since the calculation of the predetermined value for limiting the correction amount of the heading, which is a matrix element of the state vector, is performed based on the drift of the offset, the vehicle does not generate an error in the heading due to the drift. The present invention has the effect of realizing a vehicle position measuring device that can accurately determine the current position and traveling direction of the vehicle.

Embodiment 4 The fourth embodiment relates to a speed measurement device, and the configuration of the device is shown in a block diagram of FIG. In the figure, reference numeral 1 denotes a distance sensor that outputs a pulse signal according to a moving distance of a vehicle. 2 is the current position of the vehicle based on the radio wave from the satellite received by the antenna,
In the fourth embodiment, a GPS receiver for observing speed, traveling direction, and the like, which outputs at least vehicle speed data, is used. Reference numeral 4 denotes a signal processor including a computer for calculating the speed of the vehicle according to a control program stored in a memory in advance.

In the signal processor 4, 45
Is a speed calculating means for calculating a speed of the vehicle and a scale factor for converting an output signal of the distance sensor 1 into a distance. In the fourth embodiment, as the speed calculating means 45, a system model for calculating the speed from the output signal of the distance sensor 1, the scale factor, and the acceleration, the speed information observed by the GPS receiver 2, and the speed in the system model A Kalman filter that sets the speed and scale factor of the system model in matrix elements of the state vector based on an observation model representing the relationship with the speed information, and sets the speed information observed by the GPS receiver 2 in the observation value Using the distance sensor 1 at a predetermined time
Is used to calculate the state vector only when the difference between the output signal of the GPS receiver 2 and the rate of change of the speed information observed by the GPS receiver 2 is equal to or smaller than a predetermined value.

Next, the operation of the speed calculation means 45 in the signal processor 4 will be described. Here, FIG. 8 is a flowchart showing the contents of the main routine of the processing by the speed calculation means 45. In addition, a first interrupt process performed every predetermined time Δt, a second interrupt process performed when a signal is output from the distance sensor 1, and a GPS observation output from the GPS receiver 2 at each observation cycle Although there is a third interrupt process for receiving information, these processes are the same as those described in the first embodiment, and a description thereof will be omitted here.

The main routine shown in the flowchart of FIG. 8 manages the operation of the speed calculation means 45 of the signal processor 4. First, in step ST801, all processes are initialized. Next, in step ST802, it is determined whether or not it is processing start timing with reference to the processing start flag. As a result, if the processing start flag is set, it is determined that it is processing start timing, and the process proceeds to step ST803. If it is cleared, it is determined that it is not processing start timing, and the process waits until this processing start flag is set.

In step ST803, the processing start flag is cleared for the next processing. Next, step S
In T804, a predetermined time Δ
From the output signal count value ΔN _{i of} the distance sensor 1 counted for each t, the velocity Vel _{DR i} is calculated using the above equations (20) and (21), and then the output signal count is calculated for the next processing. The value ΔN _{i is set} to 0. Next, in step ST805, it is determined whether or not the reception of the GPS observation information from the GPS receiver 2 has been completed with reference to the GPS observation information reception flag. As a result, if the GPS observation information reception flag is set, the GPS
Step ST assumes that the reception of PS observation information has been completed.
The process proceeds to 806, and if the GPS observation information reception flag has been cleared, the process returns to step ST802. Step ST80
In step 6, the GPS observation information reception flag is cleared for the next processing.

The following steps ST807 to S
The processing up to T818 is based on the scale factor of the distance sensor 1.
TA SF_{(N → d) i}Is a process for correcting
Output signal of distance sensor 1 and GPS receiver in time
Speed information Vel observed in 2 _GPSiEach change of
When the difference of the rate is less than or equal to a predetermined value, the calculation formula described later
(Vehicle speed Vel) by (Kalman filter)_DRi
And scale factor SF _{(N → d) i}Is calculated.

Here, the Kalman filter according to the fourth embodiment will be described. From the output signal count value ΔNi of the distance sensor 1 and the scale factor SF _{(N → d) i} , the velocity Vel is calculated by the following equations (45) and (46).
A system model for calculating _DRi is obtained, and the relationship between the speed information Vel _GPSi observed by the GPS receiver 2 and the speed Vel _DRi of the system model is obtained by an observation model represented by the following equation (47).

[0106] SF_{(N → d) i}= SF_{(N → d) i-1} ... (45) Vel_DRi= Vel_DRi-1+ (ΔN_i−ΔN_i-1) / ΔT × SF_{(N → d) i-1}  ... (46)

Vel _GPSi = Vel _DRi + δVel _GPSi (47)

It should be noted that the above equations (45) and (46)
In equation (47), i is a discrete time, ΔT is a predetermined time
Between, SF_{(N → d) i}Is the distance sensor at discrete time i
Vel scale factor of 1_DRiIs the distance sensor 1
Vel calculated from output signal, Vel_GPSiIs a GPS receiver
Velocity, ΔN observed by transceiver 2_iIs separated from discrete time i-1
Output signal count value of distance sensor 1 during scattered time i, δ
Vel_GPSiIs Vel _GPSiIs the error contained in
You.

Next, based on the system model and the observation model, a state equation based on the following equations (48) to (50), an observation equation based on the equations (51) to (54), and an equation (55) _-The vehicle speed Vel _DRi and the scale factor SF _{(N → d) i} of the distance sensor 1 are calculated by the Kalman filter equation shown in Expression (59).

[0110]

[0111] _{y i = H · x i +} v i ··· (51) y i = Vel GPSi ··· (52) H = [0,1] ··· (53) v i = δVel GPSi ··· (54)

X_{i | i}= X_{i | i-1}+ K_i{y_i− (Hx_{i | i-1}+ V_i)} ・ ・ ・ (55) x_{i + 1 | i}= F_ix_{i | i} ... (56) K_i= Σ_{i | i-1}H^T[HΣ_{i | i-1}H^T+ Σ_vi ²]^-1  ... (57) Σ_{i | i}= Σ_{i | i-1}-K_iHΣ_{i | i-1} ... (58) Σ_{i + 1 | i}= F_IΣ_{i | i}F_I ^T ... (59)

In the above equations (48) to (50) and (51) to (54), x _i is a state vector at discrete time i, F _i is a state transition matrix at discrete time i, y _i is also observed value, v _i is the same observation noise, H is the observation matrix. In addition, the above equation (55)
In Expression (59), x _{i | i} is the estimated amount of the state vector at discrete time i, x _{i + 1 | i} is the predicted amount of the state vector at discrete time i + 1 at discrete time i, K _i is the Kalman gain, Σ _{i | i} is the estimator of the covariance matrix of the state vector at discrete time i, and Σ _{i + 1 | i} is discrete time i
Prediction amount of the covariance matrix at discrete time i + 1 in σ _vi
Reference numeral ² denotes the variance of the observation error. Since each element of the matrix can be obtained by calculating the determinant, the description thereof is omitted.

That is, in step ST807 of the main routine shown in FIG. 8, it is determined whether or not the GPS receiver 2 is performing two-dimensional or three-dimensional positioning. As a result, if positioning is in progress, the process branches to step ST808, and if non-positioning is in progress, the process branches to step ST819. In step ST808, the speed V of the GPS receiver 2 is
el _GPSi and output signal count value ΔN of distance sensor 1
i is calculated by the following equation (60) and equation (6).
Determined using 1).

Ratio _GPSi = Vel _GPSi / Vel _{GPS (i−1)} −1 (60) Ratio _DRi = ΔN _i / ΔN _i−1 −1 (61)

Then, the gradients of the respective rates of change during the continuous predetermined time T are calculated by the following equations (62) to (64), and the difference in the rates of change is _{expressed by} the speed Vel _{GPSi of the} GPS receiver 2. Multiplied by the observation error δVe
1 _Determine as _GPSi .

[0117]

The above equations (60) and (61)
And in Equations (62) to (64), Ratio
_GPSi is the rate of change of the speed of the GPS receiver 2, Ratio
_DRi is the rate of change of the output signal count value of the distance sensor 1, Mratio _GPS is an average value of the rate of change _{Ratio GPSI} speed of the GPS receiver 2, Mratio _DR rate of change of the output signal count value of the distance sensors 1 Ratio
_{This is} the average value of _DRi . Also, LampRatio
_GPS is the gradient of the rate of change of the speed of the GPS receiver 2,
LampRatio _DR is the gradient of the rate of change of the output signal of the distance sensor 1.

Next, in step ST809, the GPS
It is determined whether or not the difference between the rate-of-change gradient of the receiver 2 and the rate-of-change gradient of the output signal of the distance sensor 1 is less than a predetermined value. As a result, if the difference between them is less than the predetermined value, the flow directly branches to step ST811. On the other hand, if the difference between them is equal to or larger than the predetermined value, step ST81
After branching to 0 and clearing all elements of the Kalman gain matrix to 0, the process proceeds to step ST811.

Step ST811 is followed by step ST811.
In the calculation processing up to step ST813, the above equation (4)
8) to Expression (50), Expression (51) to Expression (54), and Expression
According to (55) -Equation (59), the state transition matrix F_i
And state vector estimator x _{i | i}And predicted quantity x
_{i + 1 | i}Is calculated. That is, step ST81
In step 1, a state transition matrix is estimated, and step ST81 is performed.
In step 2, the state vector is estimated, and in step ST813,
State vector prediction is performed. As mentioned above,
The rate of change of the speed of the GPS receiver 2 and the output of the distance sensor 1
When the difference from the change rate gradient of the force signal is more than a predetermined value
In step ST810, the Kalman gain is cleared to 0.
And then the process proceeds to step ST811.
If the difference in the change rate gradient is equal to or greater than a predetermined value, the predicted amount x
_{i + 1 | i}Is the estimated quantity x_{i | i}become.

Next, in step ST814, G
It is determined whether or not the difference between the rate-of-change gradient of the PS receiver 2 and the rate-of-change gradient of the output signal of the distance sensor 1 is less than a predetermined value. As a result, if the difference between the two change rate gradients is less than the predetermined value, the process directly branches to step ST819. On the other hand, if the difference between the two change rate gradients is equal to or larger than the predetermined value, the process branches to step ST815.

In the arithmetic processing from step ST815 to step ST817, the above equation (4)
8) to Eq. (50), Eqs. (51) to Eq. (54), and Eqs. (55) to Eq. (59), the estimated amount 誤差_{i | i} and the estimated amount Σ _{i + 1 | i of} the error covariance, and to calculate the Kalman gain _{K i.} That is, the error covariance is estimated in step ST815, the error covariance is predicted in step ST516, and the Kalman gain is calculated in step ST817.

Next, in step ST818, the scale factor SF calculated by the Kalman filter is used.
If the value of _{(N → d) i} is within the normal range, the scale factor SF _{(N → d) i} of the distance sensor 1 is updated accordingly. Next, in step ST819, after outputting the speed data, the process returns to step ST802.

As described above, according to the fourth embodiment, the system model for calculating the speed from the output signal of the distance sensor 1, the scale factor, the acceleration, etc., the speed information observed by the GPS receiver 2, and the above system Based on the observation model representing the relationship of the velocity information in the model, the velocity calculation means 45 sets the velocity and scale factor of the system model in the matrix element of the state vector, and G
Using a Kalman filter in which the speed information observed by the PS receiver 2 is set, the difference between the rate of change of the output signal of the distance sensor 1 and the rate of change of the speed information observed by the GPS receiver 2 at a predetermined time is equal to or less than a predetermined value. Since the calculation of the state vector is performed only in the case, the GPS receiver 2
An error in the speed information observed in step (1) is immediately determined, and when the error is small, a scale factor for converting the output signal of the distance sensor into a distance can be calculated by the Kalman filter. And a speed measuring device capable of quickly correcting the error while the vehicle is running, even if the speed of the vehicle calculated from the output signal of the distance sensor 1 has an error because the speed and the speed can be quickly and accurately set. Is obtained.

Embodiment 5 FIG. When the standard deviation of the scale factor of the distance sensor 1 calculated by the Kalman filter a predetermined number of times is less than a predetermined value with respect to the average value, the speed calculation unit 45 described in the fourth embodiment is used. May be corrected. The fifth embodiment of the present invention relates to such a speed detecting device, and its configuration is the same as that of the fourth embodiment shown in FIG. 7, so that its illustration and description are omitted.

The speed measuring device according to the fifth embodiment of the present invention is different from the speed measuring device 45 of the fourth embodiment only in the arithmetic processing of step ST818 by the speed calculating means 45 of the main routine shown in FIG. Since it is the same as the fourth embodiment, only the calculation processing of step ST818 in the fifth embodiment will be described below.

In this step ST818, first, when the estimation of the state vector estimation amount x _{i | i} is calculated a predetermined number of times or more, it is the matrix element of the latest predetermined number of state vector estimation amounts x _{i | i} . Scale factor SF
_{(N → d)} Calculate the average value and standard deviation of _i . When the ratio of the standard deviation to the average value is equal to or smaller than a predetermined value, the scale factor SF _{(N → d) i} of the distance sensor 1
Update.

As described above, according to the fifth embodiment, when the standard deviation of the scale factor of the distance sensor calculated by the Kalman filter a predetermined number of times becomes a ratio equal to or smaller than the predetermined value with respect to the average value, the speed is reduced. Since the calculation unit 45 is configured to correct the scale factor of the distance sensor 1, when the variance of the scale factor of the distance sensor 1 calculated by the Kalman filter is small, the scale factor of the distance sensor 1 is corrected, and the calculation is more stable. Since the correction can be performed, an effect is obtained that a speed measuring device capable of stably obtaining the moving distance and the speed from the distance sensor 1 can be realized.

Embodiment 6 FIG. The sixth embodiment relates to a vehicle position measuring device, and the configuration of the device is shown in a block diagram of FIG. In the figure, reference numeral 1 denotes a distance sensor that outputs a pulse signal according to a moving distance of a vehicle. Numeral 2 denotes a GPS receiver for observing the current position and the traveling direction of the vehicle from a radio wave from a satellite received by an antenna. In this sixth embodiment, a GPS receiver which outputs at least the position data and the direction data of the vehicle is used. Have been. Reference numeral 3 denotes a relative azimuth sensor that outputs a signal corresponding to a change in the azimuth of the vehicle. In the sixth embodiment, a gyro is used to output an angular velocity signal corresponding to the yaw rate of the vehicle.
Reference numeral 4 denotes a signal processor including a computer that calculates the traveling direction and the current position of the vehicle according to a control program stored in a memory in advance.

In the signal processor 4, 42
Is an offset detecting means for detecting an offset of the output signal from the relative azimuth sensor 3 when the speed calculated by the distance / speed calculating means described later is 0. Reference numeral 43 denotes an azimuth change calculating means for calculating the azimuth change of the vehicle per unit time from the output signal of the relative azimuth sensor 3 and correcting the offset of the relative azimuth sensor 3 and a scale factor for converting the output signal into an angle. 44 is a position / azimuth calculating means for calculating the position and azimuth of the vehicle;
Reference numeral 6 denotes a distance / speed calculating means for calculating the moving distance and speed of the vehicle per unit time from the output signal of the distance sensor 1.

The azimuth change calculating means 43 includes:
In the sixth embodiment, after the offset detected by the offset detecting means 42 is subtracted from the output signal of the relative direction sensor 3, the change in the direction of the vehicle per unit time is calculated by multiplying the result by the scale factor. Based on a system model for calculating the direction in which the change is integrated, and an observation model representing the relationship between the direction information observed by the GPS receiver 2 and the direction information in the system model, an offset of the system model is added to the matrix element of the state vector. And the azimuth obtained by integrating the scale factor and azimuth change,
As the observed value, one that calculates a state vector using a Kalman filter in which azimuth information observed by the GPS receiver 2 is set is used.

In the sixth embodiment, the position / azimuth calculating means 44 includes the position information observed by the GPS receiver 2, the moving distance of the vehicle calculated by the azimuth change calculating means 43, and the distance / speed calculation. On the basis of the change in the heading of the vehicle calculated by the means 46, one that calculates the current position and the traveling heading of the vehicle is used. Further, as the distance / speed calculating means 46, the distance / speed calculating means 4 used as the distance calculating means in the first embodiment shown in FIG.
1 is used.

Next, the operation of each means of the signal processor 4 will be described. Here, FIG. 10 shows a process of the azimuth change calculating means 43 for calculating the azimuth change of the vehicle from the output signal of the relative azimuth sensor 3 and correcting the offset of the relative azimuth sensor 3 and the scale factor for converting the output signal into an angle. It is a flowchart which shows the content.

In the sixth embodiment, the predetermined time Δ
a first interrupt process performed for each t, a second interrupt process performed when a signal is output from the distance sensor 1, and a second interrupt process for receiving GPS observation information output from the GPS receiver 2 for each observation cycle. The interrupt processing of No. 3 is the same as that of the first embodiment shown in the flowcharts of FIGS. FIG.
Is different from the first embodiment only in the azimuth change calculation process in step ST206. Therefore, the description of the operation processing other than the azimuth change calculation processing will be omitted below.

In the flowchart shown in FIG. 10, first, in step ST1001, the output signal ω _sgi of the relative azimuth sensor 3 measured at every predetermined time Δt is calculated by the equation (2).
The azimuth change Δθ _i is calculated using 2). Hereinafter, in steps ST1002 to ST1016,
The azimuth ΣΔθ _i obtained by integrating the relative azimuth Δθ _i for each predetermined time Δt calculated from the output of the relative azimuth sensor 3 with the azimuth information θ _Gi observed by the GPS receiver 2 is compared with the Kalman filter described later. The offset ω _{offi of} 3 and the scale factor SF _gryi are corrected.

Here, the Kalman filter according to the sixth embodiment will be described. System Model In the Kalman filter in the sixth embodiment, which by the following equation (65) to (67), is integrated to calculate the heading variation from the relative direction sensor 3 output signals omega _sgi offset omega _OFI and scale factor _{SF Gyri} , GP
An observation model that expresses the relationship between the azimuth information observed by the S receiver 2 and the azimuth with the system model is expressed by the following equation (68).

[0137] _{ΣΔθ i = ΣΔθ i-1 +} (ω sgi -ω ofi) × SF gyri × ΔT + δω ofi × SF gyri × ΔT + (ω sgi -ω ofi) × δSF gyri × ΔT ··· (65) ω _{ofi = ω ofi-1 + δω} ofi ··· (66) SF gyri = SF gyri-1 + δSF gyri ··· (67)

Θ _Gi = ΣΔθ _i + δθ _Gi (68)

In the equations (65) to (67) and (68), i is a discrete time, Δt is a predetermined time,
ω _sgi is the output signal of the relative orientation sensor 3 at the discrete time i, ω _offi is the offset at the discrete time i, SF _gyr is the scale factor, δ
omega _OFI is offset _error, δSF gyri scale factor error, ΣΔθ _i is integrated heading, theta _Gi azimuth information observed by the GPS receiver 2, .delta..theta _Gi azimuth information theta _Gi
Is the error of

Thereafter, based on the system model and the observation model, a state equation represented by the following equations (69) to (73), an observation equation represented by the following equations (74) to (77):
The Kalman filter equations of Expressions (78) to (82) are respectively set, and the offset ω _{offi of the} relative direction sensor 3 is set _.
And the scale factor SF _gyr is calculated.

[0141]

[0142] _{y i = Hx i + v i} ··· (74) y i = θ Gi ··· (75) v i = δθ Gi ··· (76) H = [1 0 0] ··· (77 )

X_{i | i}= X_{i | i-1}+ K_i｛Y_i− (Hx_{i | i-1}+ V)} (78) x_{i + 1 | i}= F_ix_{i | i + 1}+ G_iw ... (79) K_i= Σ_{i | i-1}H^T[HΣ_{i | i-1}H^T+ Σ_vi ²]^-1  ... (80) Σ_{i | i}= Σ_{i | i-1}-K_iHΣ_{i | i-1} ... (81) Σ_{i + 1 | 1}= F_iΣ_{i | i}F_i ^T+ G_iΣ_wiG_i ^T ... (82)

In the above equations (69) to (73) and (74) to (77), x _i is a state vector at discrete time i, F _i is a state transition matrix at discrete time i, G _i is likewise driven matrix, w _i is the same system noise, y _i is also observed signal, v _i is also the observation noise, H is an observation matrix. Further, in the equation (78) to Formula _{(82), x i | i} is the estimated amount of the state vector at discrete time _{i, x} i _{+1 |} i is the predicted amount of the state vector of the discrete time i + 1 at discrete time i, _{K i} Is the Kalman gain, Σ _{i | i} is the estimator of the covariance matrix at discrete time i, Σ _{i + 1 | i} is the discrete time i at discrete time i
The predicted amount of the covariance matrix of +1, ｗ _wi is the system error w _i
Covariance matrix of, sigma _vi ² is the variance of the measurement error v _i, since each element of the matrix such that obtained by calculating a determinant, the description thereof is omitted.

In Step ST1002 of FIG.
It is determined whether or not the reception of the GPS observation information from the GPS receiver 2 has been completed with reference to the GPS observation information reception flag. As a result, if the GPS observation information reception flag is set, it is determined that the reception of the GPS observation information from the GPS receiver 2 has been completed, and the process branches to step ST1003.
If the flag has been cleared, the process ends. In step ST1003, it is determined whether or not the speed Vel _DRi obtained by the distance sensor 1 is larger than 0. If the result of the determination is that the speed Vel _DRi is 0, it is determined that the vehicle is stopped and the process branches to step ST1004, where the offset ω _offi , which is a matrix element of the state vector, and the direction ΣΔθ
_After the initialization of _i , the process ends. As the initial values, the traveling direction θ _i calculated by the position / direction calculation unit 44 and the offset ω _offi detected by the offset detection unit 42 from the relative direction sensor 3 during the stop are used.

On the other hand, if the speed Vel _DRi is greater than 0, it is determined that the vehicle is traveling and the flow proceeds to step ST1005. In step ST1005, the GPS receiver 2
It is determined whether three-dimensional or three-dimensional positioning is being performed.
If the result of determination is that the GPS receiver 2 is performing positioning, the process branches to step ST1006, and if positioning is not in progress, the process ends as it is. In step ST1006, it is determined whether or not the time during which the two-dimensional or three-dimensional positioning in the GPS receiver 2 is continued is equal to or longer than a predetermined time. If the result of determination is that the continuation time is equal to or longer than the predetermined time, step ST100
7 and if it is less than the predetermined time, step ST10
It skips from 07 to step ST1009 and proceeds directly to step ST1010.

[0147] In step ST 1007, and from the azimuth information theta _Gi observed by the GPS receiver 2, the value .delta..theta _Gi obtained by subtracting the traveling direction theta _i calculated by the position and direction calculating unit 44 and the observed error _{v i,} the at predetermined time observation error _{v i}
To the dispersion and dispersion σ _{v i} ² of the observation error. Next, in step ST1008, according to the following equation (83), an offset error δω _offi which is an element of the system error w _i.
Is calculated.

_{Δω of} = x ₂₁ − (ω _of−stp + Lampω _of × Δt _run ) (83)

Note that in this equation (83), δω
_ofi is offset _error, the offset _{x 21} are elements of the state vector, omega _of-stp offset detected by the offset detection unit 42, Lampomega _of the slope of the offset detected by the offset detection unit 42 (drift), Delta] t _{the run} is This is the elapsed time after offset detection.

When the heading of the vehicle changes by a predetermined value or more within a predetermined time, the difference between the heading change of the heading of the vehicle and the heading change of the heading which is a matrix element of the state vector is calculated as:
The orientation change of the relative direction sensor 3, a value obtained by dividing an angle obtained by integrating separately in steps not shown, the value obtained by multiplying the scale factor by a predetermined coefficient which is an element of the state vector and the scale factor error _δSF gyri. And
Obtains the variance of the offset error [delta] [omega _OFI and scale factor error _δSF gyri at a given time, calculating the covariance matrix sigma _wi system error.

Next, steps ST1009 to ST100
In 1015, the above equations (69) to (73) and (7)
4) to Eq. (77) and Eqs. (78) to Eq. (82), the estimated amount x _{i | i of} the state vector, the state transition matrix F _i , the driving matrix G _i , and the predicted amount x of the state vector
_{i + 1 | i} , the estimated amount of the error covariance { _{i | i} and the predicted amount}
_{i + 1 | i} and the Kalman gain K _i are calculated. That is, in step ST1009, a state vector is estimated, in step ST1010, a state transition matrix is calculated, in step ST1011, a driving matrix is calculated, and in step ST1011.
Prediction of state vector at 1012, step ST101
3, the estimation of the error covariance is performed in step ST1014, and the calculation of the Kalman gain is performed in step ST1015.

Next, in step ST1016, the estimated amount x of the state vector obtained in step ST1009 is set.
_{i |} , offset ω of relative orientation sensor 3 based on _i
Update the _ofi and scale factor _{SF gyri,} it ends the calculation process of the direction change.

As described above, according to the sixth embodiment, the value obtained by subtracting the offset detected by the offset detection means 42 from the output signal of the relative direction sensor 3 is multiplied by the scale factor, and The state is calculated based on a system model that calculates the azimuth change and further calculates the azimuth obtained by integrating the azimuth change, and an observation model representing the relationship between the azimuth information in the system model and the azimuth information observed by the GPS receiver 2. The azimuth obtained by integrating the offset, scale factor, and azimuth change of the system model is set in the matrix element of the vector, and the GPS receiver 2 is used as the observation value.
The state vector is calculated by the azimuth change calculating means 43 using the Kalman filter in which the azimuth information observed in the step is set, and the position information observed by the GPS receiver 2 and the azimuth change calculating means 43 and the distance / speed calculating means 46 are calculated. The position / azimuth calculating means 44 calculates the current position and the heading of the vehicle based on the movement distance and the azimuth change of the vehicle calculated in the above. When the offset of 3 is detected, the azimuth change calculating means 43 uses the offset of the relative azimuth sensor 3 and the traveling azimuth calculated by the position / azimuth calculating means 44 as initial values to obtain a matrix other than the scale factor of the state vector in the Kalman filter. Because the element is initialized,
Even when there is an error in the offset and scale factor of the relative direction sensor 3, it is possible to quickly correct the error by one calculation method, correct each error, and accurately change the direction from the relative direction sensor 3. An effect is obtained that a vehicle position measuring device that can be obtained can be realized.

Embodiment 7 FIG. The azimuth change calculating means 43 shown in the sixth embodiment is set such that when the offset of the relative azimuth sensor 3 is detected by the offset detecting means 42, the offset and the traveling direction obtained by the position / azimuth calculating means 44 are used as initial values. , A matrix element other than the scale factor of the state vector in the Kalman filter may be initialized. The seventh embodiment of the present invention relates to such a vehicle position measuring device, and its configuration is the same as that of the sixth embodiment shown in FIG. 9, so that illustration and description thereof are omitted.

In the vehicle position measuring apparatus according to the seventh embodiment of the present invention, only the state vector estimating operation of step ST1009 in the flowchart of the azimuth change calculation process by azimuth change calculation means 43 shown in FIG. 6 is the same as that of the sixth embodiment except for the steps ST6 and ST6 in the seventh embodiment.
Only the state vector estimation operation of 1009 will be described.

Step ST1 in Embodiment 7
In 009, when the azimuth change Δθi calculated in step ST1001 is equal to or less than a predetermined value, the Kalman gain K _i is adjusted so that the scale factor SF _gyr is not corrected to a predetermined value or more by the Kalman filter. (69) to Expression (73), Expressions (74) to Expression (7)
7) and calculate the estimated amount of the state vector xi _{| i} according to equations (78) to (82).

As described above, according to the seventh embodiment, when the azimuth change calculated by the azimuth change calculating means 43 is equal to or smaller than the predetermined value, the Kalman filter is not used to correct the scale factor equal to or larger than the predetermined value. Since the state vector is calculated by adjusting the Kalman gain, the azimuth error caused by the offset error of the relative azimuth sensor 3 increases with time due to the Kalman filter, but the offset and the scale factor increase due to the scale factor error. Since the azimuth error is calculated in accordance with the characteristic that the azimuth error occurs in accordance with the turning angle, it is possible to perform correction in accordance with the characteristic of each error, and to accurately obtain the azimuth change calculated from the relative azimuth sensor 3. An effect is obtained that a vehicle position measuring device capable of performing the operation can be realized.

Embodiment 8 FIG. When the traveling direction of the vehicle changes by a predetermined value or more within a predetermined time, the azimuth change calculating means 43 shown in the above-described sixth embodiment is designed to prevent the Kalman filter from correcting the offset by a predetermined value or more until the predetermined time elapses. , The state vector may be calculated by adjusting the Kalman gain. The eighth embodiment of the present invention relates to such a vehicle position measuring device, and its configuration is the same as that of the sixth embodiment shown in FIG. 9, so that illustration and description thereof are omitted.

In the vehicle position measuring apparatus according to the eighth embodiment of the present invention, only the operation of estimating the state vector in step ST1009 in the azimuth change calculating process by azimuth change calculating means 43 shown in FIG. Since the other points are the same as those of the sixth embodiment, only the state vector estimating operation of step ST1009 in the eighth embodiment will be described below.

Step ST1 in Embodiment 8
In 009, when the traveling direction θi of the vehicle calculated by the position / azimuth calculating means 44 changes by a predetermined value or more within a predetermined time, the offset is corrected by a Kalman filter by a predetermined value or more until the predetermined time elapses thereafter. By adjusting the Kalman gain Ki so as not to cause this, the above equations (69) to (73), equations (74) to (77),
And estimating the state vector xi _{| i} according to the equations (78) to (82).

As described above, according to the eighth embodiment, immediately after the heading of the vehicle has changed by a predetermined value or more within a predetermined time, the offset by the Kalman filter is equal to or more than a predetermined value until the predetermined time elapses thereafter. So that it is not corrected
Since the state vector is calculated by adjusting the Kalman gain by the azimuth change calculating means 43, it is possible to perform correction according to the characteristics of each error, and to accurately determine the azimuth change in the vehicle position. The effect that a measuring device can be realized is obtained.

Embodiment 9 FIG. In the first embodiment, the position / azimuth calculating means 44 processes the azimuth change and the moving distance per unit time, and the position information observed by the GPS receiver 2 with the Kalman filter, and calculates the position information and the azimuth information of the vehicle. In the calculation, it has been described that the state vector is calculated by adjusting the Kalman gain so that the correction amount of the traveling azimuth which is an element of the state vector is equal to or less than a predetermined value. The state vector may be calculated by adjusting the Kalman gain so as to be equal to or less than the value. Further, a Kalman filter that uses the azimuth observed by the GPS receiver 2 as an observation value may be used.

Embodiment 10 FIG. In Embodiment 4 described above,
In the speed calculation means 45, it has been described that the scale factor of the distance sensor 1 is corrected by the Kalman filter.
If the difference between the rates of change of the speed information observed by the PS receiver 2 is equal to or less than a predetermined value, the ratio or difference between the speed calculated from the distance sensor 1 and the speed information observed by the GPS receiver 2 at that time is determined by a low-pass filter. The scale factor may be corrected on the basis of the result passed through.

Embodiment 11 FIG. In the seventh embodiment,
When the azimuth change calculated by the relative azimuth sensor 3 is equal to or less than a predetermined value, the azimuth change calculating means 43 adjusts the Kalman gain so that the scale factor of the relative azimuth sensor is not corrected to a predetermined value or more by the Kalman filter. The calculation has been described. However, as long as it indicates a state in which the vehicle is traveling substantially straight, the change may be used instead of the azimuth change calculated from the relative azimuth sensor 3.

Embodiment 12 FIG. In Embodiment 8 described above,
Immediately after the heading of the vehicle changes by a predetermined value or more within a predetermined time in the heading change calculating means 43, the Kalman gain is adjusted so that the offset of the relative heading sensor is not corrected by the predetermined value by the Kalman filter until the predetermined time elapses thereafter. Has been described as calculating the state vector, but if it indicates that the vehicle has turned right or left within a predetermined time or turned a predetermined angle or more, it may be used instead of the traveling direction of the vehicle. Good.

[0166]

As described above, according to the present invention, the GP
When combining S navigation and autonomous navigation, the current position and heading of the vehicle are calculated simultaneously by the Kalman filter from the observation information of the GPS receiver and the measurement information of the onboard sensors, and the heading is equal to or greater than a predetermined value. Since the calculation is performed by adjusting the Kalman gain so as not to be corrected, even if the observation error in the Kalman filter becomes temporarily inaccurate, the accuracy of the current position and the traveling direction of the vehicle immediately decreases. Such a situation is eliminated, and there is an effect that a vehicle position measuring device that can improve the reliability of the current position and the traveling direction of the vehicle can be obtained.

According to the present invention, at least an azimuth error is set in the matrix element of the system error in the Kalman filter, and a predetermined value for limiting the correction amount of the traveling azimuth is calculated based on the azimuth error. Therefore, the error of the traveling direction of the vehicle can be corrected, and the present position and the traveling direction of the vehicle can be accurately obtained.

According to the present invention, since the predetermined value for limiting the correction amount of the traveling direction is calculated based on the magnitude of the offset drift of the relative direction sensor, the traveling direction is affected by the drift. Therefore, there is an effect that the current position and the traveling direction of the vehicle can be accurately obtained so that no error occurs.

According to the present invention, the difference between the rate of change of the output signal of the distance sensor and the rate of change of the speed information observed by the GPS receiver at the predetermined time is determined, and only when the difference is smaller than the predetermined value, Since the speed calculation means is configured to calculate the state vector, when there is an error in the speed of the vehicle calculated from the output signal of the distance sensor, a speed measurement device capable of quickly and accurately correcting the error is provided. There is an effect that can be obtained.

According to the present invention, since the scale factor of the distance sensor is corrected when the variance of the scale factor of the distance sensor calculated by the Kalman filter is small, the vehicle calculated from the output signal of the distance sensor is corrected. The speed error can be more stably corrected.

[0171]

According to the present invention, the relative azimuth sensor is turned off.
Calculate set and scale factor with Kalman filter
And the position information observed by the GPS receiver and the azimuth change calculation
Moving distance of vehicle calculated by means and distance / speed calculating means
Calculates the current position and heading of the vehicle from separation and heading changes
In addition, since the detected offset of the relative direction sensor and the traveling direction of the vehicle are used as initial values, matrix elements other than the scale factor of the state vector in the Kalman filter are initialized, so that the relative position sensor is turned off.
If there is an error between the set and the scale factor,
Correct the error quickly by the calculation method and use the relative orientation sensor
Thus, it is possible to obtain the effect that the azimuth change can be obtained with high accuracy, and the offset of the relative position sensor and the error of the scale factor can be corrected more accurately.

According to the present invention, the relative azimuth sensor is turned off.
Calculate set and scale factor with Kalman filter
And the position information observed by the GPS receiver and the azimuth change calculation
Moving distance of vehicle calculated by means and distance / speed calculating means
Calculates the current position and heading of the vehicle from separation and heading changes
Rutotomoni, as the scale factor is not corrected more than a predetermined value when the azimuth change calculated by the orientation change calculation means is equal to or less than the predetermined value, since it is configured to calculate the state vector by adjusting the Kalman gain, relative position sensors Of the office
If there is an error between the
Error is quickly corrected by the
The azimuth change can be obtained with high accuracy, and the azimuth error caused by the offset error of the relative azimuth sensor increases with time, but the azimuth error caused by the scale factor error occurs according to the turning angle. Is obtained.

According to the present invention, the relative azimuth sensor is turned off.
Calculate set and scale factor with Kalman filter
And the position information observed by the GPS receiver and the azimuth change calculation
Moving distance of vehicle calculated by means and distance / speed calculating means
Calculates the current position and heading of the vehicle from separation and heading changes
In addition, after the heading of the vehicle has changed by a predetermined value or more within a predetermined time, the state vector is calculated by adjusting the Kalman gain so that the offset is not corrected by the predetermined value until the predetermined time elapses. The relative position
If there is an error in the offset and scale factor of the sensor
In one case, one calculation method can quickly correct the error and
Direction change can be obtained accurately from the direction sensor
At the same time, the azimuth error caused by the offset error of the relative azimuth sensor increases with time, but the azimuth error caused by the scale factor error can be corrected according to the characteristic that it occurs according to the turning angle. Can be

[Brief description of the drawings]

FIG. 1 is a block diagram showing a configuration of a vehicle position measuring device according to a first embodiment of the present invention.

FIG. 2 is a flowchart showing processing contents of a main routine in the first and third embodiments.

FIG. 3 is a flowchart showing the contents of a first interrupt process according to the first embodiment.

FIG. 4 is a flowchart showing the contents of a second interrupt process according to the first embodiment.

FIG. 5 is a flowchart showing the contents of a third interrupt process according to the first embodiment.

FIG. 6 is a flowchart showing processing contents of a position / azimuth calculation means in the first to third embodiments.

FIG. 7 is a block diagram showing a configuration of a speed measuring device according to a fourth embodiment of the present invention.

FIG. 8 is a flowchart showing processing contents of a main routine in the fourth and fifth embodiments.

FIG. 9 is a block diagram showing a configuration of a vehicle position measuring device according to a sixth embodiment of the present invention.

FIG. 10 is a flowchart showing processing contents of an azimuth change calculating unit according to the sixth to eighth embodiments.

FIG. 11 is a block diagram showing a configuration of a conventional vehicle position measuring device.

FIG. 12 is a flowchart showing a calculation process of a main routine of dead reckoning in the conventional vehicle position measuring device.

FIG. 13 is a flowchart showing a calculation process of an azimuth change amount and a moving distance in a conventional vehicle position measuring device.

FIG. 14 is a flowchart showing a relative trajectory calculation process in a conventional vehicle position measuring device.

FIG. 15 is a flowchart showing a calculation process of an absolute azimuth and an absolute position in a conventional vehicle position measuring device.

FIG. 16 is a flowchart showing a Kalman filter calculation process for combining dead reckoning navigation and GPS navigation in a conventional vehicle position measurement device.

[Explanation of symbols]

Reference Signs List 1 distance sensor, 2 GPS receiver, 3 relative direction sensor, 41 distance / speed calculation means (distance calculation means), 42
Offset detecting means, 43 azimuth change calculating means, 44
Position / azimuth calculation means, 45 speed calculation means, 46 distance / speed calculation means.

Continuation of front page (56) References JP-A-6-34379 (JP, A) JP-A-6-288776 (JP, A) JP-A-7-239236 (JP, A) JP-A-7-301541 (JP) , A) JP-A-8-68849 (JP, A) JP-A-9-14962 (JP, A) (58) Fields investigated (Int. Cl. ⁷ , DB name) G01C 21/00-21/36 G01C 22/00 G01C 19/00-19/72

Claims (8)

(57) [Claims] 1. A GPS receiver for observing at least a current position of a vehicle using a satellite, a relative direction sensor for outputting a signal corresponding to a change in direction of the vehicle, and a signal corresponding to a moving distance of the vehicle. A distance sensor to be output; an azimuth change calculating means for calculating an azimuth change of the vehicle per unit time from an output signal of the relative azimuth sensor; and a movement distance of the vehicle per unit time from the output signal of the distance sensor. A distance calculation unit, a system model for calculating the position information and the azimuth information of the vehicle from the azimuth change and the movement distance per unit time calculated by the azimuth change calculation unit and the distance calculation unit, and the GPS receiver Based on the position information observed in and an observation model representing the relationship between the position information in the system model, the matrix element of the state vector Position information and azimuth information, and using the Kalman filter in which the position information observed by the GPS receiver is set as the observation value, so that the correction amount of the traveling azimuth which is an element of the state vector is equal to or less than a predetermined value. A vehicle position measuring device comprising: a position / azimuth calculating means for adjusting a Kalman gain of the Kalman filter and calculating the state vector. 2. A position / azimuth calculation means sets at least an azimuth error in a matrix element of a system error in a Kalman filter, and adjusts a Kalman gain so that a correction amount of a traveling azimuth, which is a matrix element of a state vector, becomes a predetermined value or less. 2. The vehicle position measuring device according to claim 1, wherein, when adjusting, the predetermined value is calculated based on the azimuth error. 3. An azimuth change calculating means subtracts an offset of the output signal detected from the relative azimuth sensor from the output signal of the relative azimuth sensor, and multiplies the result by a scale factor to change the azimuth of the vehicle per unit time. And at the time of updating the offset, the drift is calculated from the history.The position / azimuth calculating means adjusts the Kalman gain so that the correction amount of the traveling azimuth, which is a matrix element of the state vector, becomes a predetermined value or less. When doing, a predetermined value for limiting the correction amount of the traveling direction of the vehicle, which is a matrix element of the state vector,
2. The vehicle position measuring device according to claim 1, wherein the calculation is performed based on the offset drift calculated by the direction change calculating unit. 4. A GPS receiver for observing at least the speed of a vehicle using a satellite, a distance sensor for outputting a signal corresponding to a moving distance of the vehicle, and an output signal of the distance sensor and a scale factor and acceleration. A system model for calculating speed, and the GPS
Based on the speed information observed by the receiver and the observation model representing the relationship between the speed information in the system model, the speed and scale factor of the system model are set in the matrix element of the state vector, and the GPS value is used as the observed value. Using a Kalman filter that sets the speed information observed by the receiver, only when the difference between the output signal of the distance sensor and the rate of change of the speed information observed by the GPS receiver at a predetermined time is equal to or less than a predetermined value. And a speed calculating means for calculating the state vector. 5. When the standard deviation of the scale factor of the distance sensor calculated by the Kalman filter a predetermined number of times is less than a predetermined value with respect to the average value,
The speed measuring device according to claim 4, wherein the scale factor of the distance sensor is corrected. 6. At least the current position of the vehicle using a satellite
GPS receiver for observing the position and heading, and a distance sensor for outputting a signal according to the travel distance of the vehicle
And a relative heading section for outputting a signal corresponding to a heading change of the vehicle.
From the output signal of the distance sensor for each unit time of the vehicle.
Distance / speed calculating means for calculating a moving distance and a speed, and when the speed calculated by the distance / speed calculating means is 0,
OFF of the output signal from the output signal of the relative direction sensor
Offset detection means for detecting a set, and the offset detection from an output signal of the relative orientation sensor
After subtracting the offset detected by the means,
The vehicle's heading per unit time
Calculation, ThisA system for calculating the azimuth by integrating the azimuth change
Stem model and bearing information observed by the GPS receiver
And the relationship between the orientation information in the system model
Based on the observation model
Offset and scale factors and
And the azimuth obtained by integrating the azimuth change.
Karman filter set with azimuth information observed by PS receiver
Azimuth change calculation for calculating the state vector using
Means, position information observed by the GPS receiver, and the azimuth change
The calculating means and the distance / speed calculating means
Based on the moving distance and direction change of the vehicle, the current
A position / azimuth calculation means for calculating the current position and the heading is provided.
e, The azimuth change calculating means may include an offset detecting means.
When the offset of the relative bearing sensor is detected by the
Offset and the position / direction detected by the offset detection means
The heading of the vehicle calculated by the position calculation means and the initial value
The state vector scale in the Kalman filter
Initialize matrix elements other than the rule factor Vehicle position measurement
apparatus. 7. At least the current position of the vehicle using a satellite
GPS receiver for observing the position and heading, and a distance sensor for outputting a signal according to the travel distance of the vehicle
And a relative heading section for outputting a signal corresponding to a heading change of the vehicle.
From the output signal of the distance sensor for each unit time of the vehicle.
Distance / speed calculating means for calculating a moving distance and a speed, and when the speed calculated by the distance / speed calculating means is 0,
OFF of the output signal from the output signal of the relative direction sensor
Offset detection means for detecting a set, and the offset detection from an output signal of the relative orientation sensor
After subtracting the offset detected by the means,
The vehicle's heading per unit time
Calculation, ThisA system for calculating the azimuth by integrating the azimuth change
Stem model and bearing information observed by the GPS receiver
And the relationship between the orientation information in the system model
Based on the observation model
Offset and scale factors and
And the azimuth obtained by integrating the azimuth change.
Karman filter set with azimuth information observed by PS receiver
Azimuth change calculation for calculating the state vector using
Means, position information observed by the GPS receiver, and the azimuth change
The calculating means and the distance / speed calculating means
Based on the moving distance and direction change of the vehicle, the current
A position / azimuth calculation means for calculating the current position and the heading is provided.
e, The azimuth change calculating means may determine that the azimuth change is equal to or less than a predetermined value.
If there is a scale factor in the Kalman filter
Adjust the Kalman gain so that it is not corrected beyond the set value.
And calculate the state vector Vehicle position measurement equipment
Place. 8. At least the current position of the vehicle using a satellite
GPS receiver for observing the position and heading, and a distance sensor for outputting a signal according to the travel distance of the vehicle
And a relative heading section for outputting a signal corresponding to a heading change of the vehicle.
From the output signal of the distance sensor for each unit time of the vehicle.
Distance / speed calculating means for calculating a moving distance and a speed, and when the speed calculated by the distance / speed calculating means is 0,
OFF of the output signal from the output signal of the relative direction sensor
Offset detection means for detecting a set, and the offset detection from an output signal of the relative orientation sensor
After subtracting the offset detected by the means,
The vehicle's heading per unit time
Calculation, ThisA system for calculating the azimuth by integrating the azimuth change
Stem model and bearing information observed by the GPS receiver
And the relationship between the orientation information in the system model
Based on the observation model
Offset and scale factors and
And the azimuth obtained by integrating the azimuth change.
Karman filter set with azimuth information observed by PS receiver
Azimuth change calculation for calculating the state vector using
Means, position information observed by the GPS receiver, and the azimuth change
The calculating means and the distance / speed calculating means
Based on the moving distance and direction change of the vehicle, the current
A position / azimuth calculation means for calculating the current position and the heading is provided.
e, The azimuth change calculating means determines that the traveling azimuth of the vehicle is a predetermined direction.
Immediately after a change of a predetermined value or more within
Until the time elapses, offset by the Kalman filter
Kalman gain is adjusted so that
Adjust and calculate the state vector Vehicle position measurement equipment
Place.