JP2000137040A - Angular velocity detector for moving body, and angle detctor for moving body - Google Patents

Abstract

PROBLEM TO BE SOLVED: To accurately find angular velocity and an angle for a moving body. SOLUTION: A angular velocity sensor 2 for outputting a detection voltage in response to angular velocity is provided in a prescribed portion of a vehicle, and an output of the sensor 2 is A/D-converted in a period (td) by an A/D converter 4. A microcomputor 5A determines complete rest to register an output data value of the A/D converter 4 as a zero point, when a varying width of the angular velocity indicated by an output of the A/D converter 4 from the present time upto the past t1 is within a fixed value, when an output of a vehicular speed detecting part 6 indicates zero of vehicular speed consecutively between the present time and the past t2, and when a vibration frequency indicated by a vibration detecting part 7 is a prescribed value or less successively between the present time and the past t3. Average angular velocity during a unit time T, an angle changed during the time T and a direction of the vehicle are calculated while a data output from the A/D converter 4 in the period (td) is correted on a zeropoint, using a value of the zero point registered.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a moving object angular velocity detecting device and a moving object angle detecting device, and more particularly to a method for calculating a current position using a moving object navigation device or the like. The present invention relates to a moving object angular velocity detecting device and a moving object angle detecting device suitable for detecting an angle viewed from a reference direction of a moving object.

[0002]

2. Description of the Related Art An on-vehicle navigation device is provided with an angular velocity sensor for detecting an angular velocity of a vehicle and a vehicle speed sensor for detecting a vehicle speed in order to calculate a current position and a current azimuth of a vehicle as a moving body by autonomous navigation. ing. In the autonomous navigation, the vehicle position obtained in the previous calculation is represented by O _i (longitude X = x _i ,
Latitude Y = y _i ), the vehicle azimuth at O _i (here, the absolute azimuth where true north is 0 ° in the reference direction and the clockwise direction is positive) is δ _i , and the detection output of the angular velocity sensor and the vehicle speed sensor (Relative angle) θ _{i + 1} changed during the unit time T from O _i based on the distance R _{i + 1} moved during the unit time T
Is calculated. Then, assuming that the length of the arc on the earth per one minute of the latitude difference and the length of the arc on the earth per one minute of the longitude difference are M, a new vehicle position (longitude X = x
_{i + 1} , latitude Y = y _{i + 1} ) The vehicle direction δ _{i + 1} at O _{i + 1} is calculated by the following equation. δ _{i + 1} = δ _i + θ _{i + 1} ... (1) x _{i + 1} = x _i + (R _{i + 1} .sin δ _{i + 1} ) / M (2) y _{i + 1} = y _i + (R _{i + 1} · cos δ _{i + 1} ) / P (3) The initial value of i is set to 0, and O ₀ and δ ₀ are set by the user. Each time the unit time T elapses, (1) to (3)
By calculating the formula to obtain the vehicle position O _i and the vehicle direction δ _i and performing a process of incrementing i, the vehicle position and the vehicle direction that change every moment can be calculated in real time.

FIG. 6 is a configuration diagram of a vehicle direction detecting system in a conventional vehicle-mounted navigation device. Reference numeral 1 denotes an angular velocity detection unit that detects an angular velocity of the vehicle and outputs angular velocity detection data. Among them, 2 is installed at a predetermined location of the vehicle, detects an angular velocity in a horizontal plane, and changes in proportion to the angular velocity. An angular velocity sensor 3 for outputting an analog detection voltage AV is provided on the output side of the angular velocity sensor, and an analog LPF having a cutoff frequency f _C for extracting a low-frequency component is provided.
The output of the PF A / D conversion at a predetermined sampling period t _d, an A / D converter outputs digital detection data DV. Reference numeral 5 denotes a microcomputer for reading data from the A / D converter and performing arithmetic processing, based on digital detection data DV output from the A / D converter.
The angle θ _{i + 1} changed during the unit time T is calculated, and (1)
In accordance with Equation, and to obtain the vehicle azimuth [delta] _{i + 1} of the current by adding the heading [delta] _i determined in the previous calculation.

Here, a method of calculating θ _{i + 1} will be specifically described. Note that the angular velocity detection range of the angular velocity sensor 2 is + ω ₀ to
−ω ₀ (° / s) (+ indicates the clockwise angular velocity, −
Indicates the angular velocity in the counterclockwise direction), the output range of the detection voltage AV is 0 to + E (V), and the A / D converter 4 sets the input voltage 0 to + E (V) as the quantization bit length = a. , Nature 2
It is assumed that the data is linearly quantized into data having a value in the range of 0 to 2 ^a -1 in ^a hexadecimal notation and output, and that n = T / t _d is set to an integer of 2 or more (see FIG. 7). . The microcomputer 5 outputs the n pieces of digital detection data DV ₁ , DV ₂ , output from the A / D converter 4 during the unit time T.
..., the average value of the DV _n seek <DV>. Then, a function g (see FIG. 7; see FIG. 7) representing a relationship between a previously prepared angular velocity ω of the system from the angular velocity sensor 2 to the A / D converter 4 and the value y (natural binary representation) of the digital detection data. In FIG. 7, a linear function of y = bω + d is taken as an example), and the average angular velocity <ω> corresponding to <DV>
And the angle θ _{i + 1} changed during the unit time T from the following equation
May be calculated. θ _{i + 1} = <ω> · T (4)

[0005]

Incidentally, it is known that the detection characteristic of the angular velocity sensor 2 changes its zero point under the influence of temperature, humidity and the like, and in the case of FIG. 7, the function graph moves up and down. If the function g representing the relationship between the angular velocity of the system from the angular velocity sensor 2 to the A / D converter 4 and the value of the digital detection data (natural binary number) is fixed, the A / D converter An error occurs in the average angular velocity <ω> calculated from the output data of No. 4 and the angle changed during the unit time T (offset error). In the autonomous navigation, since the error gradually accumulates, the accuracy of the vehicle position and the vehicle direction deteriorates.

The present invention has been made in view of the above-mentioned problems of the prior art,
It is an object of the present invention to provide a moving object angular velocity detecting device and a moving object angle detecting device capable of correctly obtaining the angular velocity of a moving object and an angle viewed from a predetermined reference direction.

[0007]

According to the first aspect of the present invention, there is provided an apparatus for detecting angular velocity of a moving body, which detects a voltage corresponding to the angular velocity of the moving body, converts the detected voltage into an analog signal, and outputs the detected voltage. In an angular velocity detecting device for a moving body provided with a detecting means, a speed detecting means for detecting a speed of the moving body, a vibration detecting means for detecting a vibration frequency of the moving body, an output of the angular velocity detecting means and an output of the speed detecting means, From the output of the vibration detection means, a state in which the variation range of the angular velocity is within a predetermined constant value continues for a first predetermined time, and a state in which the speed is zero continues for a second predetermined time, A determining means for determining whether or not a state in which the vibration frequency is equal to or lower than a predetermined fixed value has continued for a third predetermined time; and a state in which the change width of the angular velocity is within a predetermined fixed value. For a certain period of time State continues second predetermined period of time, and the vibration frequency is a predetermined constant value following states third
Correction means for determining a zero point from the output of the angular velocity detection means when it is determined that the predetermined time has elapsed, and performing zero point correction on the output of the angular velocity detection means using the value of the zero point. Features. Further, in the moving object angle detecting device according to claim 2 of the present invention, an angular velocity detecting means for detecting a voltage corresponding to the angular velocity of the moving body, A / D converting the detected voltage and outputting the detected voltage, and an angular velocity detecting means Based on the output of the moving body, in the moving body angle detecting device comprising calculating means for calculating the angle of the moving body, a speed detecting means for detecting the speed of the moving body, a vibration detecting means for detecting the vibration frequency of the moving body, From the output of the angular velocity detecting means, the output of the velocity detecting means, and the output of the vibration detecting means, a state in which the variation width of the angular velocity is within a predetermined constant value continues for a first predetermined predetermined time,
Determining means for determining whether the state of zero speed has continued for a second predetermined time and the state where the vibration frequency is equal to or lower than a predetermined value has continued for a third predetermined time; The calculating means continues the state in which the change width of the angular velocity is within the predetermined constant value by the determining means for a first predetermined constant time,
Further, when it is determined that the state in which the speed is zero has continued for the second predetermined time and the state in which the vibration frequency is equal to or lower than the predetermined value has continued for the third predetermined time, It is characterized in that a zero point is determined from the output, the zero point is corrected for the output of the angular velocity detecting means using the value of the zero point, and the angle of the moving body is calculated. The state in which the vibration frequency is equal to or lower than the predetermined constant value continues for a first predetermined time, and the state in which the velocity is zero continues for a second predetermined time, and the variation width of the angular velocity is within a predetermined constant value. When the entering state continues for the third predetermined time, the moving object is considered to be completely stationary. A zero point is determined from the output of the angular velocity detecting means at this time, and zero point correction is performed on the output of the angular velocity detecting means using the value of the zero point,
The angular velocity can be detected without error, or the angle viewed from a predetermined reference direction can be obtained without error.

[0008]

Next, an embodiment of the present invention will be described with reference to FIG. FIG. 1 is a block diagram showing a configuration of a vehicle azimuth detecting system of an in-vehicle navigation device according to the present invention. The same components as those in FIG. 6 are denoted by the same reference numerals. Reference numeral 1 denotes an angular velocity detector for detecting the angular velocity of the vehicle. Of these, 2 is installed at a predetermined position of the vehicle and detects an angular velocity in a horizontal plane, and outputs an analog detection voltage AV that changes in proportion to the angular velocity. an angular velocity sensor, 3 an analog LPF, 4 which are provided on the output side of the angular velocity sensor taking out low frequency components are a / D converting the output of the analog LPF sampling period t _d, the digital detection data DV
Is an A / D converter. 5A is an A / D converter 4
Is a microcomputer that calculates a new vehicle azimuth δ at a cycle of a unit time T (sec) based on the digital detection data DV output from the computer.

Reference numeral 6 denotes a vehicle speed detecting unit which is installed at a predetermined position of the vehicle and detects the vehicle speed.
The vehicle speed detection data DS having a value in a range of 0 to 2 ^c -1 in a natural binary number and proportional to the vehicle speed is sampled at a sampling period t.
Output with _s . Reference numeral 7 denotes a vibration detection unit which is installed at a position close to the angular velocity sensor 2 of the vehicle to detect the vibration frequency of the vehicle, and sets a quantization bit length = e to a range of 0 to 2 ^e -1 in natural binary notation. a is a vibration frequency detection data DF of value proportional to the vibration frequency, and outputs a sampling period t _f.

The microcomputer 5A inputs the output data DV, DS, DF of the angular velocity detector 1, the vehicle velocity detector 6, and the vibration detector 7 in parallel with the calculation of the vehicle direction. The change width of the angular velocity ω is continuously within the predetermined value W for the first predetermined constant time t ₁ , and the vehicle speed s is zero for the second predetermined constant time t _2. There, and the vibration frequency f _V ≦ f _V0 (f _V0 continued during a third predetermined constant time t ₃ is a predetermined constant value, here, the vehicle is in a stopped state with the engine stopped, (It is set lower than the vibration frequency that occurs when rotating on the turntable in the parking lot, and lower than the vibration frequency that occurs when the engine is idling while the vehicle is not rotating but stopped.) It was determined that the answer was YES. As the vehicle is completely stationary state, the value of the output data of the angular velocity detecting unit 1 at that time,
It is registered in a built-in memory (not shown) as a zero point of the angular velocity detection system. Then, using the value of the zero point, zero point correction is performed on the output data of the angular velocity detector 1 to calculate the vehicle direction.

Next, the operation of the above embodiment will be described with reference to FIGS. FIG. 2 shows a microcomputer 4
Flowchart showing the vehicle azimuth calculation processing by A, FIG.
FIG. 4 is a diagram showing the relationship between the angular velocity and the determination condition of the completely stationary state, and FIG. 4 is a diagram showing the relationship between the vibration frequency and the determination condition of the completely stationary state. Here, for convenience of explanation, T
= 1 (sec), t _d = 1/100 (sec), t _f =
1/10 (sec), t _s = 1 (sec), t ₁ = t ₂
= T ₃ = 10 (sec).

The initial value δ of the vehicle direction₀Is set by the user
Then, the microcomputer 5A has a built-in memory (FIG.
(Not shown) (step S1 in FIG. 2),
As a value, a predetermined value d predetermined in a design stage₀Set
(Step S2). Then, the angular velocity detecting unit 1 and the car
Data newly output from the speed detector 6 and the vibration detector 7
Reads DV, DS, and DF, if any, and stores them in internal memory
(Step S3). The built-in memory has DV
To t₁(= 10 sec) DS for t_Two(= 10s
ec) DF for t minutes_Three(= 10sec) Time memory
Is possible and the latest t₁Hours of DV and the latest
t_TwoDS for hours and the latest t_ThreeDF for hours will remain
The data is written as follows. Subsequent to step S3,
The microcomputer 5A has the latest
T₁Hours of DV and the latest t _TwoDS for hours,
New t_ThreeCheck if the DF for the time is already stored (step
S4). At first, the process proceeds to step S5, and
Times, T (=
1 sec) Time (n = T / t)_dOf DVs are already stored
Check, first step is still here, so go to step S3
Return, newly output data DV, DS, DF
This is read and stored in the built-in memory.

The same processing is repeated, and T is stored in the built-in memory.
(= 1 sec) When n DVs for the time are stored,
The microcomputer 5A determines YES in step S5, and from the relationship y = bω + d (where y is the value of DV,
b is the slope of the linear function g in FIG. 7, d is the value of the zero point, first is a predetermined initial value, ω is the angular velocity), and the corresponding angular velocity ω is obtained for each of the n DVs this time. An averaged average angular velocity <ω ₁ > is determined. Then, the angle (relative angle) θ changed to the first unit time T from the following equation
₁ is calculated, further, the 0 ° reference direction of true north, and outputs the calculated heading [delta] ₁ is the absolute angle obtained by the clockwise and positive (step S6 to S8). θ ₁ = <ω ₁ > × T (5) δ ₁ = δ ₀ + θ ₁ (6)

Thereafter, the flow returns to step S3, and the reading of new output data DV, DF, and DS, and the process of adding to the internal memory are repeated. Then, the DV for the next T time which does not overlap with the one used for the calculation of the vehicle direction last time
Is checked (step S5), and if YES, the angular velocities ω corresponding to the respective n DVs are obtained from the relationship of y = bω + d, and the average angular velocity <ω ₂ averaged for the n DVs > Then, to calculate the angle (relative angle) theta ₂ which is changed from the following equation for the first unit time T, further calculates and outputs the heading [delta] ₂ is an absolute angle (step S6 to S8). θ ₂ = <ω ₂ > × T (7) δ ₂ = δ ₁ + θ ₂ (8)

Hereinafter, similarly, the output of the angular velocity
Current vehicle direction δ at period T based on data_iIs the operation of
Done. However, the angular velocity sensor 2 is not affected by temperature and humidity.
Since the zero point fluctuates in response to this, if d is fixed, zero
Point error is accumulated and vehicle direction δ _iHas a large error
I will. For this reason, the microcomputer 5A determines the vehicle direction.
In parallel with the calculation of the angular velocity detector 1, the vehicle speed detector 6,
The vehicle stops completely based on the output data of the motion detector 7
The angular velocity detector 1 at that time.
Output of the angular velocity detector 1 by rewriting d with the data value
Is corrected for zero.

That is, when t ₁ = t ₂ = t ₃ = 10 sec has elapsed since the process of step S3 was performed for the first time, and when YES is obtained in step S4, the microcomputer 5A proceeds to step S9, where the angular velocity detecting unit 1 It is determined whether the change width of the angular velocity ω obtained by applying the value y of each angular velocity detection data DV for the latest t ₁ (= 10 sec) time output from the relational expression of y = bω + d is equal to or smaller than a predetermined constant width W. I do. If the answer is NO because the vehicle is traveling on a curve, the process proceeds to step S5 without changing d (D in FIG. 3A).
Reference V ₁ ~DV _n). Wakashi, (see DV ₁ ~DV _n in FIG. 3 (2)) when it is YES in step S9, because the vehicle might be traveling on a straight road, followed by being outputted from the vehicle speed detection unit 6 The latest t ₂ (= 10 sec)
It is determined whether the vehicle speeds s indicated by the vehicle speed detection data DF for the time are all zero (step S10). When the vehicle is running and the determination is NO, the process proceeds to step S5 without changing d.

If both steps S9 and S10 are performed with YE
When it becomes S, it is considered that the vehicle is stopped,
When the vehicle is rotating on a turntable in a parking lot, the output data of the angular velocity detector 1 includes an angular velocity component due to rotation. Can not be. Therefore, when YES is obtained in both steps S9 and S10, the latest t ₃ (= 1) further output from the vibration detection unit 7
It is determined whether all the vibration frequencies f _V indicated by the respective vibration detection data DF for the time 0 sec) are equal to or lower than f _V0 (step S11; see FIG. 4).

Although the vehicle is stopped and the engine is stopped, the vehicle is rotating on the rotary table in the parking lot. If the answer is NO, the process proceeds to step S5 without rewriting d. (see DF ₁ ~DF ₁₀₀ of FIG. 4 (1)),
When the vehicle is stopped, the engine is stopped, and the vehicle is not rotating and is completely still, f _V ≦ f _V0
Therefore, YES is determined in the step S11 (FIG.
See DF ₁ ~DF ₁₀₀ (2)). At this time, since the latest angular velocity detection data DV stored in the internal memory is the zero point data in the characteristic function of the angular velocity-angular velocity detection data of the angular velocity detector 1, the value of the zero point d registered in the internal memory is calculated based on the DV value. Update (step S12). Therefore, in the processing of step S6, the angular velocity can be calculated while correctly correcting the output data of the angular velocity detection unit 1 to zero using d, and the subsequent calculations in steps S7 and S8 determine the correct movement angle and vehicle direction. Will be required.

Once the answer is YES in step S4, the answer is YES in step S4 from the next time on, and the microcomputer 5A continues to execute steps S9 to S11.
It is determined whether or not the vehicle is completely stationary in the process (3), and if the vehicle is completely stopped and YES is obtained in all of steps S9 to S11, d is updated again. As a result, the output data of the angular velocity detection unit 1 can be correctly corrected for zero using the correct zero data at all times, and the accuracy of the angular velocity and the movement angle is improved, and the accumulated error of the vehicle azimuth is extremely small.

According to this embodiment, the timing at which the vehicle is completely stationary is correctly determined,
At this time, the zero point of the angular velocity detecting system is determined from the output data of the angular velocity detecting section 1, the zero point is corrected for the output of the angular velocity detecting section 1 using the value of the zero point, the angular velocity for each DV is obtained, and the unit time T Average angular velocity during T
Since the vehicle azimuth which is an absolute angle with the clockwise direction being positive with respect to the angle (relative angle) that has changed during this period and true north is calculated, the accumulated error in autonomous navigation can be reduced.

In the above embodiment, T = 1 (s
It was with ec), 2,3, well any other time, such as, t _d
= 1/100 (sec), but 1/200 (sec)
Other times such as c) may be used, and t _f = 1/10 (sec)
However, other times such as 1/5 (sec) may be used.
_{Although s} = 1 (sec), other times such as 2, 3, etc. may be used, and t ₁ = t ₂ = t ₃ = 10 (sec).
₁ = t ₂ = t ₃ = 20 (sec) or t ₁ = 10
(Sec), t ₂ = 20 (sec), t ₃ = 30 (sec)
Other combinations such as c) may be used.

FIG. 2 is modified as shown in FIG. 5. In particular, step S2 in FIG. 2 is replaced with a process of setting zero as an initial value of the zero point correction value Δy (step S2 '), and step S12 is replaced by the latest. Let DV be the value of DV, and (d
-D ₀ ) is replaced with the zero point correction value Δy (step S12 ′), and step S6 is performed for each DV of the unit time T that does not overlap with the previous one used for the calculation of the vehicle direction. , the value y of the DV, y ^* = After correcting the zero point in ^{y-Δy, y * = bω} + d corresponding calculated angular velocity omega relational expression of _0, and the n partial averaging processing for obtaining an average angular velocity <omega> It may be replaced (Step S6 '). The other steps in FIG. 5 perform the same processing as in FIG. Further, in step S12 of FIG. 2 and step S12 ′ of FIG. 5, instead of setting the latest DV value to d, the latest k value is used.
The average value of DVs (for example, for one to several seconds) may be d.

[0023]

According to the present invention, the timing when the moving body is in a completely stationary state is determined, a zero point is determined from the output of the angular velocity detecting means at this time, and the output of the angular velocity detecting means is determined using the value of the zero point. , The angular velocity can be detected without error, or the angle viewed from a predetermined reference direction can be obtained without error.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a configuration of a vehicle direction detection system of an in-vehicle navigation device according to one embodiment of the present invention.

FIG. 2 is a flowchart showing a vehicle azimuth calculation process by the microcomputer in FIG. 1;

FIG. 3 is a diagram showing a relationship between a state of a change in an angular velocity and a determination condition of a completely stationary state.

FIG. 4 is a diagram showing a relationship between a state of a change in a vibration frequency and a determination condition of a completely stationary state.

FIG. 5 is a flowchart showing a modification of FIG. 2;

FIG. 6 is a block diagram showing a configuration of a vehicle azimuth detection system of a conventional vehicle-mounted navigation device.

FIG. 7 is a characteristic diagram showing a relationship between an angular velocity and a value of angular velocity detection data in an angular velocity detection unit in FIG. 6;

[Explanation of symbols]

 DESCRIPTION OF SYMBOLS 1 Angular velocity detector 2 Angular velocity sensor 3 Analog LPF 4 A / D converter 5A Microcomputer 6 Vehicle speed detector 7 Vibration detector

Claims (2)

[Claims] And detecting a voltage corresponding to an angular velocity of the moving body.
An angular velocity detecting device for a moving body including an angular velocity detecting means for A / D converting and outputting the detected voltage, comprising: a speed detecting means for detecting a speed of the moving body; a vibration detecting means for detecting a vibration frequency of the moving body; From the output of the angular velocity detecting means, the output of the velocity detecting means, and the output of the vibration detecting means, the state where the variation width of the angular velocity is within the predetermined constant value continues for the first predetermined constant time, and the speed is zero. Determining whether the state of the vibration frequency has continued for a second predetermined time and the state in which the vibration frequency is equal to or lower than a predetermined predetermined value has continued for a third predetermined time. A state in which the state is within the predetermined constant value for a first predetermined period of time, a state in which the speed is zero continues for a second predetermined time, and a state in which the vibration frequency is equal to or lower than the predetermined constant value Is determined to have continued for a third predetermined time. Correction means for determining a zero point from the output of the angular velocity detecting means at the time of separation and correcting the zero point for the output of the angular velocity detecting means by using the value of the zero point; . 2. A method for detecting a voltage corresponding to an angular velocity of a moving body,
A moving object angle detecting device comprising: an angular velocity detecting means for A / D converting the detected voltage and outputting the detected voltage; and a calculating means for calculating an angle of the moving object based on the output of the angular velocity detecting means. Speed detecting means for detecting the vibration frequency of the moving object, and the output of the angular velocity detecting means, the output of the speed detecting means, and the output of the vibration detecting means, the variation width of the angular velocity within a predetermined constant value The state in which the vibration frequency has entered the first predetermined time period, the state in which the speed is zero continues for the second predetermined time period, and the state in which the vibration frequency is equal to or lower than the predetermined value is the third predetermined time period. Determination means for determining whether or not the predetermined time has continued, wherein the arithmetic means has a state in which the change width of the angular velocity is within a predetermined constant value for a first predetermined time, and The state of zero speed is the second predetermined constant During continued, and the vibration frequency is a predetermined constant value following states third
A zero point is determined from the output of the angular velocity detecting means when it is determined that the predetermined time has elapsed, and the output of the angular velocity detecting means is corrected to a zero point using the value of the zero point to calculate the angle of the moving body. An angle detection device for a moving object, characterized in that: