JP2000346661A - Locator apparatus - Google Patents

Abstract

PROBLEM TO BE SOLVED: To obtain a locator apparatus which can quickly correct a scale factor of a distance sensor different depending on vehicles after the vehicle starts running. SOLUTION: Waves from satellites are received by a GPS receiver 3 to observe a velocity or the like of the vehicle. Moreover, a signal corresponding to a move distance of the vehicle is outputted from a distance sensor L and each of average values of GPS velocities and output signals of the distance sensor is calculated by a velocity calculation means 511. When a gradient subsequent to a change with time of each average value is not larger than a predetermined value, a scale factor of the distance sensor is corrected and the vehicle velocity is estimated according to a Kalman filter equation, and an error sharing/variance is predicted and estimated. The vehicle velocity is calculated form the output signal of the distance sensor and the scale factor.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a GPS (Glo
BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a locator device provided with a compound navigation that calculates a current position, a heading, a speed, and the like of a vehicle using observation information of a bal positioning system (receiver) and measurement information of a compound sensor for autonomous navigation.

[0002]

2. Description of the Related Art FIG.
FIG. 1 is a block diagram illustrating an example of a configuration of a conventional locator device disclosed in Japanese Patent Application Laid-Open Publication No. HEI 10-115,000. In the figure, 1 is a distance sensor, 2
Is relative azimuth sensor, 3 is GP as absolute position observation means
S is a receiver, and 5 is a signal processor. In the signal processor 5, reference numeral 520 denotes a relative trajectory calculation for calculating a relative trajectory based on output signals of the distance sensor 1 and the relative azimuth sensor 2, and 521 denotes an absolute trajectory calculation for calculating an absolute trajectory thereof. Is a Kalman filter for correcting the distance coefficient of the distance sensor 1, the offset of the relative direction sensor 2, the absolute position and the absolute direction.

Next, the operation will be described. The relative trajectory calculation 520, the absolute trajectory calculation 521, and the Kalman filter 522 are realized by calculation processing by a microcomputer, and these will be described below. Here, FIG. 21 is a flow chart showing the calculation processing of the main routine of the autonomous navigation, FIG. 22 is a flow chart showing the calculation processing of the change of the traveling direction and the moving distance, FIG. 23 is a flow chart showing the relative trajectory calculation processing, and FIG. FIG. 25 is a flowchart showing the calculation processing of the absolute position, and FIG. 25 is a flowchart showing the calculation processing of a Kalman filter for combining autonomous navigation and GPS navigation.

First, in step ST2101 of FIG. 21, a change in the heading direction and a moving distance of the vehicle are calculated. FIG. 22 shows details of the processing contents of the calculation of the change in the traveling direction and the moving distance of the vehicle. When this processing is started, step S
At T2201, a change in the traveling direction of the vehicle is first obtained from the relative direction sensor 2. Next, in step ST2202, the value obtained by multiplying the offset correction amount by the moving distance L from the previous time is subtracted from the change in the traveling azimuth to perform offset correction for the change in the traveling azimuth. After that, in step ST2203, the signal of the distance sensor 1 is multiplied by the distance coefficient to calculate the moving distance, and the calculation of the change in the traveling direction and the moving distance of the vehicle is ended.

[0005] When the processing of calculating the change in the heading direction and the moving distance of the vehicle in step ST2101 is completed, a relative trajectory calculation processing is performed in step ST2102. FIG. 23 shows details of the processing content of the relative trajectory calculation. When this processing is started, first, in step ST2301, the change in the traveling direction is integrated to update the relative direction. Next, the process proceeds to step ST2302, where the X and Y components of the movement vector based on the updated relative orientation and the movement distance obtained in step ST2203 are added to the previous relative position coordinates to obtain new relative position coordinates. The trajectory calculation processing ends.

[0006] When the relative trajectory calculation processing in step ST2102 is completed, next, in step ST2103, calculation processing of absolute azimuth and absolute position is performed. FIG. 24 shows details of the processing contents of the absolute azimuth / absolute position calculation. When this processing is started, first, in step ST2401, the absolute azimuth is updated based on the change in the traveling azimuth. Next, step S
Proceeding to T2402, the absolute position coordinates are updated based on the updated absolute azimuth and the moving distance obtained in step ST2203, and the processing for calculating the absolute azimuth / absolute position ends. The absolute azimuth A and the absolute position updated in the process of step ST2103 are used in the next step ST210.
4 is used for the compounding process with the observation information of the GPS receiver 3.

FIG. 25 shows the details of the processing of combining with the observation information of the GPS receiver 3 in step ST2104.
Shown in FIG. 25 is a flowchart showing the calculation procedure of the Kalman filter 522. The Kalman filter 522 for combining autonomous navigation and GPS navigation will be described first. The Kalman filter 522 is divided into a signal generation process and an observation process. When a part of the state X (t) of the system related to the observation matrix H can be observed with respect to the state X (t) of the system represented by the linear system, the filter performs optimal estimation of X (t). Give a value. Here, ω is a system error generated in the signal generation process, and v is an observation error generated in the observation process. The input of this filter is the observed value Y (t) and the output is the optimal estimate of X (t). The estimated amount X (t | t) of the state vector using the information up to the time t at 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, 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 equation (3), Σ (t | t
-1) is a predicted amount of error covariance at time t-1 at time t-1. Σ _v is the variance of the observation error v generated in the observation process, and is represented by the following equations (4) and (5), respectively.

Σ (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 equations (4) and (5), Σ _w is the variance of the system error ω generated in the signal process, and I is the unit matrix. Note that the subscript ^T indicates a transposed matrix, and ^-1 indicates an inverse matrix. Further, the system error ω and the observation error v are white Gaussian noise having an average value of 0, and are uncorrelated with each other.

The Kalman filter 522 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. Therefore, the definition of the signal generation process will be described first. Since the Kalman filter 522 in the autonomous navigation aims at correcting the error in the autonomous navigation, the offset error εG, the absolute azimuth error εA, the distance coefficient error εK, the absolute position north direction error εY, And five error values of the absolute position eastward error εX. The state transition matrix F gives a temporal change in the error value.

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 εG 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 deterministic change, and is expressed by the following equation (8) assuming that noise is added to the previous distance coefficient error εK.

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

The absolute position northward error εY and the absolute position eastward error εX are obtained by adding the errors caused by the azimuth error and the distance error to the previous errors εY and εX, and It is represented by equation (10). Incidentally, A _t is the true absolute bearing in both these formulas, L is the movement distance from the previous, t is the elapsed time from the previous.

ΕY _t = sin (A _t + εA _t−1 + εG _t−1 · t / 2) · L · (1 + εK _t−1 ) −sin (A _t ) · L + εY _t−1 (9) εX _{t = cos (A t + εA} t-1 + εG t-1 · t / 2) · L · (1 + εK t-1) -cos (A t) · L + εX t-1 ··· (10)

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

[0024]

(Equation 1)

In the above equations (11) to (15),
A is the true absolute direction A_tPlus sensor error
Is obtained from the change in the traveling direction. Also, ω₀Ha
Offset error (offset fluctuation due to temperature drift, etc.)
Min), ω₁Is the absolute azimuth error (gyro scale factor
Error), ω_TwoIs the distance coefficient error (aging), ω_Three, Ω
_FourMeans absolute position error.

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

[0027]

(Equation 2)

In the above equations (16) to (19), the subscript _DRt in the elements of the matrix is a value obtained by the autonomous navigation at time t based on the output signals from the distance sensor 1 and the relative azimuth sensor 2. Similarly, the subscript _GPSt means a value output from the GPS receiver 3 at time t. Elements of observations Y _t (εA _DRt -εA _GPSt) is an absolute azimuth obtained by the autonomous navigation, the difference in the orientation output from the GPS receiver 3, i.e. the true absolutely azimuth obtained by the autonomous navigation Since the absolute azimuth and its error εA _DRt are included, and the azimuth output from the GPS receiver 3 also includes the true absolute azimuth and its error εA _GPSt , it is obtained by taking the difference between them. Things.

[0029] of the observed value Y _t element (εK _DRt -εK _GPSt)
Is a distance coefficient error obtained from a difference between the speed obtained by the autonomous navigation and the speed output from the GPS receiver 3,
(Speed by autonomous navigation-Speed by GPS receiver) /
(Speed by autonomous navigation). The element (εY _DRt −εY _GPSt ) is the difference between the error between the Y component of the absolute position obtained by autonomous navigation and the Y component of the position output from the GPS receiver 3, and the element (εX _DRt −εX _GPSt )
Is the difference between the X component errors of the position.

The observation error v is an error of the GPS receiver 3, and each element of the observation error v is obtained as follows. First, Doppler frequency measurement error and HDOP
(Horizontal Dilution of P
The speed accuracy is obtained by the Doppler frequency measurement error × HDOP, and the azimuth accuracy is obtained as tan ^-1 (speed accuracy / vehicle speed) from the speed accuracy and the vehicle speed. The element (−εA _GPSt ) and the element (−εK _GPSt ) are obtained by squaring the azimuth accuracy and the speed accuracy. In addition, GPS receiver 3
Error EURE (User Eq)
From the relationship between the universal ranging error (HDR) and the HDOP, the positioning accuracy is determined by UARE × HDOP. The element (−εY _GPSt ) and the element (−ε
X _GPSt ) can be obtained by squaring this positioning accuracy.

Next, the processing procedure of the Kalman filter 522 will be described with reference to the flowchart of FIG. First, step ST
At 2501, T1 from the previous positioning or prediction calculation
It is determined whether or not seconds have elapsed. In this determination process, every time two-dimensional or three-dimensional positioning is performed by the GPS receiver 3, a process of correcting an autonomous navigation error is performed in steps ST2503 to ST2509.
If the three-dimensional or three-dimensional positioning cannot be performed, the error is large.
This is provided for performing the processing periodically in T2510 and ST2511.

If T1 seconds have not elapsed since the last positioning or prediction calculation, and the result of the determination in step ST2501 is No, the process branches to step ST2502, where GP
It is determined whether there is observation information from the S receiver 3. G
If there is observation information from the PS receiver 3, the process proceeds to step ST25.
If the decision result in Step 02 is Yes, step ST250
The process proceeds to the Kalman filter calculation process 3 and subsequent. In step ST2503, first, the observation value Y is calculated. This is because the observation information about the speed, the position, and the azimuth outputted from the GPS receiver 3 and the step S in the autonomous navigation are used.
From the absolute azimuth and absolute position obtained in the process of T2103 and the speed of the vehicle obtained from the signal of the distance sensor 1 by a speed calculation process (not shown), the above formulas (16) to (1) are used.
The observation value Y shown in 9) is calculated, and the observation error v generated in the observation process is calculated based on the observation information of the GPS receiver 3 and the like.

Next, in step ST2504, a state transition matrix F is calculated. This is the previous state transition matrix F
The state transition matrix F shown in the above equations (11) to (15) is obtained from the moving distance L and the elapsed time t from the point of calculation of (2) and the absolute azimuth A obtained in step ST2401. Based on the observation value Y and the state transition matrix F calculated in this way, the calculations according to the above equations (1) to (5) are performed to obtain the state X of the system. That is, prediction calculation of error covariance is performed in step ST2505, and calculation of Kalman gain is performed in step ST2506. Next, an error covariance is calculated in step ST2507, and then, in step ST2508, an estimated amount of the state vector is obtained based on the Kalman gain and the observed value. The estimated amount 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.

When the processing up to step ST2508 is completed, the process proceeds to step ST2509, and the error of the autonomous navigation is corrected based on the calculation result. That is, the offset correction amount of the relative orientation sensor 3 is obtained by subtracting the offset error εG from the previous offset correction amount, and the distance coefficient of the distance sensor 1 is obtained by subtracting the distance coefficient error εK from 1 to the previous distance coefficient. By subtracting the absolute azimuth error εA from the previous absolute azimuth, the absolute azimuth position is calculated by subtracting the absolute azimuth error εY from the previous absolute azimuth position. By subtracting the absolute position eastward error εX from the eastward absolute position, the error correction of the autonomous navigation is performed. These processes are repeatedly executed each time the observation information from the GPS receiver 3 is received, and perform the above-described error correction.

Each element of the matrix of the system error ω generated in the signal generation process, that is, the offset error ω ₀ ,
Absolute azimuth error ω ₁ , distance coefficient error ω ₂ , absolute position error ω
₃ and ω ₄ are obtained as follows. First, prescribed values are set for the offset error ω ₀ and the absolute azimuth error ω ₁ . Thereafter, the absolute azimuth error ω is calculated as the sum of the error variances for each predetermined error generation factor generated according to the behavior state of the vehicle.
Recalculate ₁ . For the absolute position errors ω ₃ and ω ₄ , only when the element (σ _AA ² ) of the error covariance matrix is equal to or smaller than a predetermined value, the magnitude of the position error due to the absolute azimuth error is represented by the X and Y axes. Calculate based on the value divided into.

As a conventional locator device, there has been proposed a locator device which measures a moving distance from an acceleration sensor built in the device. Even when the acceleration a _ACC is 0, the acceleration sensor outputs the output signal SG _ACC at a predetermined voltage value ([mV],
Hereinafter, one which is adjusted to indicate) that the offset OF _ACC, greater voltage value than the offset OF _ACC during acceleration, also smaller voltage value than the offset OF _ACC during deceleration is output. The output signal S of this acceleration sensor
To determine the acceleration a _ACC from G _ACC, the following equation (20)
As shown in the offset OF from the output signal SG _ACC
After subtracting _ACC , the value may be multiplied by a scale factor SF _ACC ([m / S ² / mV]) for converting a voltage value into a unit of acceleration. The suffix _ACC in the equation (20) indicates data related to the acceleration sensor, and i indicates time. ΔT is a sampling cycle of the acceleration sensor.

A _ACCi = (SG _ACCi− OF _ACCi ) × SF _ACCi × ΔT (20)

However, actually, the offset OF _ACC and the scale factor SF _ACC are caused by factors such as temperature change.
Therefore, it is necessary to appropriately correct the offset OF _ACC and the scale factor SF _ACC to appropriate values. For the correction of the acceleration sensor, see, for example,
Since it is proposed in Japanese Patent Publication No. -307032, its contents will be briefly described here.

FIG. 26 is a block diagram showing the configuration of such a conventional locator device. In the drawing, 1 is a distance sensor, 2 is a relative direction sensor, 3 is a GPS receiver, and 4 is the longitudinal acceleration of the vehicle. , And 5 is a signal processor. In the signal processor 5, reference numeral 50 denotes a distance sensor 1, a relative azimuth sensor 2, a GPS
An interface circuit 51 for interfacing each output signal of the receiver 3 and the acceleration sensor 4, based on a control program stored in advance in a memory area, estimates the behavior information of the vehicle and corrects the sensors. It is a microcomputer that performs.

Next, the operation will be described. Here, FIG.
FIG. 7 is a flowchart showing an outline of the overall processing operation of the microcomputer 51, which will be described below with reference to FIG. When power is applied to the locator device, first, in step ST2701, initial value setting is performed.
Next, in step ST2702, calculation of the longitudinal acceleration based on the output signal of the acceleration sensor 4 and estimation of the speed of the vehicle are performed. Next, in step ST2703, the GPS receiver 3
The current position, heading, and speed of the vehicle observed by
It receives observation information such as a PS position, a GPS azimuth, and a GPS speed), and calculates a change in the traveling azimuth of the vehicle from the output signal of the relative azimuth sensor 2 in step ST2704. Next, the flow proceeds to step ST2705 to calculate the travel distance of the vehicle based on these calculated values, and to estimate the current position of the vehicle based on the output signals of the GPS receiver 3 and various sensors.

Thereafter, in step ST2706, the stationary state determination processing and the departure state determination processing are performed based on the output signal of the relative direction sensor 2, or, in step ST27.
At 07, based on the output signal of the relative azimuth sensor 2, the process of updating the offset at the time of stopping or traveling is performed. Further, in step ST2708, the stop state determination processing and the departure state determination processing based on the output signal of the acceleration sensor 4 are performed, or in step ST2709, the offset during stop or running is updated based on the output signal of the acceleration sensor 4. Perform processing. Next, the process proceeds to step ST2710, where the speed estimated from the output signal of the acceleration sensor 4 is reset based on the observation data of the GPS receiver 3. Next, in step ST2711, the scale factor of the acceleration sensor 4 is corrected based on the observation information of the GPS receiver 3. Thereafter, the processes from step ST2702 to step ST2711 are repeatedly executed.

Next, an explanation of the operation relating to the acceleration sensor 4 will be supplemented. First, in step ST2702, the acceleration a _{A CC} and the vehicle speed Vel _ACC in the longitudinal direction based on the output signal SG _ACC of the acceleration sensor 4, calculated respectively using the above equation (20) the following equation (21), also The travel distance Δd ([m]) in step ST2705 is calculated by the following equation (22).

Vel _ACCi = Vel _ACCI-1 + a _ACCi × ΔT (21) Δd = Vel _ACCi × ΔT + ／ × a _ACCi × ΔT ² (22)

Next, the stationary state determination process performed in step ST2706 or ST2708 is performed when the standard deviation of the output signal of the acceleration sensor 4 or the relative azimuth sensor 2 for a certain period of time is less than a predetermined value.
Alternatively, when the GPS speed is 0 km / h during GPS positioning, it is determined that the vehicle is in a stopped state. Further, after the deceleration state in which the average value of the output signal of the acceleration sensor 4 during the predetermined time period is negative, the vehicle is also determined to be in the stopped state when the output signal of the acceleration sensor 4 changes by a predetermined value or more. on the other hand,
The departure state determination processing is performed when it is determined that the vehicle is stopped, when the amount of change in the output signal of the acceleration sensor 4 or the relative azimuth sensor 2 during a certain period of time becomes larger than a predetermined value, or when the GPS Speed is 0km / h
It is determined that the vehicle is in the departure state when it is no longer.

Updating of the offset value in step ST2709 is performed during a stationary state where no longitudinal acceleration is generated and when the vehicle is traveling at a constant speed. The average value of the output signal of the acceleration sensor 4 is set as the offset. Here, the constant-speed running state is a state in which the speed estimated from the output signal of the acceleration sensor 4 is 30 km / h or more, and the estimated speed and the change amount of the change in the traveling azimuth are small.
During the positioning, the small change amount of the GPS speed is also added to the determination condition.

In step ST2710, the following equation (2)
As shown in 3) and Expression (24), the estimated speed Vel _ACC by the acceleration sensor 4 is corrected based on the GPS speed Vel _GPS . However, the maximum value of k is assumed to be 1.0.

[0047] _{Vel ACCi = Vel ACCi + (Vel} GPSi -Vel ACCi) × k ··· (23) k = Vel GPSi / 30 ··· (24)

The scale factor correction process in step ST2711 is performed in a constant speed running state and a state in which the amount of change in acceleration calculated from the output signal of the acceleration sensor 4 is small (a constant acceleration running state). This is executed in a state where the change amount of the angular velocity calculated from the output signal is small (constant angular acceleration running state) and the change amount of the acceleration which is the change amount of the GPS speed is small (constant acceleration running state). The average value of the estimated speed Vel _ACCi of the acceleration sensor 4 and the GPS speed V in the traveling state
el The average value of _GPSi is compared, and the scale factor of the acceleration sensor 4 is _adjusted so that the average value Vel _ACCavei of the estimated speed of the acceleration sensor 4 _matches the average value Vel _GPSavei of the GPS speed as shown in the following equation (25). Modify SF _ACCi .

SF _ACCi = Vel _GPSavei / Vel _ACCavei × SF _ACCi-1 (25)

[0050]

Since the conventional locator device (locator device disclosed in Japanese Patent Application Laid-Open No. 8-68655) is constructed as described above, the Kalman filter 522 uses the offset error, the distance coefficient error, The respective corrections in the autonomous navigation are performed by obtaining the absolute azimuth error and the absolute position error, and the offset error, the distance coefficient error, the absolute azimuth error, the absolute Since the accuracy of the position error is determined, and the HDOP and the UERE used for calculating the observation error are indices for the user to estimate the position accuracy, the error at each time cannot be directly expressed, and the observation error There is a problem that cannot be set with high accuracy.

Regarding the system error, a prescribed value is set for the offset error ω ₀ and the absolute azimuth error ω _1, and the sum of the error variances for each of the predetermined error generation factors generated according to the behavior state of the vehicle is expressed as: due to the absolute recalculated azimuth error omega _1, there is a problem that can not be set absolute azimuth error omega ₁ with high accuracy, also the absolute position error omega _3, the omega ₄ also, elements related to the orientation error of the error covariance matrix Only when the value is equal to or less than a predetermined value, the magnitude of the amount of position error is calculated as a value divided into the X and Y axes by the absolute azimuth error of which accuracy is not high, so that the accuracy cannot be set high. There was a problem that.

Further, offset error, distance coefficient error,
Since the absolute azimuth error and absolute position error are calculated simultaneously as the state vector, if any one of the system error and the observation error is incorrect, the other matrix elements of the state vector are also determined by the Kalman gain. There is a problem that an error is caused by being reflected in the value by an amount, and therefore, there is a problem that the Kalman filter 522 cannot perform detection and setting according to the characteristics of each error.

Further, although the accuracy of the GPS speed may be reduced when the vehicle is running at low speed, no countermeasure for this is shown in the calculation of the distance coefficient error, and the dispersion of the GPS speed is calculated from the output signal of the distance sensor 1. Since the variance of the calculated velocity is larger than the variance of the calculated velocity, it is considered that the variance of the calculated distance coefficient error increases.
When only the offset error is to be corrected and a gyro is used as the relative azimuth sensor 2, a scale factor that can vary up to about ± 20% from the standard value is not considered. There is also a problem that the azimuth error caused by the error is erroneously corrected as an offset error.

Further, the locator device disclosed in Japanese Patent Application Laid-Open No. 10-307032 estimates the speed of the vehicle from the output signal of the acceleration sensor 4, and detects the drift caused by the offset of the acceleration sensor 4 and the mounting of the locator device. In order to correct the gravitational influence of the scale factor of the acceleration sensor 4 due to the inclination of the time, the offset and the scale factor of the acceleration sensor 4 are corrected based on the GPS speed. It is required that the vehicle in a stopped state, a constant velocity traveling state, a constant acceleration state, or a constant angular velocity state be in a stable traveling state. However, in an urban area where there are many traffic lights, the vehicle must be in a stable traveling state. , The correction of the offset and scale factor of the acceleration sensor 4 becomes insufficient, and If the offset is insufficiently corrected when the drift is large, the offset gradually changes even if the longitudinal acceleration is constant, and the value obtained by subtracting the offset from the output signal is not constant. As a result, there is a problem that the vehicle is erroneously recognized as being in a state of being accelerated or decelerated, and cannot be further corrected.

When the vehicle is in a stable running state and the GPS speed is 30 km / h or more, the estimated speed by the acceleration sensor 4 and the correction amount of the scale factor are large. There is also a problem that the accuracy of the estimated speed and the scale factor is reduced due to the influence of.

The present invention has been made to solve the above-described problems. When measuring the speed or the moving distance of a vehicle from the output signal of the distance sensor, the scale factor of the distance sensor that varies greatly depending on the vehicle is calculated as follows. It is an object of the present invention to obtain a locator device that can quickly correct after a start of traveling.

Further, the present invention provides a locator device capable of stably correcting a scale factor of a distance sensor that varies greatly depending on a vehicle after the start of traveling when measuring a speed or a moving distance of the vehicle from an output of the distance sensor. With the goal.

Still another object of the present invention is to provide a locator device capable of correcting a scale factor immediately after the start of traveling, even if the GPS receiver is traveling in a place where the positioning is likely to be in a non-positioning state.

A further object of the present invention is to provide a locator device which can be easily attached to and detached from a vehicle.

Further, according to the present invention, when the GPS navigation and the autonomous navigation are combined, the present position and the heading of the vehicle can be easily obtained from the observation information of the GPS receiver and the measurement information of the sensor for the autonomous navigation by one calculation method. It is another object of the present invention to obtain a locator device which can be estimated with high accuracy.

A further object of the present invention is to provide a locator device capable of accurately correcting the offset and scale factor of an angular velocity sensor such as a gyro or a relative azimuth sensor.

Another object of the present invention is to provide a locator device capable of preventing erroneous correction of a scale factor.

Further, according to the present invention, when the locator device is used after being replaced by a plurality of vehicles, it can be determined that the locator device has been replaced by another vehicle, and when it is determined that the locator device has been replaced, It is another object of the present invention to provide a locator device that can immediately perform correction relating to a current vehicle.

It is a further object of the present invention to provide a locator device which does not require re-correction of a vehicle every time when the vehicle is replaced by another vehicle.

[0065]

A locator device according to the present invention includes an absolute position observing means, a distance sensor, and a system model for calculating a vehicle speed by multiplying an output signal of the distance sensor by a scale factor. Based on the observation model showing the relationship between the vehicle speed observed by the absolute position observation means and the speed in the system model, the speed and scale factor in the system model are used in the matrix element of the state vector, and the absolute position is observed in the observed value. A velocity calculation means for calculating a velocity and a scale factor, which are matrix elements of the state vector, by a Kalman filter in which the velocity observed by the means is set, respectively.The system model and the observation model in the velocity calculation means use an output signal of the distance sensor and an absolute value. Handles each average value of the velocity observed by the position observation means, and calculates the average Of is obtained so as to set the difference in gradient associated to time course as an absolute position observation means observing error values obtained by multiplying the average value of the observed rate.

In the locator device according to the present invention, in the speed calculation means, a low-pass filter is provided after the Kalman filter, and by passing the low-pass filter, the scale factor of the distance sensor, which is a matrix element of the state vector in the Kalman filter, is obtained. The variation is reduced and is used as a correction value of the distance sensor.

The locator device according to the present invention is provided with an acceleration sensor for outputting a signal corresponding to the longitudinal acceleration of the vehicle in place of the absolute position observing means, and when the variation of the output signal of the acceleration sensor is less than a predetermined value. Offset detection means for offsetting a value obtained by averaging the output signal; and calculating the acceleration per unit time of the vehicle by subtracting the offset from the output signal of the acceleration sensor and multiplying by the scale factor of the acceleration sensor. And speed estimation means for estimating the speed by integrating the speed sensor, and correcting the scale factor of the distance sensor so that the speed calculated from the output signal of the distance sensor matches the speed estimated by the speed estimation means. Then, the speed is calculated from the output signal of the distance sensor.

The locator device according to the present invention includes an acceleration sensor instead of the distance sensor, and speed estimation means for estimating the speed of the vehicle from an output signal of the acceleration sensor instead of the speed calculation means. When the amount of signal fluctuation is equal to or less than a predetermined value, the output signal of the acceleration sensor is provided with offset detection means for offsetting a value obtained by averaging the output signal. After the speed estimation means subtracts the offset from the output signal of the acceleration sensor, the scale factor of the acceleration sensor is calculated. Based on the system model that calculates the acceleration per unit time of the vehicle by multiplying and then calculates the speed by integrating the acceleration, and the observation model that expresses the relationship between the speed observed by the absolute position observation means and the speed in the system model , The speed and scale factor in the system model and off The Tsu bets, the Kalman filter is set respectively the rate observed at the absolute position observing means to observations, in which so as to calculate the speed is a matrix element of the state vector, the offset, and the scale factor.

The locator device according to the present invention comprises: a relative azimuth sensor for outputting a signal corresponding to a change in the heading of the vehicle;
An azimuth change calculating means for calculating a heading change of the vehicle per unit time from an output signal of the relative azimuth sensor is provided, and a moving distance per unit time and an azimuth change calculated from speed by the speed calculating means or the speed estimating means are provided. A system model that calculates the current position and heading of the vehicle from the heading change per unit time calculated by the means, and an observation model that expresses the relationship between the current position of the vehicle and the current position in the system model that is observed by the absolute position observation means Based on the Kalman filter which sets the current position and the heading in the system model to the matrix element of the state vector, and the current position of the vehicle observed by the absolute position observing means to the observed value, the matrix element of the vehicle is the matrix element of the state vector. A position / azimuth estimating means for calculating the current position and the heading is provided. A plurality of points separated by a predetermined distance or more, or a variation amount of a difference between respective current positions by an absolute position observing means and a position / azimuth estimating means at every elapse of a predetermined time is set as an observation error. The average value of the difference between the two traveling directions by the absolute position observing means and the position / azimuth estimating means is set as the azimuth error which is a matrix element of the system error, and the speed is calculated by the speed calculating means or the speed estimating means and the absolute position observing means. Is set as a distance error which is a matrix element of a system error.

The locator device according to the present invention includes offset detection means for offsetting an average of the output signals of the relative azimuth sensors when the vehicle speed obtained by the speed calculation means or the speed estimation means is zero. The change calculation means calculates the heading change per unit time of the vehicle by subtracting an offset from the output signal of the relative heading sensor and multiplying by a scale factor, and integrates the heading change with a system model. Based on the observation model representing the relationship between the heading estimated by the heading estimation means and the integrated azimuth in the system model, the offset and scale factor of the relative azimuth sensor in the system model and the integrated azimuth in the matrix element of the state vector, position·
The offset and scale factor of the relative azimuth sensor, which are the matrix elements of the state vector, are calculated by the Kalman filter in which the traveling azimuths estimated by the azimuth estimating means are respectively set, and the calculated values are used as correction values of the relative azimuth sensor. The offset which is the matrix element of the system error from the amount, and the scale which is the matrix element of the system error from the difference between the turning angles of the vehicle obtained from the respective azimuth information of the position / azimuth estimating means and the azimuth change calculating means when turning the vehicle. A factor error is set, and an azimuth error, which is a matrix element of a system error used by the position / azimuth estimating means, is set as an observation error.

In the locator device according to the present invention, the azimuth change calculating means monitors the fluctuation of the offset of the relative azimuth sensor, and the fluctuation of the offset of the relative azimuth sensor, which is a matrix element of the state vector in the Kalman filter, is determined in advance. The validity of the scale factor correction is determined based on whether or not the predetermined value is equal to or greater than the predetermined value.

The locator device according to the present invention monitors the correction of the scale factor of the distance sensor and performs a speed calculation function for judging that the locator device has been replaced by another vehicle when the change has exceeded a predetermined ratio. It is what the means have.

A locator device according to the present invention includes a storage unit having a plurality of memory areas for storing a scale factor of a distance sensor, and a storage destination specifying unit for specifying an address of a memory area as a storage destination by a user operation. And reads and writes the scale factor of the distance sensor to the storage means in accordance with the address of the designated memory area.

In the locator device according to the present invention, the scale factor held in the storage means having a plurality of memory areas for storing the scale factor of the distance sensor and the memory access time as a set is stored by the storage destination search means. When a value close to the calculated and calculated scale factor is stored in the storage means, the address of the memory area is used. When the value is not stored, the address of the unused memory area is used. If there is no memory access time, a memory location search means for searching the address of the memory area with the oldest memory access time is provided, and the calculated scale factor of the distance sensor is read and written to the memory area at the address specified by the memory location search means. And read / write the current time as the memory access time stored as a set with the scale factor. Than it is.

[0075]

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 locator device according to Embodiment 1 of the present invention. In the figure, reference numeral 1 denotes a distance sensor, which outputs a pulse signal according to the traveling distance of the vehicle. 3 is G as absolute position observation means
A PS receiver is connected to a GPS antenna for receiving radio waves from a satellite, and at least measures and outputs the speed of the vehicle based on the radio waves received from the satellite. Reference numeral 5 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 5, 51
1 is a pulse signal from the distance sensor 1 and a GPS receiver 3
Is a speed calculating means for calculating the speed of the vehicle based on the observation information from the vehicle and calculating a scale factor for converting the output signal of the distance sensor 1 into a distance. The signal processor 5 includes the speed calculating means. 511.

Next, the operation will be described. Here, the operation of the speed calculation means 511 of the signal processor 5 will be described with reference to the drawings. FIG. 2 is a flowchart showing the processing contents of the main routine of the signal processor 5, and FIG.
FIG. 4 is a flow chart showing the processing content of an interrupt processing 1 executed every time, FIG. 4 is a flow chart showing the processing content of an interrupt processing 2 executed when a signal is output from the distance sensor 1, and FIG. 5 is a GPS receiver 9 is a flowchart showing the processing contents of an interrupt processing 3 for receiving observation information output from the third observation cycle in each observation cycle.

The main routine shown in FIG. 2 manages the operation of the speed calculation means 511 of the signal processor 5. First, all processing is initialized in step ST201. Next, in step ST202, it is determined with reference to the processing start flag whether or not it is time to start the processing relating to the interrupt processing 1. As a result, if the processing start flag is set, it is determined that the processing start timing has been reached, and the process proceeds to step ST203. If the processing start flag has been cleared, it is determined that the processing has not been started, and the process waits until the processing start flag is set. In step ST203, after clearing the processing start flag for the next processing, step ST2 is executed.
Go to 04. In step ST204, first, a predetermined time Δ
For the output signal count value ΔN _i of the distance sensor 1 counted in the interrupt processing 2 executed for each T, GP
The average value ΔN _j of the count value is obtained for each observation period ΔT _GPS of the S receiver 3. Then, the velocity Vel _DRj is calculated by the following equation (26), and the output signal count value ΔN _{i is set} to 0 for the next processing.

Vel _DRj = | ΔN _j × SF _SPDj | / ΔT _GPS (26)

Further, the average value ΔN _j of the output signal count values ΔN _i of the distance sensor 1 is used for the processing described later.
_With respect to the GPS speed Vel _GPSj , a value ΔN _{avej obtained} by taking a moving average for a predetermined time and Vel _GPSavej are also calculated. The observation cycle of the GPS receiver 3 is determined by referring to the GPS observation information reception flag. SF _SPDj in the above equation (26) is a scale factor for converting the output signal of the distance sensor 1 into a distance. It is.

Next, in step ST205, the GPS
Whether the reception of the observation information from the receiver 3 has been completed or not is determined again with reference to the GPS observation information reception flag. If the GPS observation information reception flag is set, it is determined that the reception of the observation information from the GPS receiver 3 has been completed, and the process proceeds to step ST206.
It is determined that the reception has not been completed, and the process proceeds to step ST216. In step ST206, the GPS observation information reception flag is cleared for the next processing. Thereafter, in steps ST207 to ST216, processing for correcting the scale factor SF _SPDj of the distance sensor 1 is executed, and a calculation formula (Kalman filter) described below is used.
Vehicle speed Vel _DRj and scale factor SF _{SPDj at}
Is calculated.

The Kalman filter according to the first embodiment uses the output signal ΔN _avej of distance sensor 1 at discrete time j.
And the scale factor SF _SPDj and the acceleration a, the velocity Vel using the following equations (27) to (29).
_Based on a system model for calculating _DRavej, and a relation between the velocity Vel _GPSavej observed by the GPS receiver 3 and the velocity _VelDRavej of the system model, based on an observation model expressed by the following equation (30), the following equations (31) are used. ) To expression (3)
3) and equations (34) to (37)
And the Kalman filter equation expressed by the following equations (38) to (42),
The vehicle speed Vel _DRevej and the scale factor SF _SPDj of the distance sensor 1 are calculated.

[0083] SF_SPDj= SF_SPDj-1 ... (27) Vel_DRavej= Vel_DRavej-1+ A × ΔT_GPS ... (28) a = (Vel_DRavej-Vel_DRavej-1) / ΔT_GPS  = (ΔN_avej−ΔN_avej-1) / ΔT_GPS ^Two× SF_SPDj-1 ... (29)

Vel _GPSavej = Vel _DRavej + δVel _GPSavej (30)

[0085]

(Equation 3)

Y _j = H · x _j + v _j (34) y _j = Vel _GPSavej (35) H = [0,1] (36) v _j = δVel _GPSavej ... (37)

[0087] _{x j | j = x j |} j-1 + K j {y j - (Hx j | j-1 + v j)} ··· (38) x j + 1 | j = F j x j | j (39) K _j = Σ _{j | j-1} H ^T [HΣ _{j | j-1} H ^T + σ _vj ² ] ⁻¹ (40) Σ _{j | j} = Σ _{j | j-1} − K _j HΣ _{j | j-1} (41) Σ _{j + 1 | j} = F _j Σ _{j | j} F _j ^T (42)

In the above equations (27) to (29) and (30), ΔT _GPS is the observation period of the GPS receiver, j is the discrete time synchronized with ΔT _GPS , and SF _SPDj is the distance at discrete time j. Scale factor of sensor 1, Ve
l _DRavej is the average value of the speed calculated from the output signal of the distance sensor 1, Vel _GPSavej is the average value of the speed observed by the GPS receiver 2, and ΔN _avej is the discrete time j-1 to the discrete time j.
Average value of the output signal count value of the distance sensor 1 during a
Is the acceleration from discrete time j-1 to discrete time j, δVe
l _GPSavej is the average value V of the speed observed by the GPS receiver 2.
el _Error included in _GPSavej .

In the above equations (31) to (33) and equations (34) to (37), x _j , F _j , y _j ,
v _j is a state vector, a state transition matrix, an observation value, and an observation error at discrete time j, and H is an observation matrix. Further, in Equations (38) to (42), x _{j | j} is the estimated amount of the state vector at discrete time j, x _{j + 1 | j} is the predicted amount of the state vector at discrete time j + 1 at discrete time j, and K _j is the Kalman gain, Σ _{j | j} is the estimated amount of the covariance matrix of the state vector at discrete time j, Σ _{j + 1 | j} is the predicted amount of the covariance matrix of discrete time j + 1 at discrete time j,
σ _vj ² is the variance matrix of the observation error. Since each element of the matrix can be obtained by calculating a determinant, the description thereof is omitted here.

In FIG. 2, the processing is performed in step ST207.
Then, it is determined whether the GPS receiver 3 is performing two-dimensional or three-dimensional positioning. If two-dimensional or three-dimensional positioning has been performed, the process proceeds to step ST208, and if not, the process proceeds to step ST216. Step ST208
In equation (43) and equation (4), the speed of the GPS receiver 3 and the rate of change of the output signal of the distance sensor 1 are respectively shown below.
Determined in 4).

Ratio _GPSj = Vel _GPSavej / Vel _GPSavej−1 −1 (43) Ratio _DRj = ΔN _avej / ΔN _avej−1 −1 (44)

Then, the gradients of the respective change rates over the continuous predetermined time ΔT _n are calculated using the following equations (45) to (47), and the time of the change rate is calculated. The difference between the gradient due to the objective change and the GPS speed is obtained as the observation error.

[0093]

(Equation 4)

In the above equations (43), (44) and (45) to (47), Ratio _GPSj and R
“atio _DRj” is a rate of change of each of the GPS speed and the output signal of the distance sensor 1, and Lamp Ratio _GPS and L
ampRatio _DR is a gradient of the rate of change of the output signal of the distance sensor 1 with time with respect to the GPS speed.
In Equations (45) to (47), n is the number of calculations of the Ratio _GPSj and the Ratio _{DRj at} the predetermined time ΔT _n , and their average is the Ratio _DRave and the Ratio _DRt.
io _GPSave .

Next, the process proceeds to step ST209 to determine whether or not correction is possible. In this step ST209,
When the GPS speed is equal to or higher than a predetermined value and the difference in gradient due to the time change of the change rate is smaller than a predetermined value, it is determined that correction is possible. If correction is possible, the process proceeds to step ST210, and if correction is not possible, step ST216 is performed. Proceed to. Step S
From T210 to step ST214, the above equation (27)
To the estimated amount X _{j | j of the} state vector and the estimated amount X _{j + 1 | j} , the estimated amount of the error covariance Σ _{j | j} and the estimated amount に 従_って according to Equation (42).
_{j + 1 | j} and Kalman gain K _j are calculated. That is, in step ST210, the state vector is estimated and its estimated amount _{Xj | j} is estimated, and in step ST211 the state vector is estimated to obtain its estimated amount _{Xj + 1 | j} , and step ST21 is performed.
The estimated amount of sigma _j 2 estimates the error covariance in _| a _j, the predicted amount to predict the error covariance in step ST 213 sigma
_{j + 1 | j} is obtained, and in step ST214, a Kalman gain K _j is calculated.

[0096] Next, in step ST215, the estimator X _j of the state vector calculated in such a Kalman filter _| If the value is the normal range of the scale factor components X ₁₁ is a matrix element of _j, the scale factor components X ₁₁ The scale factor SF _SPDj of the distance sensor 1 is updated using the value, and the process proceeds to Step ST217. On the other hand, step S
In T216, a matrix element related to the speed of the state vector is initialized, and the process proceeds to Step ST217. Step ST
After outputting a value of the estimated amount of velocity factor X ₁₂ is a matrix element of 217 in the state vector, the flow returns to step ST 202.

Next, interrupt processing 1 will be described. The interrupt process 1 is started at a predetermined time interval Δt, and as shown in the flowchart of FIG. 3, in step ST301, a process start flag indicating a process start timing is set, and the interrupt process 1 ends.

Next, interrupt processing 2 will be described. This interrupt processing 2 is started when a signal is output from the distance sensor 1, and as shown in the flowchart of FIG.
At T401, 1 is added to the vehicle speed signal counter, and this interrupt processing 2 ends.

Next, interrupt processing 3 will be described. This interrupt processing 3 is for receiving and storing observation information output from the GPS receiver 3 for each observation cycle, and as shown in the flow chart of FIG. 5, first, in step ST501, output from the GPS receiver 3 is performed. Receiving and storing the observation information. Next, in step ST502, it is determined whether or not reception of all observation information has been completed.
The process proceeds to 03, and if not completed, this interrupt processing 3 ends. In step ST503, it is determined that reception of all observation information has been completed, and a GPS observation signal reception flag is set.

As described above, according to the first embodiment,
The GPS receiver 3 receives a radio wave from a satellite to observe the speed of the vehicle, etc., and outputs a signal corresponding to the moving distance of the vehicle by the distance sensor 1. The average value of the output signal of the speed and the distance sensor is calculated, and when the gradient of the average value with the temporal change is equal to or less than a predetermined value, the average value of the GPS speed at the predetermined time is used as an observation value according to the Kalman filter equation. The correction of the scale factor of the distance sensor 1 which is a matrix element of the state vector and the estimation of the speed of the vehicle are performed, and the difference between the GPS speed and the gradient of the average value of the output signal of the distance sensor 1 due to the temporal change is detected by the GPS. The value multiplied by the speed average is used as the error in the GPS speed to calculate the error covariance prediction and estimate, and
When the PS receiver 3 is in the non-positioning state, the speed of the vehicle is calculated from the output signal of the distance sensor 1 and the scale factor according to the Kalman filter equation, so when it is determined that the error of the GPS speed is small, Since the dedicated Kalman filter further corrects the scale factor of the distance sensor 1 based on the GPS speed and estimates the vehicle speed, even if the GPS error fluctuates,
After the start of traveling, the scale factor of the distance sensor 1 can be corrected quickly and optimally, and the speed of the vehicle can be estimated with high accuracy after the start of traveling.

Embodiment 2 FIG. Next, a second embodiment of the present invention will be described. In the locator device according to the second embodiment, a low-pass filter is provided after the Kalman filter in the speed calculation means 511 to smooth the scale factor of the distance sensor 1 calculated by the Kalman filter. Are the same as those in the first embodiment shown in the block diagram of FIG.

Next, the operation will be described. The operation is basically the same as that of the first embodiment, and is the main routine of the speed calculation means 511 shown in FIG.
Since only the operation of step ST215 in the flowchart shown in FIG. 7 is different from that of the first embodiment, only the operation of step ST215 will be described here, and the description of the other steps will be omitted.

In the second embodiment,
Matrix element of the estimator of the state vector
High-frequency noise included in the scale factor
A low-pass filter should be placed after the Leman filter.
And pass through this low-pass filter to remove
The smoothed scale factor is used as the correction value of the distance sensor 1.
are doing. That is, in step ST215 of FIG.
As shown in the following equation (48), the estimated amount of the state vector
Scale factor element X which is a matrix element₁₁And scale
Previous value SF of actor_SPDj-1Weighted average of
LE FACTOR SF _SPDjHas been updated. Note that this equation (4)
In 8), w is a weighting coefficient.

SF _SPDj = SF _SPDj-1 + (X _{11 −SF SPDj-1} ) × w (48)

As described above, according to the second embodiment,
Since the speed calculation means 511 smoothes the scale factor of the distance sensor calculated by the Kalman filter by the low-pass filter provided at the subsequent stage of the Kalman filter, the fluctuation of the scale factor of the distance sensor 1 which is a matrix element of the state vector is suppressed. This makes it possible to stably estimate the speed of the vehicle.

Embodiment 3 Next, a third embodiment of the present invention will be described. FIG. 6 shows Embodiment 3 of the present invention.
FIG. 2 is a block diagram showing a configuration of a locator device according to the first embodiment. In the figure, reference numeral 1 denotes a distance sensor equivalent to that of the first embodiment shown in FIG. 1 with the same reference numerals, and outputs a pulse signal according to the traveling distance of the vehicle. Reference numeral 4 denotes an acceleration sensor which outputs a voltage corresponding to the acceleration in the front-rear direction of the vehicle.

An offset detecting means 512 detects an offset of the acceleration sensor 4 and a speed estimating means 513 estimating a vehicle speed from an output signal of the acceleration sensor 4. Reference numeral 511 denotes speed calculating means for calculating the speed of the vehicle from the output signal of the distance sensor 1 and correcting the scale factor of the distance sensor 1 based on the output of the speed estimating means 513. Reference numeral 5 denotes a signal processor including a computer for calculating the speed of the vehicle in accordance with a control program stored in the memory in advance. The signal processor 5 includes the speed calculating means 511, the offset detecting means 512, and the speed estimating means 513. This embodiment is different from that of the first embodiment shown in FIG.

Next, the operation will be described. Here, the operation of each means of the signal processor 5 will be described with reference to FIG. FIG. 7 is a flowchart of the main routine showing the processing contents of each means of the signal processor 5. In addition, there are interrupt processing 1 performed at every predetermined time Δt shown in FIG. 3 and interrupt processing 2 performed when a signal is output from the distance sensor 1 shown in FIG. The description is omitted because it is the same as that described in the first embodiment.

The main routine shown in FIG. 7 comprises an offset detecting means 512, a speed estimating means 513 of the signal processor 5,
And each operation of the speed calculation means 511. First, all processing is initialized in step ST701. Next, in step ST702, it is determined whether or not it is the processing start timing related to the interrupt processing 1 with reference to the processing start flag. As a result, if the process start flag is set, the process proceeds to step ST703 as the process start timing, and if the process start flag is cleared, it is determined that the process is not the process start timing, and the process waits until the process start flag is set. In step ST703, the processing start flag is cleared for the next processing. Next, step S
At T704, the speed calculation unit 511 executes a speed calculation process. That is, the speed Vel _DRi is calculated from the output signal count value ΔN _{i of} the distance sensor 1 counted in the interrupt process 2 at every predetermined time by using the above-described formula (26), and the output signal for the next process is calculated. Count value Δ
Set _Ni to 0.

Next, the process proceeds to step ST705, in which the voltage output from the acceleration sensor 4 according to the longitudinal acceleration of the vehicle is A / D converted. Next, in step ST706, a process of detecting the offset of the acceleration sensor 4 by the offset detecting means 512 is executed. That is, the history of the output signal of the acceleration sensor 4 for a predetermined time is stored, and
When the output signal of the distance sensor 1 becomes 0 for a predetermined time or more, an average value of the values stored as the history of the output signal of the acceleration sensor 4 is obtained, and this average value is held as an offset of the acceleration sensor 4. I do. Next, step S
At T707, a process of speed estimation by the speed estimation means 513 is executed. That is, the offset detecting means 51
2, when the offset of the acceleration sensor 4 is updated, the estimated velocity Vel _{ACC is set} to 0, and otherwise, the above equation (2)
0), the acceleration Vel is calculated by the equation (21).
Estimate _ACC . The scale factor of the acceleration sensor 4 is a fixed value only in the third embodiment.

After that, the process proceeds to step ST 708, where the speed calculation means 511 executes a process of correcting the scale factor of the distance sensor 1. That is, when the offset of the acceleration sensor 4 is updated by the offset detection unit 512,
After that, until the predetermined time elapses, the speed Vel _ACC estimated by the speed estimating unit 513 is added to the speed calculating unit 511.
The scale factor of the distance sensor 1 is corrected so that the velocity Vel _DR calculated in the step (1) matches. Finally, step ST709
In step, the speed Vel _DR is output from the speed calculation means 511, and the process returns to step ST702.

As described above, according to the third embodiment,
When the amount of change in the output signal of the acceleration sensor 4 is equal to or less than a predetermined value, an average value of the output signal is used as an offset. After the offset is subtracted from the output signal of the acceleration sensor 4, the scale factor of the acceleration sensor 4 is multiplied. The acceleration per unit time of the vehicle is calculated, the acceleration is integrated to estimate the speed, and the scale factor of the distance sensor 1 is set so that the estimated speed matches the speed calculated from the output signal of the distance sensor 1. Is corrected and the speed is calculated from the output signal of the distance sensor 1, so that the GPS receiver 3
Even if the vehicle travels in a place that is likely to be in the non-positioning state, the scale factor of the distance sensor 1 can be corrected immediately after the traveling starts, and the effect that the speed of the vehicle can be estimated with high accuracy after the traveling starts can be obtained. .

Embodiment 4 Next, a fourth embodiment of the present invention will be described. FIG. 8 shows Embodiment 4 of the present invention.
FIG. 2 is a block diagram showing a configuration of a locator device according to the first embodiment. In the figure, reference numeral 3 denotes a GP as an absolute position observing means corresponding to that of the first embodiment shown in FIG.
The S receiver has a GPS antenna for receiving a radio wave from a satellite, and observes and outputs at least the speed of the vehicle using the radio wave from the satellite. Reference numeral 4 denotes an acceleration sensor equivalent to that of the third embodiment shown in FIG.
A voltage corresponding to the longitudinal acceleration of the vehicle is output.

Reference numeral 512 denotes an offset detecting means for detecting the offset of the acceleration sensor 4 which is equivalent to that of the third embodiment shown in FIG. 513
The third embodiment shown in FIG. 6 with the same reference numerals in FIG. 6 calculates the speed of the vehicle and corrects the offset and scale factor used when converting the output signal of the acceleration sensor 4 into acceleration. It is a different speed estimation means from that of. Reference numeral 5 denotes a signal processor including a computer for calculating the speed of the vehicle in accordance with a control program stored in a memory in advance. The signal processor 5 is constituted by the offset detecting means 512 and the speed estimating means 513. 6 are different from those of the first embodiment or the third embodiment shown in FIG.

Next, the operation will be described. Here, the operation of the offset detection means 512 and the speed estimation means 513 of the signal processor 5 will be described with reference to FIG. This figure 9
8 is a flowchart of a main routine showing processing contents of a speed estimating unit 513 of the signal processor 5; In addition, there are interrupt processing 1 performed at every predetermined time Δt shown in FIG. 3 and interrupt processing 3 receiving observation information output from the GPS receiver 3 at each observation cycle shown in FIG. However, since they are the same as those described in the first embodiment, description thereof will be omitted.

The main routine shown in FIG. 9 manages the operations of the offset detecting means 512 and the speed estimating means 513 of the signal processor 5. First, in step ST901, all processes are initialized. Next, in step ST902, it is determined whether or not it is the processing start timing related to the interrupt processing 1 with reference to the processing start flag. If the process start flag has been set, the process proceeds to step ST903 as the process start timing. If the process start flag has been cleared, it is determined that the process has not started, and the process waits until the process start flag is set. In step ST903, the processing start flag is cleared for the next processing. Next, in step ST904, the standard deviation of the output signal of the acceleration sensor 4 for a predetermined time is calculated, and if the standard deviation is equal to or smaller than a predetermined value, it is determined that the vehicle is in a stopped state; otherwise, it is determined that the vehicle is in a running state. . In step ST905, it is determined whether or not the vehicle is running based on the result of the determination in step ST904. As a result, when the vehicle is not traveling, the process proceeds to step ST906, and when the vehicle is traveling, the process proceeds to step ST907.

This step ST906 shows the processing of the offset detecting means 512, in which a value obtained by averaging the output signals of the acceleration sensor 4 for a predetermined time is set as an offset, the estimated speed is set to 0, and then the step ST9 is executed.
Proceed to 20. On the other hand, from step ST907 to step S907
T919 indicates processing of the speed estimating means 513, and the vehicle speed Vel _DRi and the offset O of the acceleration sensor 4 are calculated by a calculation formula (Kalman filter) described below.
After calculating F _ACC and scale factor SF _ACC , the process proceeds to step ST920. In step ST920, the speed Vel _DRi is output from the speed estimating means 513, and the process returns to step ST902.

The Kalman filter according to the fourth embodiment calculates the velocity Vel _DRi from the output signal SG _ACC of the acceleration sensor 4, the offset OF _ACC and the scale factor SF _{ACC using} the following equations (49) to (50).
, The velocity information Vel _GPSi observed by the GPS receiver 3, and the velocity Ve of the system model
Based on the observation model expressed by the following equation (51), the relation of l _DRi is expressed by the state equations expressed by the following equations (52) to (56) and the equations (57) to (60), respectively. The velocity Vel _{DRi of} the vehicle, the offset OF _ACC and the scale factor SF _ACC of the acceleration sensor 4 are calculated using the observation equation shown and the Kalman filter equations shown in the following equations (61) to (65).

Vel _DRi = Vel _DRi-1 + (SG _ACCi− OF _ACCi ) × SF _ACCi × ΔT (49) OF _ACCi = OF _ACCi-1 + δOF _ACCi (50)

Vel _GPSi = Vel _RRi + δVel _GPSi (51)

[0121]

(Equation 5)

[0122] _{y i = H · x i +} v i ··· (57) y i = Vel GPSavei ··· (58) H = [1 0 0] ··· (59) v i = δVel GPSavei ··· (60)

[0123] _{x i | i = x i |} i-1 + K i {y i - (Hx i | i-1 + v i)} ··· (61) x i + 1 | i = F i x i | i + G _i ω _i (62) K _i = Σ _{i | i-1} H ^T [H _{i | i-1} H ^T + σ _vi ² ] ^-1 (63) Σ _{i | i} = Σ _{i | i-1} −K _i H Σ _{i | i−1} (64) Σ _{i + 1 | i} = F _i Σ _{i | i} F _i ^T + G _i σ _wi ² G _i ^T (65)

In the above equations (49) to (50) and (51), i is a discrete time, Δt is a predetermined time, S
G _ACCi , OF _ACCi and SF _ACCi are the output signal and offset and scale factor of the acceleration sensor 4 at discrete time i, Vel _DRi is the speed calculated from the output signal of the acceleration sensor 4, and Vel _GPSi is the speed observed by the GPS receiver 3. , δVel _GPSi the error contained in the Vel _GPSi, δO
F _ACCi is an error included in OF _ACCi .

In the above equations (52) to (56) and equations (57) to (60), x _i , ω _i , F _i ,
G _i, y _i, v _i is the state Betokuru at discrete time i, the system error, the state transition matrix, drive matrix, observations,
H is the observation noise, and H is the observation matrix. Furthermore, equation (6)
1) to (65), 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, and K _i is the Kalman gain , Σ _{i | i} is the estimated amount of the covariance matrix of the state vector at discrete time i, Σ _{i + 1 | i} is the predicted amount of the covariance matrix of discrete time i + 1 at discrete time i, and σ _wi ² is the variance of the system error The matrix, σ _vi ^2, is the variance matrix of the observation error. Since each element of the matrix can be obtained by calculating a determinant, the description thereof is omitted here.

In FIG. 9, the processing is step ST907.
When the process proceeds to, it is determined whether or not the reception of the observation information from the GPS receiver 3 has been completed with reference to the GPS observation information reception flag. If the GPS observation information reception flag is set, it is determined that the reception of the observation information from GPS receiver 3 has been completed, and the process proceeds to step ST908. If the flag has been cleared, the reception is not completed and the process proceeds to step ST912. move on. In step ST908, it is determined whether or not the GPS receiver 3 performs two-dimensional or three-dimensional positioning. If two-dimensional or three-dimensional positioning has been performed, the process proceeds to step ST909; if not, the process proceeds to step ST9.
Proceed to 12.

In step ST909, the above equation (49) is used.
Calculate the estimated amount x _{i | i} of the state vector according to Equation (65). In step ST910 setting, the correction amount of the offset component X ₁₂ is a matrix element of the state vector through Kalman filter, namely the standard deviation of the values obtained by subtracting the prediction value from the estimated value, this as offset error is a system error ω I do. Next, step ST91
At 1, a standard deviation of difference in a matrix element velocity factor x ₁₁ and the GPS velocity Vel _GPSI of the state vector, after setting it as an observation error, the process proceeds to step ST 912.

In step ST912, the above equation (49) is used.
To the predicted amount x _{i + 1 | i of the} state vector according to equation (65).
Is calculated. Next, in step ST913, it is determined again whether or not the reception of the observation information from the GPS receiver 3 has been completed with reference to the GPS observation information reception flag as in step ST907. If the GPS observation information reception flag has been set, it is determined that the reception of the observation information from GPS receiver 3 has been completed, and the process proceeds to step ST914. If the flag has been cleared, the process proceeds to step ST919. Step ST
At 914, the GPS observation information reception flag is cleared for the next processing.

Next, in step ST915, similarly to step ST908, it is determined whether or not the GPS receiver 3 performs two-dimensional or three-dimensional positioning. If two-dimensional or three-dimensional positioning has been performed, the process proceeds to step ST916; if not, the process proceeds to step ST920. In step ST916, the estimated amount 誤差_{i | i} of the error covariance, in step ST917, the predicted amount 誤差_{i + 1 | i} of the error covariance, and in step ST918, the Kalman gain K _i , Calculate according to (65). In step ST919, velocity factor x ₁₁ is a matrix element of the estimated amount of the state vector, the offset components x _12, based on the scale factor components x _13, the estimated velocity Vel _DRi,
Offset OF _ACCi and scale factor SF _ACCI
After the update, the process proceeds to step ST920.

As described above, according to the fourth embodiment,
When the fluctuation amount of the output signal of the acceleration sensor 4 is equal to or less than a predetermined value, an average value of the output signals of the acceleration sensor 4 is used as an offset, and a GPS speed is used as an observed value according to the Kalman filter equation. And the scale factor is estimated, and when the GPS receiver 3 is in the non-positioning state, the value obtained by subtracting the offset from the output signal of the acceleration sensor 4 is multiplied by the scale factor of the acceleration sensor 4 according to the Kalman filter equation to obtain the acceleration of the vehicle per unit time. Is calculated, and the acceleration is integrated to estimate the speed. Therefore, even if the error between the GPS speed and the offset of the acceleration sensor 4 fluctuates, the offset of the acceleration sensor 4 and the correction of the scale factor are quickly corrected after the start of traveling. And optimally correct the speed of the vehicle after starting driving. In not only be estimated, does not use a distance sensor attached to the vehicle, effects such readily removable the locator device to the vehicle is obtained.

Embodiment 5 FIG. Next, a fifth embodiment of the present invention will be described. FIG. 10 is a block diagram showing a configuration of a locator device according to Embodiment 5 of the present invention.
In the figure, reference numeral 1 denotes a distance sensor equivalent to that of the first embodiment shown in FIG. 1 with the same reference numerals, and outputs a pulse signal according to the traveling distance of the vehicle. Reference numeral 2 denotes a relative direction sensor which outputs a voltage corresponding to the yaw rate of the vehicle. In the fifth embodiment, a gyro is used.
Reference numeral 3 denotes a GPS receiver as an absolute position observing means, which corresponds to that of the first embodiment shown in FIG. 1 with the same reference numerals, has a GPS antenna for receiving radio waves from the satellite, and The current position of the vehicle is observed and output by the radio wave.

Numeral 511a denotes a distance / speed calculating means for calculating the speed and the moving distance of the vehicle from the output signal of the distance sensor 1, which is shown in FIG. 1 or FIG. This corresponds to the speed calculation means in mode 3. Reference numeral 514 denotes an offset detecting means for detecting an offset of the relative azimuth sensor 2 from an output signal of the relative azimuth sensor 2, and 515 denotes an azimuth change calculating means for calculating a change in the traveling azimuth of the vehicle from the output signal of the relative azimuth sensor 2 is there. 516 is a position / azimuth estimating means for estimating the current position and the traveling direction of the vehicle based on the observation information from the GPS receiver 3, the output signal of the distance / speed calculating means 511a, and the output signal of the azimuth change calculating means 515. . Reference numeral 5 denotes a signal processor including a computer for calculating the traveling direction and the current position of the vehicle in accordance with a control program stored in a memory in advance. These signal / speed calculating means 511a, offset detecting means 514, azimuth change calculating means 515 and position / azimuth estimating means 516
This is different from the first to fourth embodiments shown in FIGS. 1, 6, and 8 with the same reference numerals.

Next, the operation will be described. Here, the operation of each means of the signal processor 5 will be described with reference to the drawings. FIG. 11 is a flowchart of a main routine showing processing contents of the signal processor 5, and FIG.
6 is a flowchart showing the contents of processing 6; In addition, interrupt processing 1 performed at every predetermined time Δt shown in FIG. 3, interrupt processing 2 performed when a signal is output from the distance sensor 1 shown in FIG. 4, and GPS reception shown in FIG. There is an interrupt process 3 for receiving observation information output from the device 3 for each observation cycle,
Since they are the same as those described in the first embodiment, description thereof will be omitted. FIG. 13 is an explanatory diagram showing an example of estimating the current position of the vehicle by the position / azimuth estimating means 516.

The main routine shown in FIG. 11 manages the operation of all means of the signal processor 5. First, all the processes are initialized in step ST1101. Next, in step ST1102, 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 it is the processing start timing, and the process proceeds to step ST1103. In step ST1103, the flag is cleared for the next processing. Next, in step ST1104, the distance / speed calculating means 511a
Is calculated from the output signal count value ΔN _{i of} the distance sensor 1 counted in the interrupt processing 2 at every predetermined time Δt,
_i and the speed Vel _DRi are calculated by the following equation (66) and the above-described equation (26). When the calculation is completed, the output signal count value ΔN _{i is set} to 0 for the next processing.

Δd _i = Δd _i−1 + ΔN _i × SF _SPDi (66)

Next, in step ST1105, offset OF _GYRi is calculated by the offset detecting means 514. That is, when the speed Vel _DRi calculated in step ST1104 is 0 (vehicle is stopped), the output signal S of the relative azimuth sensor 2 for a predetermined time or more when the vehicle is stopped is stopped.
The average of G is referred to as an offset OF _GYRi . Next, the process proceeds to step ST1106, in which the azimuth change calculation means 515 performs a process of calculating the azimuth change Δθ _i . That is, the output signal S of the relative azimuth sensor 2 measured every predetermined time Δt
From G _GYRi , a traveling direction change Δθ _i is calculated using the following equation (67). Note that OF _GYRi in this equation (67)
And SF _GYRi are an offset and a scale factor for converting an output signal into an angular velocity.

Δθ _i = Δθ _i-1 + (SG _GYRi −OF _GYRi ) × SF _GYRi × Δt (67)

Next, in step ST1107, GP
It is determined whether or not the reception of the observation information from the S receiver 3 has been completed.
The determination is made with reference to the PS observation information reception flag. This GP
If the S observation information reception flag is set, it is determined that the reception of the observation information from the GPS receiver 3 has been completed, and the process proceeds to step ST1108.
It returns to T1102. In step ST1108, the GPS observation information reception flag is cleared for the next processing. Next, in step ST1109, a position / direction estimation process is performed by the position / direction estimation unit 516. That is,
The current position (λ _i , φ _i ) and the traveling direction θ _i of the vehicle are estimated using a calculation formula (Kalman filter) described below. Next, in step ST1110, the current position (λ _i , φ _i ), traveling direction θ _i , and speed Vel _{Dri of the} vehicle
Output. Next, the process proceeds to step ST1111, and G
Moving distance Δd while receiving observation information from PS receiver 3
_i and the heading change Δθ _i are cleared to 0 and step ST
Return to 1102.

The position / direction estimation means 516 shown in FIG.
The Kalman filter according to the fifth embodiment calculates the current position (λ _i , φ _i ) and the traveling direction θ _i from the moving distance Δd _i and the traveling direction change Δθ _i of the vehicle, using the following equations (68) to ( 73) a system model calculated in
The following equation (74) shows the relationship between the GPS position (λ _Gi , φ _Gi ) and the current position (λ _i , φ _i ) of the vehicle in the system model.
To (75) based on the observation model.

Λ_i= Λ_i-1+ Δd_i× sin ｛θ_i-1+ Δθ_i｝ × SF_{d → λ}+ Δλ_i  ... (68) φ_i= Φ_i-1+ Δd_i× cos ｛θ_i-1+ Δθ_i｝ × SF_{d → φ}+ Δφ_i  ... (69) δλ_i  = ｛ΔΔd_i× sin θ_i+ ΔΔθ_i× Δd_i× cos θ_i｝ × SF_{d → λ}  ... (70) δφ_i  = ｛ΔΔd_i× cos θ_i−δΔθ_i× Δd_i× sin θ_i｝ × SF_{d → φ}  ... (71) sin ｛θ_i-1+ Δθ_i｝ = Cosθ_i-1・ SinΔθ_i+ Sin θ_i-1・ CosΔθ_i  ... (72) cos ｛θ_i-1+ Δθ_i｝ = Cosθ_i-1・ CosΔθ_i−sin θ_i-1・ SinΔθ_i  ... (73)

Λ _Gi = λ _i + δλ _Gi (74) φ _Gi = φ _i + δφ _Gi (75)

In the above equations (68) to (73) and equations (74) to (75), i is a discrete time,
λ _i-1 and φ _i-1 are the longitude and latitude of the current position of the vehicle at the discrete time i-1, and θ _i-1 is the traveling direction of the vehicle at the discrete time i-1. SF _{d → λ} and SF _{d → φ} indicate the unit of movement distance [m] as the amount of movement in the longitude or latitude direction [”]
Is the coefficient to be converted to Furthermore, [delta] [lambda] _i and .delta..phi _i in longitude error and latitude errors in the current position of the vehicle, δΔθ _i and Derutaderutad _i
Is the error of Δθ _i and Δd _i. Also, δλ _Gi and δφ _Gi
Is the error between the GPS positions λ _Gi and φ _Gi .

Then, the state equations represented by the following equations (76) to (80), the observation equations represented by the following equations (81) to (84), and the following equations (85) to (89) ) Is used to calculate the current position (λ _i , φ _i ) and traveling direction θ _i of the vehicle.

[0144]

(Equation 6)

[0145]

(Equation 7)

[0146] _{X i | i = X i |} i-1 + K i {Y i - (HX i | i-1 + v i)} ··· (85) X i + 1 | i = F i X i | i _{+ G i w i ··· (86} ) K i = Σ i | i-1 H T [HΣ i | i-1 H T + Σ vi] -1 ··· (87) Σ i | i = Σ i | i ₋₁ −K _i H Σ _{i | i−1} (88) Σ _{i + 1 | i} = F _i Σ _{i | i} F _i ^T + G _{i wi wi} G _i ^T (89)

In the above equations (76) to (80) and (81) to (84), x _i , F _i , G _i ,
w _i , y _i , and v _i are the state vector, the state transition matrix, the driving matrix, the system noise, the observed value,
It is observation noise, and H is an observation matrix. In Equations (85) to (89), 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 estimated amount of the covariance matrix of the state vector at discrete time i, Σ _{i + 1 | i} is the predicted amount of the covariance matrix of discrete time i + 1 at discrete time i, Σ _vi is The covariance matrix of the observation error, Σ _wi is the covariance matrix of the system error. Here, since each element of the matrix can be obtained by calculating the determinant, its description is omitted here.

Next, the processing contents of the position / azimuth estimating means 516 will be described in more detail with reference to the flowchart of FIG. First, in step ST1201, it is determined whether or not the speed Vel _DRi calculated by the distance / speed calculating means 511a is larger than 0. As a result, if it is 0, it is determined that the vehicle is stopped, and the position / azimuth estimation processing ends, and if it is larger than 0, the process proceeds to step ST1202. In step ST1202, it is determined whether or not the GPS receiver 3 has moved a predetermined distance or more in the two-dimensional or three-dimensional positioning state and since the current position and the traveling direction of the vehicle have been estimated by the Kalman filter last time. As a result of the determination, if the GPS receiver 3 moves in a two-dimensional or three-dimensional positioning state and moves a predetermined distance or more after estimating the current position and the traveling direction by the Kalman filter last time, the process proceeds to step ST1203.
If the PS receiver 3 is in the non-positioning state or has not moved a predetermined distance or more since the current position and the heading were calculated by the Kalman filter last time, step ST120 is performed.
Go to 5.

In step ST1203, the difference between the GPS position (λ _Gi , φ _Gi ) at a predetermined time and the current position (λ _i , φ _i ) by the Kalman filter, that is, the standard deviation of each of the latitude difference and the longitude difference, is calculated. The standard deviation is used as the observation error v _i, and the covariance matrix Σ _vi of the observation error is calculated. In step ST1204, the difference of the distance [Delta] d _i determined from the integral value and the distance sensor 1 of the GPS speed, with the distance error component Derutaderutad _i is a matrix element of the system error, in multiple locations separated by a predetermined distance of the direction difference between the direction of movement of the locus of the current position of the GPS position and the vehicle, after an azimuth error element Derutaderutashita _i is a matrix element of the system error, the flow proceeds to step ST1206. On the other hand, in step ST1205, the Kalman gain K
_i is cleared to 0, and the process proceeds to step ST1206.

In step ST1206, the estimated amount x _{i | i} of the state vector is calculated in accordance with the above-mentioned equations (68) to (89), using the GPS position as the observed value y _i .
Adjust all matrix elements of the Kalman gain K _i As is a value proportional to the offset variation of the relative direction sensor 2 is all matrix elements of the Kalman gain K _i becomes smaller, the estimated amount X _i of the state vector _| Get _i I do. Next, step ST120
In step 7, the predicted amount X _{i + 1 | i} of the state vector is calculated by the aforementioned equations (85) to (89). Next, step S
In T1208, similar to step ST1202,
The GPS receiver 3 is in a two-dimensional or three-dimensional positioning state, and the current position (λ _i , φ _i ) by the previous Kalman filter
And the traveling azimuth θ _i are calculated, and then it is determined whether or not the vehicle has moved a predetermined distance or more. At the GPS receiver 3 has a two-dimensional or three-dimensional positioning, and the current position by the previous Kalman filter (λ _i, φ _i) and if you move traveling direction theta _i the calculated predetermined distance or more from the step ST1209 If not, the position / azimuth estimation processing ends.

In step ST1209, the estimated amount of error covariance Σ _{i | i} , in step ST1210, the estimated amount of error covariance Σ _{i + 1 | i} , and in step ST1211, the Kalman gain K _i is calculated by the above equation (85). ) To Expression (89)
, And the processing of position / azimuth estimation is terminated.

Next, the operation of estimating the current position of the vehicle in the position / azimuth estimating means 516 will be described with reference to FIG. FIG. 13 (a) shows that the GP is running while the vehicle is traveling straight.
This shows how the current position of the vehicle is estimated when the variance of the S position increases. In this case, it is assumed that both the azimuth error element and the distance error element which are the matrix elements of the system error are small. At this time, the position / azimuth estimating means 516 detects that the error of the GPS position, which is the observation error, has increased in the section where the variance of the GPS position has increased. Calculate a smaller gain. As a result, the current position of the vehicle is estimated to be closer to the GPS position in a section where the variance of the GPS position is small.
In the section where the dispersion of the S position is large, the current position of the vehicle is GPS
Estimate by keeping the amount approaching the position small. Thus, the trajectory of the current position estimated by the Kalman filter is represented by
The result is indicated by a dotted line in FIG.

FIG. 13B shows how the current position of the vehicle is estimated when the offset of the relative direction sensor 2 drifts while the vehicle is traveling straight. In this case, it is assumed that the dispersion of the GPS positions is small. At this time, the position / direction estimation means 5
16 detects that the variance of the GPS position is small, but the traveling azimuth change per unit time is biased and an azimuth error is generated, so that the Kalman gain is calculated to be large.
As a result, the current position of the vehicle is estimated to approach the GPS position without being affected by the drift generated in the relative direction sensor 2. Thus, the trajectory of the current position estimated by the Kalman filter is indicated by a dotted line in FIG. When the general autonomous navigation without using the Kalman filter is used, the current position of the vehicle is changed as shown by a dashed line in FIG. Turn gradually in a biased direction.

FIG. 13C shows how the current position of the vehicle is estimated when the vehicle turns right when the scale factor of the relative direction sensor 2 is smaller than the true value. In this case, it is assumed that the dispersion of the GPS positions is small. At this time, the position / azimuth estimating unit 516 detects that the azimuth error has occurred after the right turn while the variance of the GPS position is small, so that the Kalman gain is calculated to be larger after the right turn. As a result, since the turning angle at the time of a right turn becomes small, an error occurs in the estimated heading. However, after detecting the azimuth error after the right turn, the current position of the vehicle is estimated to be closer to the GPS position. Thus, the trajectory of the current position estimated by the Kalman filter is indicated by a dotted line in FIG. In the case of general autonomous navigation not using a Kalman filter, the current position of the vehicle goes straight in the wrong direction while the turning angle is insufficient, as shown by the dashed line in FIG.

In the above description, the locator device shown in the first embodiment is provided with the relative azimuth sensor 2, the azimuth change calculating means 515, and the position / azimuth estimating means 516 added. It is also possible to realize them by adding them to the locator device shown in Embodiment 3 or Embodiment 4, and in that case, the same effect is exerted.

As described above, according to the fifth embodiment,
Along with calculating the change in the heading of the vehicle per unit time from the output signal of the relative heading sensor 2, the Kalman filter equation is used to estimate the current position and the heading of the vehicle, which are the matrix elements of the state vector, using the GPS position as the observed value, GP
Using the amount of change in the difference between the current position by the S receiver 3 and the position / azimuth estimating means 516 as an observation error, the traveling direction by the GPS receiver 3 position / azimuth estimating means 516 for each of a plurality of points separated by a predetermined distance or more. Is set as the azimuth error which is a matrix element of the system error, and the integrated value of the speed obtained by the distance / speed calculating means 511a and the GPS
The difference between the speed and the integral value is used as a distance error that is a matrix element of the system error, and the prediction and estimation of the error covariance are performed. When the GPS receiver 3 is in the non-positioning state, according to the Kalman filter equation, Distance / speed calculation means 5
Since the current position and the heading of the vehicle are estimated from the travel distance per unit time obtained from the speed according to 11a and the change in the heading per unit time calculated by the heading change calculation means 515, the GPS position and the autonomous navigation Even if the error of the measurement information from the sensor for fluctuates, the current position and the heading of the vehicle can be optimally estimated, and the reliability is improved.
Even if the GPS receiver is not in the positioning state, the current position and heading of the vehicle can be estimated from the measurement information of the autonomous navigation sensor, so that the current position and heading of the vehicle can be provided stably even on tunnels and elevated roads. And the like.

Embodiment 6 FIG. Next, a sixth embodiment of the present invention will be described. FIG. 14 is a block diagram showing a configuration of a locator device according to Embodiment 6 of the present invention.
In the figure, 1 is a distance sensor, 2 is a relative direction sensor, 3
Are GPS receivers as absolute position observing means, which are the same as those in the fifth embodiment shown by the same reference numerals in FIG. Note that the GPS receiver 3 observes and outputs the traveling direction of the vehicle as well as the current position of the vehicle. 511a is a distance / speed calculating means,
Numeral 514 denotes an offset detecting means, which are also equivalent to those shown with the same reference numerals in FIG.

Reference numeral 515 denotes azimuth change calculating means for calculating a change in the traveling azimuth of the vehicle from the output signal of the relative azimuth sensor 2, which also corrects a scale factor for converting the output signal of the relative azimuth sensor 2 into an angular velocity. 10 is different from the one shown in FIG. 516 is G
Observation information from PS receiver 3, distance / speed calculation means 51
1a and a position / azimuth estimating means for estimating a current position and a traveling azimuth of a vehicle based on an output signal of the azimuth change calculating means 515. It differs from the signal shown in FIG. 10 with the same reference numeral in that a signal for correcting a scale factor for converting the signal into an angular velocity is output. Reference numeral 5 comprises a distance / speed calculating means 511a, an offset detecting means 514, an azimuth change calculating means 515, and a position / azimuth estimating means 516. It is a signal processor that includes a computer that calculates the position.

Next, the operation will be described. Hereinafter, the operation of each unit of the signal processor 5 will be described with reference to the drawings. The main routine of the processing in the signal processor 5 is the same as that in the fifth embodiment, and only the azimuth change calculation process in step ST1106 in the flowchart of the main routine shown in FIG. Is different. Therefore, the description other than the azimuth change calculation processing is omitted here. In addition, in addition, the interrupt processing 1 performed at every predetermined time Δt shown in FIG. 3, the interrupt processing 2 performed when a signal is output from the distance sensor 1 shown in FIG. 4, and the interrupt processing 2 shown in FIG. There is an interrupt process 3 for receiving observation information output from the GPS receiver 3 for each observation period, but these are the same as those described in the first embodiment, and therefore description thereof will be omitted.

FIG. 15 shows the output of the relative direction sensor 2.
Calculates the change in the heading of the vehicle from the signal and
A scale converter that converts the output signal of the azimuth sensor 2 into angular velocity
The azimuth change calculator 515 corrects the factor
It is a flowchart which shows the process content of an azimuth change calculation process. this
When the azimuth change calculation process is started, first, in step ST1
At 501, the relative azimuth cell measured every predetermined time Δt
Output signal SG of sensor 2 _GYRiFrom azimuth change Δθ_iBefore
It is calculated by the above equation (67). Next, step ST1502
In the above, the traveling azimuth change Δθ every predetermined time Δt_iTo
Integrate and accumulate azimuth Δθ_iTo update. Hereafter, steps
From ST1503 to ST1515, the following
The relative orientation sensor 2 is
Direction ΣΔθ_iAnd position / direction estimation means 51
Heading direction θ estimated in 6_iAnd the relative orientation
Sa2 offset OF_GYRiAnd scale factor SF_GYRi
Is corrected.

The Kalman filter in the sixth embodiment calculates the traveling azimuth change Δθ _i from the output signal SG _GYRi of the relative azimuth _sensor 2, the offset OF _GYRi , and the scale factor SF _GYRi , and calculates the following equation (90). system model and observation model representing the relationship between the integrated heading Shigumaderutashita _i by heading theta _i and the system model of the vehicle estimated by the position and direction estimating unit 516 by the following equation (93) for integrating with the equation (92) It is based on.

ΣΔθ _i = ΣΔθ _i−1 + (SG _GYRi −OF _GYRi ) × SF _GYRi × ΔT + δOF _GYRi × SF _GYRi × ΔT + (SG _GYRi -OF _GYRi ) × δSF _GYRi × ΔT ... (90) O _GYRi = OF _GYRi-1 + δOF _GYRi (91) SF _GYRi = SF _GYRi-1 + δSF _GYRi (92)

Θ _i = ΣΔθ _i + δθ _i (93)

In the equations (90) to (93), i is a discrete time, Δt is a predetermined time, and S
G _GYRi , OF _GYRi , and SF _GYRi are the output signal and offset and scale factor of the relative direction sensor 2 at the discrete time i, δOF _GYRi and δSF _GYRi are the offset error and scale factor error, and δθ _i is the estimated traveling direction θ _i.
Is the error of

The state equations shown in the following equations (94) to (98), the observation equations shown in the following equations (99) to (102), and the equations (103) to (10)
The offset OF _GYRi and the scale factor SF _GYRi of the relative direction sensor 2 are calculated by the Kalman filter equation of 7).

[0166]

(Equation 8)

[0167] _{y i = H · x i +} v i ··· (99) y i = θ i ··· (100) v i = δθ i ··· (101) H = [1 0 0] ··· (102)

X _{i | i} = x _{i | i-1} + K _i {y _i − (Hxi _{| i-1} + v)} (103) x _{i + 1 | i} = _Fi x _{i | i} + G _i w ··· (104) K _i = ^-1 _{i | i−1} H ^T [HΣ _{i | i−1} H ^T + σ _vi ² ] ⁻¹ ··· (105) Σ _{i | i} = Σ _{i | i− 1−} K _i H Σ _{i | i−1} (106) Σ _{i + 1 | i} = F _i Σ _{i | i} F _i ^T + G _{i ｗ wi} G _i ^T (107)

In the above equations (94) to (98) and equations (99) to (102), x _i , F _i ,
G _i, w _i, y _i, v _i is the state Betokuru at discrete time i, state transition matrix, drive matrix, a system noise, the observed signal and measurement noise, H is an observation matrix. In the above equations (103) to (107), 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, and K _i is the Kalman gain, Σ _{i | i} is the estimated amount of the covariance matrix at discrete time i, Σ _{i + 1 | i} is the predicted amount of the covariance matrix at discrete time i + 1 at discrete time i, Σ _wi is the system error w _i covariance variance, and sigma _vi ² is the variance of the observation error v _i. Here, since each element of the matrix can be obtained by calculating the determinant, its description is omitted here.

In FIG. 15, the processing is carried out at step ST15.
In step 03, it is determined whether the reception of the observation information from the GPS receiver 3 has been completed with reference to the GPS observation information reception flag. If the GPS observation information reception flag is set, it is determined that the reception of the observation information from GPS receiver 3 has been completed, and the process proceeds to step ST1504. If the flag has been cleared, this azimuth change calculation process ends. Step ST1
In 504, the speed Vel _DRi obtained from the distance sensor 1 is 0.
It is determined whether it is greater than. As a result of the determination, the speed Vel
If _DRi is greater than 0, the process proceeds to Step ST1506 as the vehicle is traveling, if the speed Vel _DRi is 0, the offset component X ₁₂ is a matrix element of the state vector branches to step ST1505 an integrated heading Shigumaderutashita _i After initialization, the azimuth change calculation process ends. As the initial values, the traveling azimuth θ _i estimated in the position / azimuth estimation processing 516 and the offset OF _GYRi of the relative azimuth sensor 2 detected by the offset detecting means 514 are used.

At step ST1506, the GPS receiver 3
It is determined whether or not is performing two-dimensional or three-dimensional positioning. As a result, if the GPS receiver 3 performs two-dimensional or three-dimensional positioning, the process proceeds to step ST1507, and if non-positioning, the azimuth change calculation process ends. Step ST
At 1507, it is determined whether the time during which the two-dimensional or three-dimensional positioning by the GPS receiver 3 is continued is equal to or longer than a predetermined time. As a result of the determination, if the positioning continuation time is equal to or longer than the predetermined time, the process proceeds to step ST1508, and if it is shorter than the predetermined time, steps ST1508 to ST1510.
Is skipped and the process directly proceeds to step ST1511. In step ST1508, an error .delta..theta _i of heading theta _i estimated by the position and direction estimating unit 516 and the observation error v _i, as a dispersion sigma _vi ² of dispersed observation error of the observed error v _i at a given time Delta] t, handle Proceed to step ST1509.

In step ST1509, the offset OF _GYRi of the relative orientation sensor 2 detected by the offset detection means 514 is added with the value obtained by multiplying the drift which is the offset gradient by the elapsed time after the offset detection, and then the matrix element of the state vector is added. a value obtained by subtracting from the offset element X ₁₂ is, the offset error component δOF _GYRi a matrix element of the system error w _i. If the traveling direction of the vehicle changes by a predetermined value or more within a predetermined time, the traveling direction θ _{i of the} vehicle and the integrated direction element X
Obtains the difference between the ₁₁ orientation, further divided by the angle obtained by integrating separately in step which is not shown the travel direction change of the relative direction sensor 2, a scale factor element X ₁₃ is a matrix element of the state vector and predetermined The value obtained by multiplying the coefficients together is referred to as a scale factor error ΔSF _GYRi . And computing the covariance sigma _wi system error seeking dispersion offset error δOF _GYRi and scale factor error δSF _GYRi at a given time.

Then, the flow advances to step ST1510 to calculate an estimated amount x _{i | i} of the state vector. Next, step ST
In step 1511, the predicted amount x _{i + 1 | i} of the state vector is determined. In step ST 1512, the estimated amount of error covariance Σ _{i | i}
The predicted amount sigma _{i + 1} of the error covariance in step ST1513 _| a _i, calculating respectively the Kalman gain K _i at step ST1514. Note that these calculations are performed using the above-described equations (90) to (107). Next, in step ST1515, the offset OF _GYRi and the scale factor SF _GYRi of the relative azimuth sensor 2 are updated based on the estimated amount x _{i | i} of the state vector, and then the azimuth change calculation processing ends.

As described above, according to the sixth embodiment,
When the speed of the vehicle obtained by the distance / speed calculation means 511a is 0, the average of the output signals of the relative direction sensor 2 is used as an offset, and the position / speed is calculated according to the Kalman filter equation.
The traveling azimuth estimated by the azimuth estimating means 516 is used as an observation value,
The offset and scale factor of the relative azimuth sensor 2, which are the matrix elements of the state vector, are estimated, and the difference between the value obtained by predicting the offset from the offset fluctuation amount for each stop and the current offset is referred to as the offset error which is the matrix element of the system error. And a scale factor error, which is a matrix element of a system error, is set from the difference between the turning angles of the vehicle obtained from the respective azimuth information of the position / azimuth estimating means 516 and the azimuth change calculating means 515 when turning the vehicle. , Even if there is an error in the estimated heading of the vehicle and the offset and scale factor of the relative heading sensor,
This makes it possible to optimally correct the offset and the scale factor of the relative direction sensor 2, so that even when the GPS receiver 3 is in the non-positioning state, or when the vehicle is running slowly, an error in the observation information of the GPS receiver 3 increases. However, since the offset and the scale factor of the relative azimuth sensor 2 can always be corrected, an effect such as a change in the traveling azimuth from the relative azimuth sensor 2 can be obtained with high accuracy can be obtained.

Embodiment 7 FIG. Next, a seventh embodiment of the present invention will be described. In the locator device according to the seventh embodiment, when the offset of the relative azimuth sensor 2 in the sixth embodiment exceeds a predetermined value, the scale factor is not corrected, and the configuration thereof is shown in FIG. Since the configuration is the same as that of the sixth embodiment shown in the block diagram, illustration and description are omitted.

Next, the operation will be described. The operation thereof is basically the same as that of the sixth embodiment. Only the operation of step ST1515 in the flowchart of FIG. 15 showing the processing contents of the azimuth change calculation processing by the azimuth change calculation means 515 is the same. Since this is different from the sixth embodiment, only the operation of step ST1515 will be described here, and the description of the other steps will be omitted.

In the seventh embodiment, if the variation of the offset of relative azimuth sensor 2 is equal to or more than a predetermined value in step ST1515 shown in the flowchart of FIG. 15, the Kalman filter sets the scale factor to a predetermined value. The Kalman gain is adjusted so as not to be corrected as described above, and the state vector is calculated using the above-described equations (90) to (10).
The scale factor is not updated even if the calculation is performed according to 7) or the same calculation as in the sixth embodiment is performed.

As described above, according to the seventh embodiment,
When the variation of the offset of the relative azimuth sensor 2 which is the matrix element of the state vector in the Kalman filter becomes equal to or larger than a predetermined value, the correction of the scale factor of the relative azimuth sensor 2 which is the matrix element of the state vector is invalidated,
Since the correction of the scale factor of the relative azimuth sensor 2 is performed only when the variation of the offset of the relative azimuth sensor 2 is small, erroneous correction of the scale factor of the relative azimuth sensor 2 can be prevented. The effect is obtained that the change can be obtained with high accuracy.

Embodiment 8 FIG. Next, an eighth embodiment of the present invention will be described. The locator device according to the eighth embodiment uses the speed calculation means 511 of the first embodiment to detect that the locator device has been replaced by another vehicle and to provide information on the vehicle or the mounting of the locator device. Has a function of initializing the information according to the first embodiment, and its configuration is the same as that of the first embodiment shown in the block diagram of FIG.

Next, the operation will be described. The operation is basically the same as that of the first embodiment, and is the main routine of the speed calculation means 511 shown in FIG.
Since only the operations of steps ST215 and ST217 in the flowchart shown in FIG. 7 are different from those of the first embodiment, only the operations of steps ST215 and ST217 will be described here, and the description of the other steps will be omitted. .

In the eighth embodiment, if the value of the scale factor element, which is the matrix element of the state vector calculated by the Kalman filter, is within the normal range in step ST215 shown in the flowchart of FIG. The latest calculated value is held for a predetermined time. When the ratio of the standard deviation of the calculated value of the scale factor element to the average value for a predetermined time is equal to or less than a predetermined value,
The scale factor of the distance sensor 1 is updated using the average value. Further, when the scale factor is updated by a predetermined ratio or more from the previous value, it is determined that the locator device has been replaced by another vehicle, and information relating to the vehicle or information relating to the attachment of the locator device is determined. initialize. Thereafter, in step ST217, a determination result as to whether or not the locator device has been mounted on another vehicle is output together with the speed of the vehicle.

As described above, according to the eighth embodiment,
When the scale factor of the distance sensor 1 changes by a predetermined ratio or more, it is determined that the locator device has been replaced by another vehicle, and a function for initializing information relating to the vehicle or information relating to mounting of the device is calculated by speed calculation. Since the means 511 is provided, even when the locator device is used after being mounted on a plurality of vehicles, it becomes possible to immediately correct the currently mounted vehicle, and the behavior information of the vehicle can be obtained. The effect of being able to set optimally is obtained.

Embodiment 9 FIG. Next, a ninth embodiment of the present invention will be described. FIG. 16 is a block diagram showing a configuration of a locator device according to Embodiment 9 of the present invention.
In the figure, 1 is a distance sensor, 3 is a GPS receiver as absolute position observing means, and these are the parts corresponding to those in the first embodiment shown in FIG. The GPS receiver 3 observes and outputs the current position and traveling direction of the vehicle.

Reference numeral 52 denotes storage means having a plurality of memory areas for storing scale factors of the distance sensor 1 which differ for each vehicle type, and reference numeral 53 denotes a storage means for storing the memory area in which the scale factor is stored. Storage destination specifying means provided with a switch for instructing the storage destination. Reference numeral 511 denotes speed calculation means for calculating a scale factor for converting the output signal of the distance sensor 1 into a distance. The scale of the distance sensor 1 is stored in the memory area of the storage means 52 designated by the storage destination designation means 53. It differs from that shown in FIG. 1 with the same reference numerals in that the factors are read and written. Reference numeral 5 denotes a signal processor including a computer for calculating the speed of the vehicle in accordance with a control program stored in a memory in advance. The signal processor 5 includes a storage unit 52, a storage destination designation unit 53, and a speed calculation unit 511. 1, 6, 8, 10, and 14.
Are different from those of the above-described embodiments, which are denoted by the same reference numerals.

Next, the operation will be described. Hereinafter, the operation of the signal processor 5 will be described with reference to the drawings. However, since the operation of the speed calculation means 511 in the signal processor 5 is the same as that in the first embodiment, the description is omitted here, and the operation of reading and writing the scale factor to the storage means 52 will be described. Note that FIG.
FIG. 4 is an explanatory diagram simply showing a memory map of No. 2; First,
The storage destination specifying unit 53 checks the setting state of the switch operated by the user, and instructs the storage unit 52 to the address of the memory area determined in advance according to the setting state. The storage means 52 stores, in the memory area at the address (addresses x to x + n-1 shown in FIG. 17) designated by the storage destination specifying means 53, the distance sensor 1 obtained by the speed calculation means 511 and different for each vehicle type. (SF value 1 to SF value n in FIG. 17).

As described above, according to the ninth embodiment,
While the scale factor of the distance sensor 1 that differs for each vehicle type is stored in the storage unit 52, the address of the memory area that is the storage destination of the scale factor of the distance sensor 1 specified by the user is read and stored by the storage destination specification unit 53. Since the scale factor of the distance sensor 1 is read and written to the memory area at the designated address of the means 52, if the scale factor of the distance sensor 1 is manually switched, the locator device may be replaced with another vehicle. There is no need to redo the correction for the vehicle every time, and immediately after replacing it with another vehicle,
The effect is obtained that the speed of the vehicle can be estimated with high accuracy.

Embodiment 10 FIG. Next, a tenth embodiment of the present invention will be described. FIG. 18 is a block diagram showing a configuration of a locator device according to Embodiment 10 of the present invention. In the figure, 1 is a distance sensor, 3 is a GPS receiver as absolute position observing means, and these are the parts corresponding to those in the first embodiment shown in FIG. The GPS receiver 3 observes and outputs the current position and traveling direction of the vehicle.

Reference numeral 52 denotes a storage unit having a plurality of memory areas for storing a set of a scale factor of the distance sensor 1 and a memory access time which are different for each vehicle type.
54 searches the history of the scale factor and the memory access time stored in the storage means 52 based on the scale factor calculated by the speed calculation means 511 described later, and stores the address of the memory area as a search result. The scale factor is stored in the storage unit 52 and the speed calculation unit 511.
Is a storage destination search means for transmitting the information to the respective storage destinations. Reference numeral 511 denotes speed calculating means for calculating the speed of the vehicle and calculating a scale factor for converting the output signal of the distance sensor 1 into a distance. The embodiment shown in FIG. 16 with the same reference numerals in FIG. 16 in that reading and writing to the area is performed, the calculated scale factor is further sent to the storage destination searching unit 52, and the search result of the storage destination searching unit 54 is received. 9 is different. Reference numeral 5 denotes a signal processor including a computer for calculating the speed of the vehicle in accordance with a control program stored in a memory in advance. The signal processor 5 includes a storage unit 52, a storage destination search unit 54, and a speed calculation unit 511. In this respect, the present embodiment is different from those of the above-described embodiments shown in FIG. 1, FIG. 6, FIG. 8, FIG. 10, FIG. 14 and FIG.

Next, the operation will be described. Hereinafter, the operation of each unit of the signal processor 5 will be described with reference to the drawings. However, the speed calculation means 511 in the signal processor 5 is the same as that in the eighth embodiment, and the description is omitted here. FIG. 19 is an explanatory diagram simply showing a memory map of the storage means 52.

The speed calculating means 511 calculates the scale factor of the distance sensor 1, and determines whether or not the locator device has been transferred to another vehicle in the same manner as in the eighth embodiment. As a result, when it is determined that the vehicle has been transferred to another vehicle, the speed calculation unit 511 sends the calculated scale factor of the distance sensor 1 to the storage destination search unit 54. The storage destination search means 54 searches the scale factors stored in the respective memory areas of the storage means 52 for ones whose difference from the calculated value of the scale factor received from the speed calculation means 511 is equal to or smaller than a predetermined value. I do. When a corresponding one is found, the searched scale factor is passed to the speed calculating means 511, and the address of the memory area where the searched scale factor is stored is transmitted to the storage means 52.

The storage means 52 includes a storage destination search means 54
From the address (from address y shown in FIG. 19 to y + n-
In the memory area at address 1), the distance sensor 1
(SF value 1 to SF value n in FIG. 19)
Is updated with the current time. Thereafter, the speed calculation unit 511 calculates the speed of the vehicle using the scale factor received from the storage destination search unit 54, that is, the result corrected in the past.

[0192] The storage destination searching means 54 determines that the difference between the calculated value of the scale factor received from the speed calculating means 511 and the scale factor stored in the storing means 52 is equal to or smaller than a predetermined value. If the search is not possible from the memory area, it is determined that the locator device has been replaced by another unconfirmed vehicle, and the address of the empty memory area of the storage means 52 is transmitted to the storage means 52. The storage unit 52 stores the scale factor calculated by the speed calculation unit 511 in a free memory area to which the passed address is assigned, and stores the current time as a memory access time stored in combination with the scale factor. Remember.

Further, the difference between the calculated value of the scale factor received from the speed calculating means 511 and the scale factor stored in the storage means 52 is set to be equal to or smaller than a predetermined value.
If no search is possible from the memory area of the storage means 52 and there is no empty memory area in the storage means 52,
The storage destination search means 54 informs the storage means 52 of the address of the memory area storing the scale factor with the oldest memory access time. The storage memory 52 stores the scale factor calculated by the speed calculation means 511 in the memory area of the passed address, and also stores the current time as the memory access time stored in combination with the scale factor.

As described above, according to the tenth embodiment, when it is determined that the locator device has been replaced with another vehicle, the current calculated value is calculated from the scale factors stored in storage means 52. Is found and passed to the speed calculation means 511, and the address of the memory is passed to the storage means 52. At this time, the scale factor stored in the storage means 52 is compared with the current calculated value by a predetermined ratio. If not, it is determined that the locator device is mounted on the unconfirmed vehicle, and the storage unit 52 is instructed to read and write the scale factor in the empty memory area. In
Since the memory access time instructs the storage means 52 to overwrite the oldest memory area, even if the locator device is mounted on another vehicle, it is not necessary to perform the correction for the vehicle every time. There is an effect that the speed of the vehicle can be estimated with high accuracy immediately after the vehicle is remounted.

Embodiment 11 FIG. In the second embodiment,
In the speed calculation unit 511, the scale factor element, which is a matrix element of the estimated amount of the state vector, and the previous value of the scale factor are weighted and averaged using Expression (48) to update the scale factor. A calculation formula other than the above formula (48) may be used as long as the scale factor is updated by smoothing the scale factor element which is a matrix element of the state vector estimator.

Embodiment 12 FIG. In the third embodiment,
After the offset of the acceleration sensor 4 is updated in the speed calculating unit 511, until a predetermined time elapses,
A description has been given of a case in which the scale factor of the distance sensor 1 is corrected so that the speed calculated by the speed calculation unit 511 matches the speed estimated by the speed estimation unit 513 from the output signal of the acceleration sensor 4. If the error in the speed is small, the predetermined time during which the scale factor of the distance sensor 1 can be corrected may be variably set in accordance with the amount of change in the offset of the acceleration sensor 4.

Embodiment 13 FIG. In the fifth, sixth and tenth embodiments, the case where the distance / speed calculating means 511a or the speed calculating means 511 uses the distance sensor 1 to calculate the speed and the moving distance of the vehicle will be described. However, the acceleration sensor 4 may be used instead.

Embodiment 14 FIG. In Embodiment 8 described above,
In the speed calculation means 511, the case where it is determined that the locator device has been replaced by another vehicle when the scale factor of the distance sensor 1 has been updated by a predetermined ratio or more from the previous value has been described. Based on the history holding the calculated value of the scale factor element which is a matrix element of the state vector for a predetermined time, for example, when the ratio of the standard deviation to the average value for the predetermined time changes by a predetermined value or more, or in the history When the gradient accompanying the temporal change of the scale factor element becomes equal to or more than a predetermined value, it may be determined that the locator device has been replaced by another vehicle.

Embodiment 15 FIG. In the ninth embodiment,
The storage destination specifying unit 53 has been described as including a switch for the user to specify the storage location of the scale factor to the storage unit 52. However, the storage unit specifying unit 53 further includes a communication unit instead of the switch, so that the user can use the communication unit. The storage location of the scale factor may be specified via the storage device.

Embodiment 16 FIG. In the tenth embodiment, the storage destination search unit 54 determines that the difference between the scale factor of the distance sensor 1 calculated by the speed calculation unit 511 and the scale factor stored in the storage unit 52 is equal to or smaller than a predetermined value. Although the search from the 52 has been described, a search may be made for a search in which the ratio of the difference to the scale factor stored in the storage means 52 is equal to or less than a predetermined value.

[0201]

As described above, according to the present invention, there are provided absolute position observing means for observing at least the speed of a vehicle using a satellite, and a distance sensor for outputting a signal corresponding to the moving distance of the vehicle. The speed calculation means estimates the vehicle speed, and the Kalman filter is used to correct the scale factor of the distance sensor based on the speed observed by the absolute position observation means and to estimate the vehicle speed. Therefore, even if the error of the speed observed by the absolute position observation means fluctuates, the scale factor of the distance sensor can be optimally and promptly corrected after the vehicle starts running,
There is an effect that a locator device that can estimate the speed of the vehicle with high accuracy after the start of traveling is obtained.

According to the present invention, since the low-pass filter is provided after the Kalman filter in the speed calculating means, the scale factor of the distance sensor calculated by the Kalman filter is smoothed, and the high-frequency noise contained in the distance sensor is reduced. Since it is removed, the variation of the scale factor of the distance sensor can be reduced, and the speed of the vehicle can be stably estimated.

According to the present invention, a distance sensor for outputting a signal corresponding to the moving distance of the vehicle, an acceleration sensor for outputting a signal corresponding to the longitudinal acceleration of the vehicle, and an acceleration sensor based on the output signal of the acceleration sensor An offset detecting means for detecting an offset; and a speed estimating means for estimating a speed of the vehicle from an output signal of the acceleration sensor. Since it was configured to correct the scale factor of the distance sensor and calculate the speed of the vehicle,
When the offset error of the acceleration sensor is small, the scale factor of the distance sensor is corrected so that the speed of the vehicle measured from the distance sensor matches the speed of the vehicle estimated from the acceleration sensor, and the absolute position observation means is likely to be in the non-positioning state. Even if the vehicle travels in a place, the scale factor of the distance sensor can be corrected immediately after the traveling starts, and the speed of the vehicle can be estimated with high accuracy after the traveling starts.

According to the present invention, absolute position observing means for observing at least the speed of a vehicle using a satellite, an acceleration sensor for outputting a signal corresponding to the longitudinal acceleration of the vehicle, and an output signal of the acceleration sensor Offset detection means for detecting the offset of the acceleration sensor, the speed estimation means for estimating the speed of the vehicle, and using a Kalman filter, the offset and scale of the acceleration sensor based on the speed observed by the absolute position observation means Since the correction of the factor and the estimation of the speed of the vehicle are performed, even if the error between the speed observed by the absolute position observation means and the offset of the acceleration sensor fluctuates, the offset of the acceleration sensor and the correction of the scale factor are corrected. After the start of traveling, it is possible to correct the speed optimally and promptly, and It can be estimated with accuracy, further, does not use a distance sensor attached to the vehicle, there are effects such as the detachment of the locator device to the vehicle can be easily performed.

According to the present invention, a relative azimuth sensor for outputting a signal corresponding to a change in the heading of the vehicle, and a heading change calculating means for calculating a heading change for each unit time of the vehicle from the output signal of the relative heading sensor The position / azimuth estimating means estimates the current position and heading of the vehicle, and the Kalman filter estimates the current position and heading of the vehicle based on the position observed by the absolute position observing means. With the dedicated Kalman filter, the current position and traveling direction of the vehicle are simultaneously estimated from the position observed by the absolute position observation means and measurement information from the relative direction sensor, etc. Even if the error of the measurement information of the azimuth sensor fluctuates, the current position and the heading of the vehicle can be estimated optimally, and the reliability is improved. Even in the non-positioning state, the current position and traveling direction of the vehicle can be estimated from the measurement information of the relative direction sensor, etc., and the current position and traveling direction of the vehicle can be provided stably even on tunnels and elevated roads There are effects such as being able to do.

According to the present invention, the offset detecting means for detecting the offset of the relative azimuth sensor from the output signal of the relative azimuth sensor is provided. It is configured to calculate the change in the heading direction and to correct the offset and scale factor of the relative heading sensor based on the heading of the vehicle estimated by the position and heading estimating means using the Kalman filter. Even if there is an error between the traveling direction of the vehicle and the offset and scale factor of the relative direction sensor, it is possible to optimally correct the offset and scale factor of the relative direction sensor and that the absolute position observation means is in the non-positioning state. However, if the error in the observation information of the absolute position observation means becomes large, such as when driving slowly, Even, it is possible to constantly correct the offset and scale factor of the relative direction sensor, there are effects such as the moving direction changes from the relative direction sensor can be determined with high accuracy.

According to the present invention, the azimuth change calculating means is configured to monitor the fluctuation of the offset of the relative azimuth sensor and determine the validity of the scale factor correction. By correcting the scale factor of the relative direction sensor, it is possible to prevent erroneous correction of the scale factor of the relative direction sensor, and there is an effect that a change in traveling direction can be obtained with high accuracy from the relative direction sensor.

According to the present invention, by monitoring the correction of the scale factor of the distance sensor, it is determined whether or not the locator device has been replaced by another vehicle. It is possible to detect that the locator device has been replaced by another vehicle from the amount of change, and when the locator device is used after being replaced by a plurality of vehicles, the correction related to the currently mounted vehicle is performed. Can be performed immediately, and the behavior information of the vehicle can be set optimally.

According to the present invention, there are provided storage means having a plurality of memory areas for storing a scale factor of a distance sensor, and storage destination specifying means for allowing a user to specify an address of a memory area as a storage destination. , While storing the scale factor of the distance sensor different for each vehicle type in the storage means,
Since the storage location of the scale factor of the distance sensor is configured to be switched by the user using the storage destination specifying means,
By switching the scale factor of the distance sensor by this storage destination designating means, when the locator device is mounted on another vehicle, it is not necessary to redo the correction for the vehicle each time, and immediately after replacing the locator device on another vehicle. There is an effect that the speed of the vehicle can be estimated with high accuracy from the vehicle.

According to the present invention, the storage means having a plurality of memory areas for storing sets of the scale factor of the distance sensor and the memory access time, and the storage destination search means for searching the scale factor held in the storage means are provided. Provided,
While the scale factor of the distance sensor that differs for each vehicle type is stored in the storage unit, the storage location of the scale factor of the distance sensor is automatically switched according to the search result of the storage destination search unit. When the vehicle is mounted on a vehicle, it is not necessary to perform the correction for the vehicle every time, and the speed of the vehicle can be estimated with high accuracy immediately after the vehicle is mounted on another vehicle.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a configuration of a locator device according to Embodiment 1 of the present invention.

FIG. 2 is a flowchart showing processing contents of a main routine in the operation of the locator device according to Embodiment 1 of the present invention.

FIG. 3 is a flowchart showing processing contents of interrupt processing 1 of the operation of the locator device according to Embodiment 1 of the present invention;

FIG. 4 is a flowchart showing the contents of interrupt processing 2 of the operation of the locator device according to Embodiment 1 of the present invention;

FIG. 5 is a flowchart showing processing contents of interrupt processing 3 of the operation of the locator device according to Embodiment 1 of the present invention.

FIG. 6 is a block diagram showing a configuration of a locator device according to Embodiment 3 of the present invention.

FIG. 7 is a flowchart showing processing contents of a main routine in an operation of the locator device according to Embodiment 3 of the present invention.

FIG. 8 is a block diagram showing a configuration of a locator device according to Embodiment 4 of the present invention.

FIG. 9 is a flowchart showing processing contents of a main routine of an operation of the locator device according to Embodiment 4 of the present invention.

FIG. 10 is a block diagram showing a configuration of a locator device according to Embodiment 5 of the present invention.

FIG. 11 is a flowchart showing the processing content of a main routine in the operation of the locator device according to Embodiment 5 of the present invention.

FIG. 12 is a flowchart showing the contents of a position / azimuth estimation process in the operation of the locator device according to Embodiment 5 of the present invention.

FIG. 13 is an explanatory diagram showing an example of the operation of the locator device according to Embodiment 5 of the present invention, in which the current position of the vehicle is estimated by position / azimuth estimating means.

FIG. 14 is a block diagram showing a configuration of a locator device according to Embodiment 6 of the present invention.

FIG. 15 is a flowchart showing the contents of an azimuth change calculation process in the operation of the locator device according to Embodiment 6 of the present invention.

FIG. 16 is a block diagram showing a configuration of a locator device according to Embodiment 9 of the present invention.

FIG. 17 is an explanatory diagram simply showing a memory map of a storage unit of the locator device according to Embodiment 9 of the present invention.

FIG. 18 is a block diagram showing a configuration of a locator device according to Embodiment 10 of the present invention.

FIG. 19 is an explanatory diagram simply showing a memory map of storage means of a locator device according to Embodiment 10 of the present invention.

FIG. 20 is a block diagram illustrating a configuration example of a conventional locator device.

FIG. 21 is a flowchart showing the processing contents of the main routine of autonomous navigation in the operation of the locator device.

FIG. 22 is a flowchart showing the operation of the locator device for calculating the change in the heading direction and the moving distance.

FIG. 23 is a flowchart showing the relative trajectory calculation processing of the operation of the locator device.

FIG. 24 is a flowchart showing the contents of a process of calculating an absolute azimuth and an absolute position in the operation of the locator device.

FIG. 25 shows autonomous navigation and G of the operation of the locator device.
9 is a flowchart showing the processing contents of Kalman filter arithmetic processing for compounding PS navigation.

FIG. 26 is a block diagram illustrating another configuration example of a conventional locator device.

FIG. 27 is a flowchart showing the processing contents of the main routine of autonomous navigation in the operation of the locator device.

[Explanation of symbols]

1 distance sensor, 2 relative direction sensor, 3 GPS receiver (absolute position observation means), 4 acceleration sensor, 52 storage means, 53 storage destination designation means, 54 storage destination search means, 511 speed calculation means, 511a distance / speed calculation Means (speed calculating means), 512, 514 offset detecting means, 513 speed estimating means, 515 azimuth change calculating means, 516 position / azimuth estimating means.

 ──────────────────────────────────────────────────続 き Continuing on the front page (72) Inventor Tetsuo Kamikawa 2-3-2 Marunouchi, Chiyoda-ku, Tokyo Mitsui Electric Co., Ltd. (72) Inventor Yosuke Asai 2-3-2 Marunouchi, Chiyoda-ku, Tokyo Rishi Electric Co., Ltd. (72) Inventor Ichiro Nakabori 2-3-2 Marunouchi, Chiyoda-ku, Tokyo F-term (reference) 2F029 AA02 AB01 AB07 AB09 AC01 AC02 AC04 AD03 AD07 5J062 BB01 CC07

Claims (10)

[Claims] 1. An absolute position observing means for observing at least a speed of a vehicle using a satellite, a distance sensor for outputting a signal corresponding to a moving distance of the vehicle, and a scale factor for an output signal of the distance sensor. Based on a system model that calculates the speed of the vehicle by multiplying, based on an observation model that represents the relationship between the speed of the vehicle and the speed in the system model observed by the absolute position observation means,
The speed and scale factor in the system model are set in the matrix element of the state vector, and the Kalman filter that sets the speed observed by the absolute position observing means is used as the observed value. In addition to performing the calculation, the system model and the observation model use the average value of the output signal of the distance sensor and the average value of the velocity observed by the absolute position observation means at a predetermined time, and change the average value over time. A speed calculating means for multiplying the difference in gradient accompanying the average by the average value of the speeds observed by the absolute position observing means and setting the value as an observation error. 2. A speed calculation unit, comprising: a low-pass filter that removes high-frequency noise included in a scale factor, which is a matrix element of a state vector, at a stage subsequent to the Kalman filter; 2. The locator device according to claim 1, wherein the locator value is a correction value of a sensor. 3. An acceleration sensor for outputting a signal corresponding to a longitudinal acceleration of a vehicle, and an offset having an average value of the output signals as an offset when a variation amount of an output signal of the acceleration sensor is equal to or less than a predetermined value. Detecting means for calculating the acceleration per unit time of the vehicle by subtracting the offset from the output signal of the acceleration sensor, multiplying the output by the scale factor of the acceleration sensor, and integrating the acceleration to estimate the speed. Speed estimating means, a distance sensor for outputting a signal corresponding to the moving distance of the vehicle, and the distance sensor so that the speed calculated from the output signal of the distance sensor matches the speed estimated by the speed estimating means. A speed calculating means for calculating a speed from an output signal of the distance sensor by correcting the scale factor of the distance sensor. 4. An absolute position observing means for observing at least a speed of a vehicle using a satellite, an acceleration sensor for outputting a signal corresponding to a longitudinal acceleration of the vehicle, and a fluctuation of an output signal of the acceleration sensor. When the amount is equal to or less than a predetermined value, an offset detecting means for offsetting a value obtained by averaging the output signal; and after subtracting the offset from the output signal of the acceleration sensor, multiplying the offset signal by the scale factor of the acceleration sensor. Based on a system model that calculates acceleration per unit time, calculates the speed by integrating the acceleration, and an observation model that expresses the relationship between the speed observed by the absolute position observation means and the speed in the system model, The speed, scale factor and offset in the system model are set in the matrix element of the state vector, and the observed value is A locator device comprising: a speed estimation unit that estimates a speed, an offset, and a scale factor, which are matrix elements of a state vector, using a Kalman filter that sets a speed observed by the absolute position observation unit. 5. A relative direction sensor for outputting a signal corresponding to a change in the heading of the vehicle, a heading change calculating means for calculating a change in the heading of the vehicle per unit time from an output signal of the relative heading sensor, Position / azimuth estimating means for estimating a current position and a traveling azimuth of the vehicle, wherein the position / azimuth estimating means calculates a moving distance per unit time obtained from a speed by speed calculating means or speed estimating means; A system model for calculating the current position and heading of the vehicle from the heading change per unit time calculated by the change calculating means, and a relation between the current position of the vehicle observed by the absolute position observing means and the current position in the system model are shown. Based on the observation model obtained, the current position and the heading in the system model are set in the matrix element of the state vector, and the absolute position The Kalman filter that sets the current position of the vehicle observed by the means calculates the current position and the traveling direction of the vehicle, which are the matrix elements of the state vector, and calculates the absolute position at a plurality of points separated by a predetermined distance or more or every predetermined time. The variation amount of the difference between the current position by the position observing means and the current position by the position / azimuth estimating means is set as an observation error, and for each of a plurality of points separated by a predetermined distance or more, the traveling azimuth by the absolute position observing means and the position The average value of the difference between the traveling directions by the direction estimating means is set as the direction error which is a matrix element of the system error, and the integrated value of the speed by the speed calculating means or the speed estimating means and the absolute position observing means are used. 2. The method according to claim 1, wherein the difference between the integral values of the speed is set as a distance error which is a matrix element of the system error. Locator apparatus according to any one of claims 4. 6. An offset detecting means for averaging an output signal of a relative azimuth sensor when the speed of the vehicle obtained by the speed calculating means or the speed estimating means is 0 is provided. After subtracting the offset from the output signal of the relative heading sensor, multiplying by the scale factor to calculate the heading change per unit time of the vehicle,
Based on a system model that integrates the change in the traveling direction, and an observation model that represents the relationship between the traveling direction estimated by the position / direction estimation means and the integrated direction in the system model, matrix elements of the state vector are included in the system model. An offset, a scale factor, and an integrated orientation of the relative orientation sensor are set, and a Kalman filter in which the traveling direction estimated by the position / azimuth estimating means is set as an observation value is offset by the relative orientation sensor, which is a matrix element of a state vector. And a scale factor to calculate the correction value of the relative azimuth sensor, set an offset error which is a matrix element of a system error from the amount of offset fluctuation for each stop, and set the position / azimuth estimating means and the azimuth during vehicle turning. Difference in turning angle of vehicle obtained from each direction information of change calculation means A scale factor error which is a matrix element of a system error, and an azimuth error which is a matrix element of a system error used in the position / azimuth estimating means is set as an observation error. A locator device as described. 7. The azimuth change calculating means for monitoring the variation of the offset of the relative azimuth sensor to determine the effectiveness of the correction of the scale factor of the relative azimuth sensor. Locator device. 8. The speed calculating means determines that the locator device has been replaced by another vehicle when the change in the scale factor of the distance sensor calculated by the speed calculating means has exceeded a predetermined ratio. 7. The locator device according to claim 5, wherein: 9. A storage means having a plurality of memory areas for storing a scale factor of a distance sensor calculated by a speed calculation means, and a storage destination specifying means for a user to specify an address of a memory area of the storage means. 4. The storage device according to claim 1, wherein the storage unit reads and writes a scale factor of the distance sensor in a memory area at an address designated by the storage destination designation unit. 5. Locator device. 10. A storage unit having a plurality of memory areas for storing a set of a scale factor of a distance sensor and a memory access time calculated by a speed calculation unit, and a value close to the scale factor calculated by the speed calculation unit. It is searched whether or not it is recorded in the storage means, and if it is recorded, the address of the recorded memory area, if it is not recorded, the address of an unused memory area, and further unused If there is no memory area, there is provided storage destination search means for searching for the address of the memory area in which the memory access time is the oldest, and the storage means comprises a memory area at the address designated by the storage destination search means. In addition, the scale factor of the distance sensor calculated by the speed calculating means is read and written, and the memory access time stored as a pair with the scale factor is read. To, the locator apparatus according to claim 8, wherein the read and write the current time.