JPH09292248A - Navigation system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To determine the position and speed of a vehicle with a simple structure using an acceleration sensor and two gyro sensor as an inertial navigation sensor. SOLUTION: A navigation calculating means 15 determines the position and speed of a vehicle from the outputs of each sensor (first acceleration sensor 1, second and third gyro sensors 5, 6) and a GPS receiver 7. A stoppage judging means 16 judges the stopping of the vehicle by the output of each sensor. The speed is determined from the acceleration obtained by the first acceleration sensor 1 and the inclination of a road surface obtained by the second gyro sensor 5. Further, the advancing direction is determined from the output of the third gyro sensor 6 to determine the position of the vehicle. A correction data calculating means 17 estimates the error contained in each sensor output. The navigation calculating means 15 performs the correction by the determined error, and also calculates the speed, position and advancing direction when the output from a sufficient number of satellites are received by the GPS receiver 7, and performs the direct substitution of this value or a correction so as to be close to this value.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a navigation device that calculates the position and speed of a vehicle.

[0002]

2. Description of the Related Art As a conventional navigation device using inertial navigation means, for example, (Reference 1) Journal of the Aerospace Society of Japan, Volume 41, No. 468 (January 1993), "GPS Navigation in Aerospace Research Institute" About flight experiments (Murata, Shingu, Ono, Zhang) "or Japanese Patent Laid-Open No. 5-248882. Hereinafter, an example of the navigation device will be described with reference to the drawings.

This type of navigation device is constructed as shown in FIG. In FIG. 5, 1, 2, 3
Are first, second, and third acceleration sensors that detect accelerations in three axial directions orthogonal to each other, and 4, 5, 6 are first, second, and third acceleration sensors that detect angular velocities around these three axes. A gyro sensor, 7 is a GPS receiver, 8 is a navigation calculation means, 9 is a vehicle stop detector, 10 is a vehicle stop determination means, and 11 is a correction data calculation means.

Next, the operation of the conventional navigation device configured as described above will be described. First,
The second and third acceleration sensors 1, 2 and 3 are fixed to the vehicle so as to detect the acceleration, and the first, second and third gyro sensors 4, 5 and 6 are fixed to the vehicle so as to detect the angular velocity around the axis. Installed. Here, each acceleration sensor and gyro sensor will be called an inertial navigation sensor. Further, as shown in FIG. 6, three axes orthogonal to each other are set to the traveling direction of the vehicle,
The vertical direction and the horizontal direction, and the directions of rotation about each axis are the rolling direction, the pitching direction, and the
We will call it the yawing direction.

The navigation calculation means 8 calculates the position and speed of the vehicle using the output values of these six inertial navigation sensors and the output result of the GPS receiver 7. The method of fixing the inertial navigation sensor to the moving body and calculating the position, speed, etc. is called the strapdown method.
It has already been commercialized and put into practical use in the aircraft field and will not be described in detail here.

Although the relative position and the relative velocity from the initial value can be obtained by the navigation calculation using the inertial navigation sensor, the initial position and the initial velocity are often unknown in advance.
The GPS receiver 7 is used to know the absolute position and the absolute speed. When the GPS receiver 7 can receive signals from a sufficient number of GPS satellites, the position and speed of the vehicle can be obtained.

Further, since the navigation calculation using the inertial navigation sensor is basically an integral calculation using the acceleration and the angular velocity obtained from each acceleration sensor and gyro sensor, the error contained in the output value of the sensor is accumulated. However, as a result, the position and speed of the vehicle diverge.

Therefore, it is appropriately included in the output value of the sensor based on the position and speed of the vehicle obtained from the GPS receiver 7 or the information "the vehicle is stopped" obtained from the vehicle stop determination means 10. It is necessary to estimate and correct the error and the error included in the position and speed of the vehicle. The correction data calculation means 11 calculates the data required for such correction.

The method of estimating the error is described in (Reference 1) and Japanese Patent Application Laid-Open No. 5-248882 and will not be described here any further.

Further, a vehicle stop detector 9 is provided for determining that the vehicle is stopped. In addition to the output of the vehicle stop detector 9, the vehicle stop determination means 10 includes an inertial navigation sensor (each acceleration sensor and gyro). By referring to the output value of the sensor), it is determined whether or not the vehicle is truly stopped.

Finally, in the navigation calculation means 8, the position and speed of the vehicle calculated using the inertial navigation sensor are corrected by the correction data calculated by the correction data calculation means 11, and the corrected vehicle is corrected. The position and speed of are output.

With the above configuration, the position and speed of the vehicle are obtained using the six inertial navigation sensors, and when the position and speed of the vehicle are obtained from the GPS receiver, it is determined that the vehicle is stopped. The error contained in the output value of the sensor and the error contained in the vehicle position, speed, etc.
Accurate vehicle position and speed can be obtained by performing the correction.

[0013]

However, in the structure of such a navigation device, as the inertial navigation sensor, the first, second, and third acceleration sensors and the first, second, and third acceleration sensors are used.
A total of six sensors, which are the third gyro sensor, are required, and as a result, the amount of calculation required for the navigation calculation increases. In addition, there is a problem that a special vehicle stop detector is required to determine whether the vehicle is stopped.

The present invention solves the above-mentioned problems of the prior art by reducing the number of inertial navigation sensors to eliminate the need for a vehicle stop detector, and the inclination sensor accurately determines the inclination angle of the road surface. An object of the present invention is to provide a navigation device capable of estimating and reducing the influence of an error included in the output value of an inertial navigation sensor.

[0015]

In order to achieve this object, a navigation device according to the present invention detects a first acceleration sensor for detecting an acceleration in a traveling direction of a vehicle and an angular velocity in a pitching direction of the vehicle. A second gyro sensor, a third gyro sensor that detects the angular velocity of the vehicle in the yaw ink direction, a GPS receiver or GLONASS receiver that receives a signal from a satellite, a first acceleration sensor, and the second and the second acceleration sensors. The vehicle stop determination means for determining that the vehicle is stopped using the output value of the gyro sensor of No. 3, the correction data calculation means for calculating the error included in the output value of each sensor, and the position and speed of the vehicle And a navigation calculation means for calculating. Further, it is configured to include an inclination sensor that detects an inclination angle of the vehicle in the pitching direction.

According to the above construction, the position and speed of the vehicle are calculated by a simple construction using only the first acceleration sensor and the second and third gyro sensors as the inertial navigation sensor, and the inertial navigation sensor The stop of the vehicle is determined based on the output value. Further, the inclination sensor that detects the inclination angle of the vehicle in the pitching direction can accurately estimate the inclination angle of the road surface and reduce the influence of an error included in the output value of the inertial navigation sensor.

[0017]

BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. 1 is a configuration diagram showing a navigation device according to a first embodiment of the present invention. Here, the same reference numerals are given to those having the same function and effect described in FIG. In FIG. 1, 1
Is a first acceleration sensor, 5 and 6 are second and third gyro sensors, 7 is a GPS receiver, 15 is a navigation calculation means, 16 is a stop determination means, and 17 is a correction data calculation means.

The operation of the navigation device constructed as above will be described. Further, the first acceleration sensor 1 can detect the acceleration in the traveling direction of the vehicle, the second gyro sensor 5 can detect the angular velocity in the pitching direction of the vehicle, and the third gyro sensor 6 can detect the angular velocity in the yawing direction of the vehicle. It is fixed to the vehicle.

First, the navigation calculation means 15 uses the output values of these three inertial navigation sensors (first acceleration sensor 1, second and third gyro sensors 5, 6) and the output result of the GPS receiver 7. Calculates the position and speed of the vehicle.

The method of calculating the position and speed of the vehicle will be described below. FIG. 2 is an explanatory diagram relating to the navigation calculation method according to the first embodiment, where the east is the horizontal plane with the x-axis and the north is the y-axis, and the traveling direction θa of the vehicle and the inclination angle θp of the road surface are defined in the traveling direction of the vehicle. .

At this time, the acceleration a (t) in the traveling direction of the vehicle
Is removed from the output value of the first acceleration sensor 1 by removing the influence of gravitational acceleration, and is calculated as in (Equation 1).

[0022]

[Formula 1] a (t) = ag ^ (t) −G × sin (θp (t)) a (t); acceleration in the traveling direction of the vehicle at time t ag ^ (t); Output value of acceleration sensor G: Gravity acceleration θp (t); Road surface inclination angle at time t Here, the road surface inclination angle θp (t) in (Equation 1) is obtained from (Equation 2).

[0023]

[Equation 2] θp (t) = θp (t-1) + wp ^ (t) × Δt θp (t); Road surface inclination angle wp ^ (t) at time t; Output of the second gyro sensor at time t Value Δt; In the sampling time (Equation 2), the initial value of the road surface inclination angle θp (t) is the average value of the output values of the first acceleration sensor 1 when it is determined that the vehicle is stopped. Calculate. The specific calculation method will be described later in the description of the correction data calculation means 17.

The velocity v (t) in the traveling direction of the vehicle is calculated by the following equation (3) using the acceleration a (t) in the traveling direction of the vehicle obtained from (equation 1).
To ask.

[0025]

## EQU00003 ## v (t) = v (t-1) + a (t) *. DELTA.t v (t); speed v (t) in the traveling direction of the vehicle at time t; acceleration .DELTA.t in the traveling direction of the vehicle at time t At the sampling time (Equation 3), the initial value of the speed v (t) in the traveling direction of the vehicle is set to 0 when it is determined that the vehicle is stopped, or the GPS receiver 7 outputs the signal from the satellite. It receives and sets the value when the speed of the vehicle is obtained.

The vehicle positions x (t) and y (t) are obtained as in (Equation 4).

[0027]

X (t) = x (t−1) + v (t) × cos (θp (t)) × cos (θa (t)) × Δ
ty (t) = y (t-1) + v (t) × cos (θp (t)) × sin (θa (t)) × Δ
tx (t), y (t); vehicle position v (t) at time t; velocity in the traveling direction of vehicle at time t θa (t); traveling direction of vehicle at time t Δt; sampling time (Equation 4) In, the initial position of the vehicle is set to a value when the GPS receiver 7 receives the signal from the satellite and the position of the vehicle is obtained. In addition, in (Formula 4), the term multiplied by cos (θp (t)) may be omitted in order to correct the influence of the inclination of the road surface.

In (Equation 4), the traveling direction θa (t) of the vehicle is
Calculated by (Equation 5).

[0029]

## EQU5 ## θa (t) = θa (t-1) + wa ^ (t) × Δt θa (t); traveling direction of vehicle at time t wa ^ (t); output of third gyro sensor at time t Value Δt; in the sampling time (Equation 5), the initial value of the traveling direction θa (t) is set to a value when the GPS receiver 7 receives a signal from the satellite and the traveling direction of the vehicle is obtained.

In this way, the navigation calculation means 15 can determine the vehicle position x (t), y (t) and the speed v (t).

Next, the vehicle stop determination means 16 uses only the output values of the three inertial navigation sensors of the first acceleration sensor 1, the second gyro sensor 5, and the third gyro sensor 6,
Determine if the vehicle is stopped. Here, an example of a method of determining a stop will be described. FIG. 3 is a flowchart showing a process related to the vehicle stop determination method according to the first embodiment.

First, the time .DELTA.T is determined as an interval for judging a vehicle stop. That is, it is determined whether or not the vehicle is stopped at each time ΔT. Here, the differences D1 to D3 between the maximum value and the minimum value of the output value of each sensor during ΔT are considered. In general, the value of the difference D is small when the vehicle is stopped and is large when the vehicle is moving. Therefore, threshold values Ds1 to Ds3 of the values of the differences D1 to D3 are provided for each sensor. For each interval ΔT, the values of the differences D1 to D3 of the respective sensors are obtained, and it is judged whether these values are larger or smaller than the respective thresholds Ds1 to Ds3. When it is smaller than the threshold value Ds, it is determined that the vehicle is stopped.

The stop can be determined by the above method. Note that the difference between the maximum value and the minimum value of the output value of each sensor during the interval ΔT was considered, but the determination may be made using the variance value of each sensor. Further, the determination of the vehicle stop may be performed by using only some output values instead of using all the output values of the three sensors. Furthermore, the output result of the GPS receiver may be referred to when determining whether the vehicle is stopped.

Next, in the correction data calculation means 17, the second,
The offset error included in the output values of the third gyro sensors 5, 6 is estimated, and the inclination angle of the road surface, the vehicle speed,
The correction data for correcting the position etc. is obtained. Further, the navigation calculation means 15 uses the correction data calculated by the correction data calculation means 17 to subtract the offset error from the output value of each sensor, or the vehicle calculated using (Equation 1) to (Equation 5). The position and speed of the vehicle are corrected and the corrected position and speed of the vehicle are output.

The method of calculating the correction data in the correction data calculating means 17 and the correction method in the navigation calculating means 15 will be described below.

First, the second and third gyro sensors 5, 6
The method of estimating the offset error included in the output value of is described. The offset error included in the output values of the second and third gyro sensors 5 and 6 is estimated using the output values of the respective sensors when it is determined that the vehicle is stopped. If the vehicle stop determination means 16 determines that the vehicle is stopped from the time ta to the time tb, the second gyro sensor 5,
The offset errors wp * and wa * included in the output value of the third gyro sensor 6 are, as shown in (Equation 6), as an average value of the output values of the sensors when it is determined that the vehicle is stopped. You can ask.

[0037]

(Equation 6)

The offset error of the sensor estimated in this way is used for correction calculation in the navigation calculation means 15. That is, wp ^ (t), wa ^ in (Equation 2) and (Equation 5)
wp (t), wa (t) calculated by (Equation 7) instead of (t)
Is used.

[0039]

[Formula 7] wp (t) = wp ^ (t) −wp * wa (t) = wa ^ (t) −wa * wp (t); Output of the second gyro sensor after offset error correction at time t Value wa (t); Output value of third gyro sensor after offset error correction at time t wp ^ (t); Output value of second gyro sensor at time t wa ^ (t); Third value at time t Gyro sensor output value wp *; second gyro sensor offset error estimated value wa *; third gyro sensor offset error estimated value Next, the road surface inclination angle θp ( t) is corrected using the inclination angle θp * calculated from the output value of the first acceleration sensor 1 when the vehicle is stopped. Time t
If it is determined that the vehicle is stopped from a to time tb, θp * can be calculated by (Equation 8).

[0040]

(Equation 8)

When it is judged that the vehicle is stopped and θp * is calculated by (Equation 8), the value of θp * is substituted for the road surface inclination angle θp (t) obtained from (Equation 2). , Or correct it so that it approaches the value of θp *.

Next, the speed v (t) in the traveling direction of the vehicle obtained from (Equation 3), and the position x of the vehicle obtained from (Equation 4)
Vehicle heading θa calculated from (t), y (t), (Equation 5)
In (t), the output result of the GPS receiver 7 is used for correction. When the GPS receiver 7 can receive signals from a sufficient number of GPS satellites, the GPS receiver 7 can calculate the speed, position, and heading of the vehicle. Therefore, when these values are obtained from the GPS receiver,
These values are directly substituted, or correction is performed so as to approach these values.

Further, the speed v (t) in the traveling direction of the vehicle is corrected by setting the value of v (t) to 0 when the vehicle stop determination means 16 determines that the vehicle is stopped. .

Thus, as the inertial navigation sensor, the first
Acceleration sensor 1, second and third gyro sensors 5, 6
The position and speed of the vehicle can be calculated with a simple configuration using only.

FIG. 4 is a block diagram showing a navigation device according to the second embodiment of the present invention. In FIG. 4, the same reference numerals are given to those having the same function and effect described in FIG. 1, and reference numeral 18 denotes a tilt sensor. In the second embodiment, the inclination sensor 18 is further provided in the configuration of the first embodiment. The operation of the navigation device configured as described above will be described.

The first acceleration sensor 1, the second gyro sensor 5, the third gyro sensor 6, and the GPS receiver 7 have the same functions as those in the first embodiment, and the description thereof is omitted here. . The inclination sensor 18 is fixed to the vehicle so that the inclination angle of the vehicle in the pitching direction, that is, the inclination angle of the road surface can be detected. In the navigation calculation means 15, the first
In addition to the acceleration sensor 1, the second gyro sensor 5, and the third gyro sensor 6 of FIG.
The position and speed of the vehicle are calculated using the output result of the S receiver 7.

The vehicle position / speed calculation method is basically performed using the formulas (Equation 1) to (Equation 5) described in the first embodiment. However, the inclination angle θp of the road surface in (Equation 2)
In the calculation of (t), the average value of the output values of the inclination sensor 18 when it is determined that the vehicle is stopped is used as the initial value of the inclination angle of the road surface.

The vehicle stop determination means 16 determines the vehicle stop using the output values of the inclination sensor 18 in addition to the first acceleration sensor 1, the second and third gyro sensors 5 and 6. You may do it.

Next, the correction data calculation means 17 calculates the offset error of the first acceleration sensor 1 in addition to the correction data calculation described in the first embodiment. The method of calculating the offset error of the first acceleration sensor 1 will be described below.

The offset error of the first acceleration sensor 1 is the output value a ^ (t) of the first acceleration sensor 1 and the output value θk of the tilt sensor 18 when it is determined that the vehicle is stopped. Calculate using ^ (t). When the vehicle stop determination means 16 determines that the vehicle is stopped between the time ta and the time tb, the offset error of the first acceleration sensor 1
ag * can be calculated by (Equation 9).

[0051]

[Equation 9]

The offset error of the first acceleration sensor 1 calculated in this way is used in the navigation calculation means 15 for correction calculation. That is, ag ^ in (Equation 1)
Use ag (t) calculated by (Equation 10) instead of (t).

[0053]

## EQU10 ## ag (t) = ag ^ (t) -ag * ag (t); output value of the first acceleration sensor after offset error correction at time t ag ^ (t); first at time t Accelerometer output value ag *; Estimated value of offset error of the first accelerometer Also, the value of road surface inclination angle θp (t) obtained from (Equation 2) is the average of the output values of the inclination sensor 18 when the vehicle is stopped. Correct using the value. That is, when it is determined that the vehicle is stopped, the average value of the output values of the tilt sensor 18 in the stopped state is substituted for the road inclination angle θp (t), or correction is performed so as to approach the average value. .

As described above, by further adding the inclination sensor 18, the offset error of the first acceleration sensor and the error due to the inclination angle of the road surface can be reduced, and more accurate vehicle position and speed can be obtained. It can be calculated.

[0055]

As described above, according to the present invention,
The position and speed of the vehicle can be calculated with a simple structure using an acceleration sensor and two gyro sensors as the inertial navigation sensor, or the position and speed of the vehicle can be calculated with higher accuracy by adding a tilt sensor. It has the effect of being able to.

[Brief description of drawings]

FIG. 1 is a configuration diagram showing a navigation device according to a first embodiment of the present invention.

FIG. 2 is an explanatory diagram related to a navigation calculation method according to the first embodiment.

FIG. 3 is a flowchart showing processing relating to a vehicle stop determination method according to the first embodiment.

FIG. 4 is a configuration diagram showing a navigation device according to a second embodiment of the present invention.

FIG. 5 is a configuration diagram showing a conventional navigation device.

FIG. 6 is a diagram showing a relationship among a rolling direction, a pitching direction, and a yawing direction with respect to three axes of a vehicle traveling direction, a longitudinal direction, and a lateral direction.

[Explanation of symbols]

1 ... 1st acceleration sensor, 2 ... 2nd acceleration sensor,
3 ... 3rd acceleration sensor, 4 ... 1st gyro sensor, 5 ... 2nd gyro sensor, 6 ... 3rd gyro sensor, 7 ... GPS receiver, 8, 15 ... Navigation calculation means, 9 ... Stop detection Container, 10, 16 ... Stop determination means, 1
1, 17 ... Correction data calculation means, 18 ... Inclination sensor.

 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Hiroaki Tsuji 4-3-1, Tsunashima-higashi, Kohoku-ku, Yokohama-shi, Kanagawa Matsushita Communication Industrial Co., Ltd. (72) Inventor Masaru Mizuno Tsunashima-higashi, Kohoku-ku, Yokohama-shi, Kanagawa 4th, 3rd, 1st, Matsushita Communication Industrial Co., Ltd.

Claims (2)

[Claims] 1. A first device for detecting an acceleration in a traveling direction of a vehicle.
Acceleration sensor, a second gyro sensor that detects the angular velocity of the vehicle in the pitching direction, and a third gyro sensor that detects the angular velocity of the vehicle in the yawing direction,
GPS receiver or GLON that receives signals from satellites
ASS receiver, the first acceleration sensor, the second,
A vehicle stop determination unit that determines whether the vehicle is stopped using the output value of the third gyro sensor, a correction data calculation unit that calculates an error included in the output value of each sensor, and the position of the vehicle. And a navigation calculation means for calculating the speed, and calculates the position and speed of the vehicle. 2. The navigation device according to claim 1, wherein the navigation device includes an inclination sensor that detects an inclination angle of the vehicle in a pitching direction, and calculates the position and speed of the vehicle with higher accuracy.