JPH03188317A - Azimuth detector - Google Patents

Abstract

PURPOSE:To accurately calculate an estimated azimuth by estimating the offset value of a revolving angular velocity sensor as a time function and using the noise data contained in the data of a gesmagnetism sensor and the revolving angular velocity sensor and the estimated value. CONSTITUTION:The respective noises sigmaH, sigmaG of the azimuth data thetaH of a gesmagnetism sensor and the output data DELTAthetaG of a revolving angular velocity sensor are calculated by a noise calculation means 12 to be preliminarily stored and the function form of the offset value o(t) of the azimuth data of the revolving angular velocity sensor is supposed by theory or actual measurement and an apparent noise component is calculated according to formula. The measured noise value is supplied to a filter gain calculation means 13 to calculate a Karman filter gain K. Predetermined weighting is applied by a Karman filter means 11 to output an estimated azimuth theta. Since the weighting corresponding to the change of the offset value can be performed, the azimuth of a moving body can be accurately estimated.

Description

[Detailed Description of the Invention] <Industrial Application Field> The present invention relates to an azimuth detection device, and more specifically, it uses a geomagnetic sensor to detect the direction of movement of a moving body to determine the turning angular velocity. The present invention relates to an orientation detection device that detects the orientation of a moving body in one direction using a turning angular velocity sensor (for example, an optical fiber gyro, a mechanical gyro, a vibration gyro, or a gas rate gyro).

<Prior Art> Conventionally, the explanation will be based on the assumption that vehicles traveling at any point on a road transportation network, aircraft navigating air routes, ships navigating sea routes, etc. (hereinafter referred to as "vehicles").Vehicles of"
A distance sensor, a direction sensor (either a geomagnetic sensor or a turning angular velocity sensor) are used as a method for detecting the position of an aircraft or ship. , and a processing device that performs necessary processing on the output signals from both sensors, and is equipped with a processing device that performs necessary processing on the output signals from both sensors. Dead Reckoning has been proposed to obtain current position data of a moving object using a turning angular velocity sensor. This method calculates, for example, the east-west component ΔX (-ΔJX cos θ) and the north-south component Δy (-ΔJX sin θ) of Δ based on Δ and θ, and then calculates the east-west component ΔX (-ΔJX cos θ) and the north-south component Δy (-ΔJX sin θ) of Δ On the other hand, each of the above components ΔX.

By adding Δy, the current position output data (P
This method calculates the values (x, Py), but it has the drawback that errors included in the obtained current position data are accumulated due to the errors that the orientation sensor inevitably has.

That is, when the direction sensor is a geomagnetic sensor that detects the earth's magnetism and knows the absolute direction of the moving body, the geomagnetic direction sensor detects the strength of the weak earth's magnetic field,
If the main body of the moving body is magnetized, an error will occur in the output data. In order to cancel out this error, the geomagnetic direction sensor is initialized, but when a vehicle is driving, it must pass through railroad crossings, power cable burial sites, railway bridges, expressways with soundproof walls, and valleys between high-rise buildings. As the amount of magnetization of the vehicle body changes over time due to the influence of strong electromagnetic fields from outside, errors may occur again. Therefore, the correct orientation cannot be determined unless the geomagnetic sensor output data containing such magnetic field disturbances is accurately detected and eliminated.

On the other hand, when using a turning angular velocity sensor, for example,
When the direction change exceeds a predetermined value, when the power (ignition) is turned on, when driving at extremely low speeds, or when driving on rough roads such as mountain roads is detected, a lot of noise may appear in the sensor output data. is known, and if these data are used as is, the direction detection accuracy will decrease.

Therefore, two orientation sensors, a turning angular velocity sensor and a geomagnetic sensor, are used together as an azimuth sensor, and if the reliability of either the output data of the turning angular velocity sensor or the output data of the geomagnetic sensor decreases, the other A direction detection device that supplements the information with data is conceivable.

One method that can be applied to this direction detection device is to consider the noise included in the output data of the turning angular velocity sensor and the noise included in the output data of the geomagnetic sensor as random noise that follows a normal distribution, and use the Kalman filter theory. This is a method of estimating the final direction by giving weight to data with less predicted noise, that is, data that is considered to be more reliable.

However, with turning angular velocity sensors, even when the sensor output should be zero during straight travel, there is a tendency for some output (offset) to occur due to the influence of temperature and humidity. Since this offset output has the property of accumulating, a direction deviated from the actual running direction will be detected.

Furthermore, this offset output cannot be detected as random noise in the turning angular velocity sensor output data.

The reason for this is that in the case of random noise, the average value is 0, so it can be detected by taking the variance value of the data, but in the case of an offset output, the average value does not become 0.

Therefore, when estimating the final orientation by weighting data with less noise, that is, data with more reliability, using the Kalman filter theory, some kind of correction processing for the offset is desired.

An object of the present invention is to obtain an azimuth detection device that captures the output data of a turning angular velocity sensor and a geomagnetic sensor, calculates the current azimuth of a moving object from those values and estimated past azimuths, and thereby determines the current position of the moving object. Provided is an azimuth detection device capable of accurately estimating the current azimuth of a moving body by determining the reliability of the output data of the turning angular velocity sensor by determining the reliability including offset deviation. It's about doing.

Means for Solving the Problems> The orientation detection device of the present invention for achieving the above objects has the following features:
As shown in FIG. 1, the offset value prediction means 15 predicts the offset value of the turning angular velocity sensor as a function of time o (t), and the turning angular velocity sensor stores the noise component σH of the output data θH of the geomagnetic sensor. Noise calculation means that stores the noise component σG of the output data ΔθG of 12, filter gain calculation means 13 for calculating the Kalman filter gain K using the noise component, and azimuth data θH of the geomagnetic sensor.
, and an azimuth estimating means 11 that calculates the current estimated azimuth θ of the moving body by subjecting the output ΔθG of the turning angular velocity sensor to weighting processing based on the above-mentioned Kalman filter gain K.

Note that the noise calculation means uses the output data θ of the geomagnetic sensor.
The noise component σ9 included in "" and the noise component σG included in the output data ΔθG of the turning angular velocity sensor may be calculated based on travel data.

<Operation> The azimuth data θH1 of the geomagnetic sensor and the azimuth data θG obtained from the output of the angular velocity sensor are expressed as follows, including random noise and offset.

θH−θ+V θG■θ+Δθ′+W+0(t) Here, V and W are random noises included in the outputs of the geomagnetic sensor and the turning angular velocity sensor, and their average value is 0. o(t) is an offset value that changes over time.

According to the orientation detection device of the present invention, the noise calculation means 12 records the noise σ1.σH of the output data θ” of the geomagnetic sensor and the output data ΔθG of the turning angular velocity sensor as the random noise components v and W. Then, assuming the functional form of the offset value o (t) by theory or actual measurement, the apparent noise component is calculated using equation 4 (%()).

Thereby, it is possible to evaluate the reliability of the output data θG of each sensor as well as the reliability of ΔθG including the offset. The measured noise value is supplied to filter gain calculation means 13, and Kalman filter gain K is calculated.

Then, the Kalman filter means 11 performs a predetermined weighting process and outputs the estimated orientation θ.

In this way, it is possible to weight ΔθG according to the change in offset, so it is possible to estimate the orientation of the moving object more accurately.

In addition, if the noise calculation means collects the noise component σHσH based on driving data, it is possible to evaluate σHσG in real time according to the driving conditions during actual straight-line driving, which further improves the reliability of the estimated heading. can be improved.

The reason for measuring the noise component during running is that it is preferable to evaluate noise using data during running, which includes noise caused by vibrations during running, dynamic characteristics of the moving body, and the like.

In particular, it is more preferable to use data during straight running that does not include angular velocity components due to turns.

<Example> Embodiments will be described in detail below with reference to the accompanying drawings showing examples.

FIG. 3 is a block diagram showing an embodiment of the direction detection device of the present invention applied to a vehicle. Vehicle speed sensor 41 that detects the rotational speed of both left and right wheels (this sensor is used as a distance sensor). )・Geomagnetic sensor 42,・Gyro 43 (an optical fiber gyro that reads the turning angular velocity as a phase change of interference light, a vibrating gyro that detects the turning angular velocity using cantilever vibration technology of a piezoelectric element, a mechanical gyro, etc.) selected from.

Used as a turning angular velocity sensor. ), ・Road map memory 2 storing road map data, ・Calculates the estimated heading of the vehicle based on the output data detected by the gyro 43 and the geomagnetic sensor 42, and also calculates the vehicle position along with the data of the vehicle speed sensor 41. It consists of a locator 1 that calculates output data and stores it in a buffer memory 3; a navigation controller 5 that displays the read vehicle current position on a display 7 overlaid on a map and interfaces with a keyboard 8;

The locator 1 obtains the number of wheel revolutions by counting the number of output pulse signals from the vehicle speed sensor 41 with a counter, and uses a multiplier to calculate the number of rotations per count for the count output data output from the counter. The mileage output data per unit time is calculated by multiplying by a predetermined constant indicating the distance, and the relative change in vehicle direction is obtained from the gyro 43, and this and the geomagnetic sensor 42
The vehicle's azimuth output data is calculated from the absolute azimuth output data of the vehicle.

The road map memory 2 stores road map data covering a predetermined range in advance, and includes a semiconductor memory,
Cassette tape, CD-ROM.

IC memory, DAT, etc. can be used.

The display 7 uses a CRT, liquid crystal display, or the like to display a road map and the vehicle position while the vehicle is traveling.

The navigation controller 5 is composed of a graphic processing processor, an image processing memory, etc., and performs map searching, scale switching, scrolling, etc. on the display 7.
The current position of the vehicle is displayed.

A procedure for detecting vehicle direction using the device configured as described above will be explained. While the vehicle is running, the position of the vehicle is displayed on the display 7 together with a map based on the output data of each sensor captured by the locator 1, but even during the display, the output data of each sensor is displayed due to interruptions at regular intervals. I am trying to import the data and update the vehicle position. FIG. 2 shows the vehicle direction detection flow at the time of this interruption. Note that this interruption may be performed every fixed distance traveled, which is determined based on the distance output data of the vehicle. The length of the above-mentioned fixed time or fixed travel distance is appropriately set depending on the type of gyro used, the performance of the geomagnetic sensor, etc.

First, in step (2), output data ΔθG of the gyro 43 and output data θH of the geomagnetic sensor 42 are taken in.

Next, determine whether the vehicle is traveling in a straight line (step ■
). Whether or not the vehicle is traveling on a straight road can be determined by simple comparative calculations based on output data from the gyro 43 over a relatively short period of time. That is, θ6−θ+ΔθG is calculated using the output data ΔθG of the gyro 43 and the previous estimated orientation θ. If the difference 1θG <θ>1 between this value and, for example, the average value θ> of several past estimated directions is less than the reference value e, it can be determined that the vehicle is traveling in a straight line.

In addition to this, the determination may be made using a map matching method.

If the vehicle is not traveling in a straight line, the estimated orientation θG is registered for use in orientation detection (step ■), a counter n indicating the number of continuous interruptions is set to 0 (step ■), and the process proceeds to step [stock].

If the vehicle is traveling in a straight line, the counter n is incremented by 1 (Step ■), and the sensor output data ΔθG θH is stored in the nth address of the buffer memory 3 (Step ■); Hereafter, the data stored in the nth address is incremented by Δθ' n, θH
(expressed as n), it is determined whether n has reached the maximum value m determined from the capacity of the buffer memory 3, etc. (step
). If the maximum value m has been reached, it is determined that a sufficient amount of data has been collected for the following calculation, and the process proceeds to step (2). If the maximum value m has not been reached, the process returns to the state before the interrupt and repeats the above process.

In step ■, data Δθ' collected in the past m times is
Using n (rrl~m), calculate the average value, n■m Δθ'> −(1/m) Σ ΔθGn.

r1 variance value, . "m σ" - (1/m) Σ (ΔθGn-kuθG〉)2
Find n=1. In step ■, the data θ' n (n-1-11) collected in the past m times are used to calculate the average value, 11 m θ'>-(17i) Σ θHn -1 variance, n m σH2-( 1/i) Σ (θ'n-<θH>)2n
Find =1.

The variance value σG2 σH2 obtained as described above corresponds to the magnitude of random noise in the output data of the turning angular velocity sensor that follows a normal distribution.

Next, the offset of the output data ΔθG of the gyro 43 is determined. FIG. 4 is a graph showing the instantaneous value of the output data of the gyro 43. The offset value is corrected from A to O between the time of stopping t2 and the next time of starting t3, and the offset value o (t) is corrected again after starting. The figure shows how it appears and increases with time t. Therefore, assuming that the functional form of the offset after the start time t3 is o (0) - 0, the apparent variance value is calculated using the following formula.

The shape of o (t) may be determined theoretically or experimentally depending on the characteristics of the gyro. For example, a linear function αt that increases with time, a quadratic function αt2 an exponential function α(e'-
1) etc.

From now on, this apparent dispersion value will be used as the output data Δ of the gyro 43.
It will be regarded as the variance value of θG, and will be rewritten as σG2.

In step ■, the variance value σG2. The estimated orientation taking σH2 into consideration is calculated based on the following equation.

θi − (1−Ki ) (θi−1+Δθ′f)+
Ki θHt Here, θi is the estimated direction obtained by this interruption, θj
-1 is the direction found last time. ΔθGH9θH+ is the output data of the sensor used when calculating the estimated heading this time. For example, the latest data ΔθGm and θHm may be used, or the above average value ΔθG〉 θ”〉 may be used. .K1 is a variable with Q<Kl<1, and is called Kalman gain.Ki uses the previously calculated Kalman gain KI-1, and is expressed as Ki -σG2+K1-1 σ″2 σG 2 +σH2+K1-1 σH2 Desired.

As described above, the Kalman gain Kj is determined in real time according to the amount of noise that takes into account the offset of the gyro 43, and as a result of weighting, a new vehicle orientation θi is determined (step @), and the initial state is return(
Step @).

The estimated position of the vehicle can be calculated from this direction θi and the distance data from the vehicle speed sensor 41.

Of course, at this time, a map matching method may be adopted in which the estimated position of the vehicle is corrected by comparing it with the road map data, evaluating the degree of correlation with the road map data, and setting the current position of the vehicle on the road. (See Publication No. 63-148115)
.

Although the orientation detection device of the present invention has been described above based on the embodiments, the present invention is not limited to the above embodiments. For example, in step ■■ of the above embodiment, data ΔθGn collected during past driving.

The average value and variance value were calculated using θHn, but it is possible to memorize certain values in advance according to the characteristics of the turning angular velocity sensor, geomagnetic sensor, and actual measurement data while traveling in a straight line, and calculate using only those values. You can. This simplifies control and makes it possible to estimate accurate heading even when the vehicle is not traveling in a straight line.

Furthermore, the present invention can be applied to moving bodies such as aircraft and ships instead of vehicles, and various other design changes can be made without changing the gist of the present invention.

Calculation Example> In order to confirm the effectiveness of the present invention, the following simulation was performed.

The output of the geomagnetic sensor is θH1 The output of the turning angular velocity sensor is ΔθGn = ΔθGO + βt ■ Create a pseudo input in the form (ΔθG0 is the initial value, β is a constant), and for simplicity, assume that there is no change in direction (running in a straight line). did. Therefore, Δθ'o−o, θH− was set as a constant value. In addition, the data variance value σ' (2 was also set as a constant value).

Then, assuming o (t) to be the -order number αt, the apparent variance value was calculated while varying the value of α.

(1) First, in the case of α-0, that is, the estimated heading was obtained using the formula (%) without taking into account changes in offset. In this case, the Kalman constant Ki is σ1. It is a constant determined by σH.

FIG. 5(a) is a graph showing the tendency for errors to accumulate between the estimated heading obtained as described above and the "true" heading according to the pseudo input of the above equation 0.

In both cases, it can be seen that the error increases linearly with offset. In particular, when σG is small, the data from the turning angular velocity sensor is given more weight than the data from the geomagnetic sensor, so the accumulation of errors is significant.

(2) Next, calculations were performed taking into account α≠0, that is, the offset. FIG. 5(b) is a graph showing the tendency for errors to accumulate when σH is kept constant and α is set to three types. In both cases, it can be seen that the errors have converged to a constant value. In particular, if α is large, more emphasis is placed on data from the geomagnetic sensor, and the accumulated error converges quickly.
If α is small, more emphasis is placed on the data from the turning angular velocity sensor, and as a result, the accumulated error increases and converges slowly.

As mentioned above, by calculation that takes offset into account,
It was found that the error was within a certain value.

This proves that the effects of the present invention are significant.

<Effects of the Invention> As described above, according to the orientation detection device of the present invention, the offset value of the turning angular velocity sensor is predicted as a function o (t) that changes with time, and the output data θ of the geomagnetic sensor is
H and the noise component included in the output data ΔθG of the turning angular velocity sensor and the above predicted offset o (t)
Using this, it is possible to estimate the accurate orientation of the moving object in consideration of the offset, and based on the above-mentioned orientation, it is possible to accurately determine the position of the moving object while it is moving.

Also, if the noise components included in the output data θH of the geomagnetic sensor and the output data ΔθG of the turning angular velocity sensor are determined in real time based on actual driving data,
A more accurate direction can be estimated.

[Brief explanation of drawings]

Figure 1 is a functional block diagram of the orientation detection device of the present invention, Figure 2 is a flowchart showing the orientation detection procedure, Figure 3 is a block diagram showing the hardware configuration of the orientation detection device, and Figure 4 is a diagram of the turning angular velocity sensor. Graph showing the change in offset value over time. FIG. 5 is a graph showing the results of computer simulation. 1... Locator, 3... Buffer memory, 11...
- Orientation estimation means, 12... Noise calculation means, 13...
- Filter gain calculation means, 1 5... Off-cerat prediction means, 2... Geomagnetic sensor, 3... Gyro

Claims (1)

[Claims] 1. An azimuth detection device that takes in azimuth data from a geomagnetic sensor and azimuth data obtained from the output of a turning angular velocity sensor, and calculates the current estimated azimuth of a moving body from these values and past estimated azimuths. An offset value prediction means for predicting the offset value of the turning angular velocity sensor as a function o(t) that changes with time; Output data Δθ
Noise calculating means for storing the noise component σ^G of ^G and calculating the apparent noise component of the output data Δθ^G of the turning angular velocity sensor using the following formula using the predicted offset value o(t); (σ^G+o(t))^2 A filter gain calculation means that calculates the Kalman filter gain K using the above noise component, and azimuth data θ^H obtained from the output of the geomagnetic sensor and the output of the turning angular velocity sensor. An azimuth detecting device comprising azimuth estimating means for calculating the current estimated azimuth θ of a moving body by subjecting azimuth data θ^G to weighting processing based on the Kalman filter gain K. 2. The noise calculation means uses the output data θ^ of the geomagnetic sensor
The azimuth detecting device according to claim 1, wherein the noise component σ^H included in H and the noise component σ^G included in the output data Δθ^G of the turning angular velocity sensor are calculated based on actual travel data.