JPH0814494B2 - Position detection device - Google Patents

Description

Description: TECHNICAL FIELD The present invention relates to a position detection device that autonomously detects the position of a vehicle such as a carrier vehicle using information from a distance sensor and a direction sensor.

[Conventional technology]

An example of a conventional self-supporting position detecting device for a vehicle will be described with reference to FIGS. 4 and 5.

In FIG. 4, (1) is a running body such as a carrier whose position at the time of running is to be detected. (2)
Is an angular velocity sensor, which is attached to the transport vehicle (1) such that its input shaft is parallel to the turning axis of the transport vehicle (1).
(3) is a distance sensor, a typical example of which is a pulse generator that generates a pulse according to the rotation of the wheel (5) attached to the rotating shaft of the wheel (5) of the transport vehicle (1). (4),
(4 ') ... are position sensors separately installed at arbitrary places in the action area of the transport vehicle (1), and when the transport vehicle (1) passes near them, the transport vehicle (1) ) Is detected, its detected position is input to the self-standing position detecting device, that is, the position calculator (6), and its output position is reset to the true position. Position sensor (4),
As (4 ') ..., various methods such as an optical method, an ultrasonic wave, and a radio wave are conceivable, but since the details thereof are not related to the present invention, further description will be omitted.

The position calculator (6) will be described with reference to FIG. As shown in the figure, the output of the angular velocity sensor (2) is integrated by the azimuth integrator (7) in the position calculator (6), and from this, the azimuth angle φ of the carrier vehicle (1) at each time is obtained. This azimuth angle φ is input to the sin / cos generator (8). The sine sinφ and the cosine cosφ of the azimuth angle φ from this are input to the X distance calculator (10) and the Y distance calculator (9), respectively. The angular velocity sensor (2) and the azimuth integrator (7) can be replaced with a single azimuth sensor such as an integrating gyro.

Y, X distance calculators (9), (10)
And the distance output ΔD from the distance sensor (3), the carrier vehicle (1) calculates and outputs the distance from the reference position of the carrier vehicle (1) by integration, that is, the position sensor (4), for example.
When the vehicle passes through the vicinity of, the correct position of the carrier vehicle (1) at that moment is input to the position correction calculator (11) of the position calculator (6), and the Y and X distance calculation input to it is performed. Reset the position output from devices (9) and (10) to the correct values.

[Problems to be solved by the invention]

However, in the above-described conventional self-sustaining position detecting device, the output position of the position calculator (6) is only reset to a correct value by the outputs of the position sensors (4), (4 '), etc. If there is an error in the azimuth angle φ that is the output of the integrator (7), the error in the position output will increase with the passage of time, and the output of the angular velocity sensor (2) will show a normal gyro drift. There is a so-called error, which also causes a problem that the position error increases.

Further, in order to reset the azimuth angle itself, the position sensor (4) and the special vehicle for detecting the azimuth of the guided vehicle (1) are required for the guided vehicle (1), and the detected azimuth accuracy is sufficiently improved. It is essential that there is a problem that the system cost rises overall.

Therefore, the present invention is to provide a position detecting device which eliminates the above-mentioned drawbacks of the conventional device.

[Means for solving problems]

The present invention includes a distance sensor (3) and an angular velocity sensor (2).
And the azimuth sensor including the azimuth integrator (7), the position calculation device (6), and the position detection device of the running body including the position sensors (4), (4 '), and the distance sensor (3). A sensor error calculator (31) that receives the outputs of the azimuth integrator (7) and the position sensor (4), the output of the distance sensor, and the scale factor correction value of the sensor error calculator To the X, Y distance calculators (10) and (9), the output of the angular velocity sensor, and the drift correction value of the sensor error calculator ( Δ
) And an output of the drift corrector (34) that outputs the output to the azimuth integrator, the output of the azimuth integrator and the azimuth correction value of the sensor error calculator. And an azimuth error corrector (32) which inputs the output to the sin / cos generator (8) and which uses a Kalman filter for the calculation of the sensor error calculator.

[Action]

Velocity component from distance sensor (3), azimuth integrator (7)
The sensor error correction system (30) always predicts the system error occurrence state using the azimuth from the position, and when the position from the position sensor (4) is input, it and the system position of the position correction calculator (11). Then, the Kalman filter is used to estimate the cause of the error, the position error, the azimuth error, the scale factor of the distance sensor, and the gyro drift by using the comparison result as an observation error.

〔Example〕

An embodiment of the present invention will be described below with reference to FIGS. 1 to 3. In the figure, the same members as those in FIGS. 4 and 5 are designated by the same reference numerals, and the description thereof will be omitted.

The present invention is obtained by adding a sensor error correction system (30) surrounded by a broken line in FIG. 1 to the conventional device shown in FIG. This sensor error correction system (30) is a sensor error calculation that receives the outputs of the position sensors (4), (4 ') ... Position correction calculator (11), distance sensor (3) and direction integrator (7). And an azimuth error corrector (32) provided between the azimuth integrator (7) and the sin / cos generator (8), which receives the output of the sensor error calculator (31) as input. It is mainly composed of a distance S / F (scale factor) corrector (33) provided on the output side of the sensor (3). still,
(34) is a drift correction device provided between the angular velocity sensor (2) and the azimuth integrator (7), and is a sensor error calculator (31)
Is used as an input to correct the drift error of the angular velocity sensor (2).

Here, the sensor error calculator (31) which is the basis of the present invention
An outline of the Kalman filter used in the above will be explained. If the system is described by a first-order differential equation, the following equation is obtained.

(T) = A (t) · x (t) + W (1) x (t) = state variable matrix A (t) = system matrix W = random vector (1) is transformed into a discrete value form by the transition matrix method. Then, The observation system can be expressed by the following equation.

y (t) = H · x (t) + v (3) y (t) = observed value When the system is described by the equations (2) and (3), the optimum estimated value x (t + Δt, t + Δt) by the Kalman filter is obtained by the following equation.

Where II is the unit matrix.

Of (4) Is the error covariance matrix, Is to predict how much the system error statistic will change after Δt seconds at the current time t. Of (5) Is a Kalman gain, which is calculated when the position information is obtained, and is a quantity that determines whether to weight the system or the observation value by comparing the statistics of the system error and the observation noise. Equation (7) is the optimum estimated value, and the value after Δt seconds is predicted using the estimated value of the system error at time t, but this predicted value is not always correctly predicted due to the influence of disturbance. , The deviation between the observed value and the predicted value is used when the observed value is obtained, and the Kalman gain is used to correct the optimum estimated value. The equation (8) is an error covariance matrix, and when the observed value is obtained and the system error is corrected, the statistic of the system error also needs to be corrected, and is corrected using the equation (8).

When FIG. 1 is modeled using the Laplace operator S,
It becomes FIG. In the figure, 1 / S indicates integration,
Let φ ', X', Y'represent azimuth XY distances that each contain an error.

The angular velocity sensor (2) measures not only the turning angular velocity ω but also the error due to the gyro drift, and the integrated value is the azimuth angle φ ′. The velocity V from the distance sensor (3) is multiplied with a scale factor error ΔSF, and sin and cos of the azimuth angle are taken and integrated to obtain a distance X ′ in the X direction and a distance Y ′ in the Y direction. Become.

By the way, if the distance error Δ in the X direction is φ ′ = φ + Δφ, Δ = ′ − = V (1 + ΔSF) sinφ′−Vsinφ = Vsinφ · ΔSF + Vcosφ · Δφ Similarly, the distance error Δ in the Y direction is Δ = ′ − = V (1 + ΔSF) cosφ′−Vcosφ = Vcosφ · ΔSF + Vsinφ · Δφ The azimuth error Δφ is Δ = ′ − = (ω + U) −ω = U If the scale factor error ΔSF and the gyro drift U do not change with time, From the above, if this system is written as a first-order differential equation, Becomes The state variable matrix at this time can be expressed as follows.

x ^T = (ΔX, ΔY, Δφ, ΔSF, ΔU) FIG. 3 is a block diagram of a specific example of the sensor error calculator (31) shown in FIG. 1, which is the system matrix in the equation (11). A system error calculator (41) for calculating
Of the transition matrix of equation (10) Transition matrix calculator (42) for calculating and the error covariance matrix of equation (4) An error covariance matrix calculator (43) for calculating the position information, a position information switch (44) for determining the presence or absence of information from the position sensor (4), and an optimum prediction for calculating the formula (6) when the position information is not input. A position error calculator (46) for calculating a current position error from the outputs of the position sensor (4) and the position correction calculator (11) when the position information is input to the calculator (45).
And a Kalman gain calculator (47) for calculating the equation (5)
And the optimum estimated value calculator (4
8) and an error covariance matrix correction calculator (49) for calculating the formula (8).

Next, the input / output relation of the position information switch (44) of the sensor error calculator (31) will be described in more detail. When there is no position information from the position sensor (4), the output of the transition matrix calculator (42) is supplied to the optimum predicted value calculator (45) via the position information switch (44), which is the formula (6). Is calculated. On the other hand, when there is position information from the position sensor (4), the output of the error covariance matrix calculator (43) is sent to the Kalman gain calculator (4) via the position information switch (44).
7), where the calculation of equation (5) is performed, and the outputs of the Kalman gain calculator (47), position error calculator (46), and optimum predicted value calculator (45) are output. It is input to the optimum estimated value calculator (48), where the formulas (6) and (7) are calculated, and the error correction value outputs Δ, Δ, Δ is output, and these outputs are output to the position correction calculator (11), azimuth error corrector (32), distance S / F corrector (3
3), while supplying to the drift corrector (34) respectively, the output of the error covariance matrix calculator (43) and the Kalman gain calculator (47) via the position information switch (44) is corrected to the error covariance matrix. It is input to the computing unit (49) and the computation of equation (8) is performed. Output of the error covariance matrix correction calculator (49) Is used as the input of the error covariance matrix calculator (43) next time.

Although the position detecting device using the azimuth angle φ obtained by integrating the signal of the angular velocity sensor (2) by the azimuth integrator (7) has been described above, the present invention is not limited thereto.
A gyro of a type that directly outputs an azimuth angle such as a rate integration gyro, a ring laser gyro, or a tuned dry gyro, or a sensor that directly outputs an absolute azimuth angle from true north such as a gyro compass can be used.

〔The invention's effect〕

In the position detection device using the angular velocity sensor and the distance sensor, only the discrete external position information is used, and the system error such as the position error, the azimuth error, and the gyro drift is detected and calculated by using the Kalman filter. In particular, the position error caused by the azimuth error caused by the drift of the angular velocity sensor can be greatly reduced, and the position detecting device with high accuracy can be obtained. Further, since the azimuth error is corrected based on the position information, it is possible to use the angular velocity sensor with low accuracy and low cost. Further, by using the position information to detect and correct the scale factor error of the distance sensor, a highly accurate position detecting device can be obtained. Further, since it becomes possible to correct the azimuth angle and the drift of the angular velocity sensor only by the position information without using the device for correcting the azimuth angle, it is possible to reduce the cost of the entire position measuring system. .

[Brief description of drawings]

1 is a block diagram of a position detecting device having a sensor error correction system according to the present invention, FIG. 2 is a block diagram modeling FIG. 1, FIG. 3 is a block diagram of the sensor error correction system of FIG. 1, FIG. 4 is an external view of a conventional position detecting device for a vehicle, and FIG. 5 is a block diagram of a conventional position detecting device. In the figure, (1) is a carrier vehicle, (2) is an angular velocity sensor,
(3) is a distance sensor, (4), (4 '), ... are position sensors, (6) is a position calculator, (31) is a sensor error calculator,
(32) is a bearing error corrector, (33) is a distance S / F corrector, (3
4) is a drift corrector, (41) is a system error calculator,
(42) is a transition matrix calculator, (43) is an error covariance matrix calculator, (44) is a position information switch, (45) is an optimum predicted value calculator, (46) is a position error calculator, (47) Is a Kalman gain calculator, (48) is an optimum estimated value calculator, and (49) is an error covariance matrix correction calculator.

 ─────────────────────────────────────────────────── ─── Continuation of front page (72) Inventor Kazuki Sato 2-16-46 Minami-Kamata, Ota-ku, Tokyo Tokyo Keiki Co., Ltd. (56) References JP 61-155813 (JP, A) JP Sho 63-302317 (JP, A)

Claims (1)

[Claims] 1. A navigation vehicle position detecting device comprising a distance sensor, an azimuth sensor including an angular velocity sensor and an azimuth integrator, a position calculation device, and a position sensor, wherein the distance sensor, the azimuth integrator, and A sensor error calculator that receives the output of the position sensor, the output of the distance sensor and the scale factor correction value of the sensor error calculator Is a distance S / F that outputs the output to the X, Y distance calculator
A corrector, an output of the angular velocity sensor, and a drift correction value (Δ) of the sensor error calculator, and a drift corrector that outputs the output to the azimuth integrator, and an output of the azimuth integrator. Direction correction value of the above sensor error calculator A position detecting device characterized in that a Kalman filter is used for the calculation of the sensor error calculator, and a azimuth error corrector for inputting and is output to a sin / cos generator.