JPH03188318A - Azimuth detector - Google Patents

Abstract

PURPOSE:To accurately calculate an estimated azimuth by estimating the offset value of a revolving angular velocity sensor on the basis of the offset value at the stop time of a moving body and the period from stop to stop and using the noise components of a gesmagnetism sensor and the revolving angular velocity sensor and the estimated value. CONSTITUTION:The noises sigmaH, sigmaG the azimuth data thetaH of a gesmagnetism sensor and the output data DELTAthetaG of a revolving angular velocity sensor are stored by a noise calculation means 12 and the offset value o(t) of the revolving angular velocity sensor is embodied on the basis of the actually measured offset value at the stop time of a moving body and the period from stop to the next stop. Then, an apparent noise component is calculated according to formula. The measured noise component is supplied to a filter gain calculation means 13 to calculate a Karman filter gain K. Predetermined weighting processing is applied by a Karman filter means 11 to output an estimated azimuth theta.

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), It is equipped with a processing device that performs the necessary processing on the output signals from both sensors, and is equipped with a processing device that performs the necessary processing on the output signals from both sensors. Dead Reckoning has been proposed to obtain current position data of a moving object using an angular velocity sensor. This method uses, for example, ΔJ
By calculating the east-west direction component ΔX (-ΔJXcosθ) and the north-south direction component Δy (-ΔJXsinθ) of This method calculates the current position output data (Px, Py), but it has the disadvantage 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 due to the influence of strong electromagnetic fields from the outside, errors may occur again. Therefore, unless the geomagnetic sensor output data containing such magnetic field disturbances is accurately detected and eliminated, the correct orientation cannot be determined.

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, the offset value and its time change characteristics vary depending on the temperature, humidity, and type of turning angular velocity sensor. Further, even if the same type of turning angular velocity sensor is used, individual differences are reflected.

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.
This is because the average value does not become zero in the offset output.

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. When determining the reliability of the output data of the turning angular velocity sensor mentioned above, the reliability is determined by including the offset deviation that occurs in the turning angular velocity sensor actually used, and the current orientation of the moving object is accurately determined. The object of the present invention is to provide a direction detection device that can estimate the direction of the direction.

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 of the turning angular velocity sensor is calculated as a function of time o
(t), the offset value prediction means 15 stores the noise component σH of the output data θ8 of the geomagnetic sensor, and also stores the noise component σG of the output data ΔθG of the turning angular velocity sensor, and stores the noise component σG of the output data ΔθG of the turning angular velocity sensor,
(t) to calculate the apparent noise component of the output data ΔθG of the turning angular velocity sensor using the formula % formula % ()); and a filter gain calculation unit 12 that calculates the Kalman filter gain K using the noise component. Means 13, the above-mentioned Kalman filter gain K is applied to the azimuth data θ” obtained from the output of the geomagnetic sensor and the azimuth data θ6 obtained from the output of the turning angular velocity sensor.
The apparatus includes an azimuth estimating means 11 that calculates the current estimated azimuth θ of the moving body by performing weighting processing based on .

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

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

θH−θ+V θG−〇+Δθ′+W+o(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 O. o(t) is an offset value that changes over time.

According to the orientation detection device of the present invention, the noise calculating means 12 stores the noise σH1σG of the output data θH1 of the geomagnetic sensor and the output data ΔθG of the turning angular velocity sensor as the random noise components v and W, and offsets 0 ( t) is specified based on the actually measured offset value when the moving body stops and the period from one stop to the next stop.

That is, since the offset value can be actually measured when the moving object stops, the above function □(1) can be set to a constant value A1 of 1-0. Further, if the period from one stop to the next stop is T, and the actually measured offset value at the time of this stop is A2, then o (T) - A2 can be obtained. This means that the function 0 (
t) means giving boundary conditions to Functional form 0(t
) can be assumed by theory or actual measurement, then
It is possible to obtain an offset function 0(t) that accurately reflects the offset characteristics of the turning angular velocity sensor actually used.

Then, the apparent noise component is calculated using the formula (σG+o(t))2. As a result, the output data of each sensor θ
In addition to the reliability of H, the reliability of ΔθG can be evaluated including the offset. Off the coast! The determined 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 the offset value, so it is possible to accurately estimate the orientation of the moving body according to the offset characteristics of the turning angular velocity sensor.

Furthermore, if the noise calculation means collects the noise component σHσG based on driving data, it is possible to evaluate σ8 and σ6 according to the actual driving conditions in real time, further improving the reliability of the estimated heading. I can do it.

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 embodiments.

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, for example, counting the number of output pulse signals from the vehicle speed sensor 41 with a counter. The mileage output data per unit time is calculated by multiplying by a predetermined constant indicating the distance of
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 θ8 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θ6 - θ>1 between this value and, for example, the average value of several past estimated headings θ>1 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 you are traveling in a straight line, increment the counter n by 1 (Step ■), and store the sensor output data ΔθG θ'' in the nth address of the buffer memory 3 (Step ■); Hereinafter, the data stored in the nth address will be incremented by ΔθG. n1θHn
), determine 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
Average value using n (n-t1), -1 〈Δθ'> = (1/m) Σ ΔθG n
.

n snow 1 Variance value, 　　　　. ,.

σG 2 − (1/at) Σ (ΔθG n
−〈θG〉)2ri is calculated.

In step ■, data θ' n collected in the past m times
(n-t1), average value, nm 〈θ'> −(1/a+) Σ θHnr1 Variance value, −m σ” = (1/+) Σ (θHn <θH>)
Find 2n=1.

The variance value σG2 σH2 obtained as described above corresponds to the magnitude of random noise in the sensor output data 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. If the functional form o of the offset
Assuming that (t) is a linear function αt of time, the time rate of change α of the offset is calculated by dividing the offset correction value A by the time T-t2-tl required from the previous start time tl to the stop time t2. be able to.

The above-mentioned time rate of change α is determined each time the vehicle stops and starts, so it is sufficient to update α each time. With this, it is possible to accurately evaluate offset drift due to temperature or the like at each point in time.

The reason why we assumed the function form o (t) to be a linear function αt of time as described above is because we thought that the offset value often increases with time, as shown in Figure 4, so it can be sufficiently approximated by a linear function. However, the function is not limited to the number of -order intervals, and the optimum function may be determined according to the characteristics of the turning angular velocity sensor and actual measurement data. For example, a quadratic function αt2, an exponential function α(e'-1), etc. may be assumed as the form of o(t).

Therefore, using the time change rate α of the offset previously determined, the apparent variance value is calculated using the following formula (% formula %).

In this embodiment, the data ΔθG is sampled every time Δt, and in step [stock], Δt
It is calculated as (σ6+α'n)2 using the offset increase rate α'-α/Δt that increases with each shift. From now on, this apparent variance value will be calculated as Gyro 4.
It is assumed to be the dispersion value of the output data ΔθG of No. 3, and it is set as σG2 again.

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

θi − (1−Ki ) (θi−1+Δθ'i)+
Ki θH1 Here, θi is the estimated direction obtained by this interruption, θi
-1 is the direction found last time. ΔθGi.

θHi is the output data of the sensor used when calculating the estimated direction this time, for example, the latest data ΔθG m,
θHrt, or the above average value Δθ'><
K1 is a variable with 0<Ki<1 and is called the Kalman gain.Kl is calculated using the previously calculated Kalman gain K i-1, σG 2 +σG2+K1-1 σH2 It is required as.

As described above, the Kalman gain Ki 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 θl is determined (step @), and the initial state is return(
Step @).

The estimated position of the vehicle can be calculated from this direction θl 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 ΔθG n collected during past driving.

Although the average value and the variance value are obtained using θHn, it is also possible to store a certain value in advance according to the characteristics of the turning angular velocity sensor and the geomagnetic sensor and calculate using only that value. 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.

<Effects of the Invention> As described above, according to the azimuth detection device of the present invention, the offset value of the turning angular velocity sensor can be determined based on the actually measured offset value when the moving body is stopped and the period measured from the time when the moving body is stopped. Based on the noise component included in the output data θH of the geomagnetic sensor and the output data ΔθG of the turning angular velocity sensor, and the predicted offset o (t), It is possible to estimate an accurate orientation of the moving object that reflects the actual offset characteristics of the turning angular velocity sensor, and to accurately determine the position of the moving object during movement based on the orientation.

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. It is a graph showing a time change of an offset value. 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 of time o(t) based on the actually measured offset value when the moving object is stopped and the period measured from the time of stop to the time of the next stop. , the noise component σ^H of the output data θ^H of the geomagnetic sensor is stored, and the output data Δθ of the turning angular velocity sensor is stored.
Noise calculation means that stores the noise component σ^G of ^G and calculates the apparent noise component of the output data Δθ^G of the turning angular velocity sensor using the following equation using the predicted offset o(t); σ^G+o(t))^2 A filter gain calculation means that calculates the Kalman filter gain K using the above noise component, azimuth data θ^H obtained from the output of the geomagnetic sensor, and azimuth obtained from 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 object by subjecting the 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.