JP4941199B2 - Navigation device - Google Patents

Description

  The present invention relates to a navigation apparatus for guiding a current position.

  There are two types of navigation devices that measure the position of a vehicle and provide travel guidance: a stationary type that is installed by wiring or the like in the vehicle, and a portable type that is called a PND (Personal Navigation Device). In recent years, in a stationary navigation device (see, for example, Patent Document 1), in addition to position information acquired based on GPS signals, a direction obtained from a built-in gyro sensor or a geomagnetic sensor, and a vehicle speed obtained from a vehicle speedometer included in the vehicle By using this information in combination, a device equipped with a function for obtaining the current position of the vehicle with high accuracy has been put into practical use. Such a function is particularly useful when the GPS signal cannot be acquired or the accuracy of the GPS signal is not good.

On the other hand, in a portable navigation device, it is not easy to obtain vehicle speed information from a vehicle speedometer installed inside a vehicle instrument panel, etc., so as an auxiliary means for cases where GPS signals cannot be obtained, etc. Conventionally, a method has been adopted in which vehicle speed is obtained by integrating acceleration measured using a built-in acceleration sensor, thereby obtaining a position.
JP-A-9-42979

  However, since an acceleration sensor generally has an offset error, there is a serious problem that a sufficiently good accuracy cannot be obtained in position measurement. Here, the offset error is an error in which a certain finite acceleration value is output from the acceleration sensor even though the acceleration due to the acceleration motion is originally zero, and the offset error is included in each acceleration sensor. In addition to the inherent electrical offset (that is, due to individual differences), the acceleration of the acceleration sensor is misaligned from the desired direction with respect to the vertical direction during measurement. There is an offset due to The influence of the former offset error can be reduced relatively easily by selecting an acceleration sensor.

  However, there has been no effective method for measuring the latter offset error. For example, the output value of the acceleration sensor that is output when the vehicle is stopped on a horizontal road can be regarded as the offset error, but the value of the offset error changes when the vehicle is tilted from a horizontal surface such as a slope. It is not possible to know how much the exact offset error is. As an example of the influence of the offset error on the positioning accuracy, if the acceleration sensor is tilted at an angle of 0.1 °, an error in the position of about 100 meters will occur after 100 seconds. Is important to.

  The present invention has been made in view of the above points, and an object of the present invention is to provide a navigation device that can correct an offset error of an acceleration sensor and thereby perform positioning using the acceleration sensor with high accuracy. It is to provide.

The present invention has been made in order to solve the above-described problems, and includes first measurement means for obtaining a position by GPS and obtaining a moving speed from a plurality of positions at the obtained different times, and acceleration obtained from an acceleration sensor. A navigation device that performs position guidance, and a second measuring unit that obtains a moving speed and a position based on the corrected acceleration and an azimuth obtained from an azimuth sensor. The position and moving speed obtained by the first measuring means and the position and moving speed obtained by the second measuring means are input to the Kalman filter to estimate the best value regarding the current most likely position. Based on the best value estimating means and the best value estimated by the best value estimating means, the second measuring means obtains it. An acceleration offset calculation means for calculating the acceleration offset value for correcting the amount corresponding acceleration of the offset error as the error of the position is that due to the offset error of the acceleration sensor, by the first measuring means An accuracy determination unit that determines the accuracy of the measurement result, and the second measurement unit using the best value estimated by the best value estimation unit when the accuracy determination unit determines that the accuracy is good. A position correction unit that corrects the position obtained by the step (1) and outputs the current position as the current position, and outputs the position obtained by the second measuring means as the current position when it is determined that the accuracy is not good. The acceleration offset calculating means includes the best value only when the accuracy determining means determines that the accuracy is within a predetermined good range. A navigation apparatus and calculates the acceleration offset based.

  In this invention, the best value related to the position is obtained based on the position and moving speed obtained from the acceleration sensor and the azimuth sensor, and the position and moving speed obtained from the GPS, and the acceleration offset value is calculated using the obtained best value. The acceleration from the acceleration sensor is corrected by this acceleration offset value. Therefore, since the acceleration offset value is calculated from data (acceleration sensor, azimuth sensor, position from GPS, and moving speed) measured even while actually traveling, the offset error of the acceleration sensor is always calculated. It is possible to correct. As a result, positioning using the acceleration sensor can be performed with high accuracy.

  According to the present invention, in the navigation device, the best value estimating means includes a difference between a position obtained by the first measuring means and a position obtained by the second measuring means, and the first The best value of these differences is estimated from the difference between the moving speed obtained by the measuring means and the moving speed obtained by the second measuring means.

  According to the present invention, since the best value is estimated from the difference between the measurement data of the first measurement means and the second measurement means, when the best value is estimated compared to the case where each measurement data is handled independently. The amount of computation can be reduced.

  The navigation apparatus may further include an accuracy determination unit that determines an accuracy of a measurement result obtained by the first measurement unit, and the acceleration offset calculation unit may have a predetermined accuracy good by the accuracy determination unit. The acceleration offset value is calculated based on the best value only when it is determined to be within the range.

  In the present invention, the acceleration offset value is calculated if the accuracy of the measurement result by GPS is good, and the acceleration offset value is not calculated if the accuracy is not good. Thereby, the correction of the acceleration is performed only when the accuracy of the measurement result by GPS is good. Usually, the measurement results by GPS differ in accuracy depending on, for example, the moving speed, but if this accuracy is poor, the accuracy of the estimated best value is also degraded, and the accuracy of the acceleration offset value is also degraded. Therefore, the acceleration correction can be made reliable by correcting the acceleration only when the accuracy of the GPS is good, that is, when an accurate acceleration offset value is obtained.

  In the navigation apparatus according to the present invention, the accuracy determination unit determines that the accuracy is good when the moving speed obtained by the first measurement unit is equal to or higher than a predetermined value. .

  In the present invention, when the moving speed is slow (when it is stopped as an extreme case), the influence of the position measurement error due to GPS on the speed error is significant, whereas when the movement speed is fast, the position measurement by GPS is performed. Considering that the influence of the error on the speed error becomes relatively thin, it is determined that the accuracy of the GPS is good when the moving speed is fast, and the acceleration offset value is calculated only in this case. Therefore, since the acceleration is corrected based on the accurate acceleration offset value, the acceleration correction can be made reliable.

  Further, according to the present invention, in the navigation device, the accuracy determination unit calculates an angle of change in the traveling direction from a plurality of positions at different times obtained by the first measurement unit, and the calculated angle is a predetermined value. It is characterized in that it is determined that the accuracy is good when the value is less than or equal to the value.

  In the present invention, when the position measurement error by GPS is large, considering that the traveling direction obtained from the measured position changes rapidly, the accuracy of GPS is good when the traveling direction changes at a small angle. Therefore, the acceleration offset value is calculated only in this case. Therefore, since the acceleration is corrected based on the accurate acceleration offset value, the acceleration correction can be made reliable.

  According to the present invention, it is possible to correct an offset error of an acceleration sensor, and thereby it is possible to perform positioning using the acceleration sensor with high accuracy.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
FIG. 1 is a configuration diagram of a navigation apparatus 1 according to the first embodiment of the present invention. The navigation device 1 includes a CPU 10, a GPS unit 11, an acceleration sensor 12, a magnetic sensor 13, a memory 14, an external storage device 15, a communication unit 16, a display device 17, and an audio output device 18. Each part is connected by a bus line 19. These parts of the navigation device 1 are accommodated in a single casing, and the navigation device 1 can be carried and attached to any position in the vehicle. Henceforth, the navigation apparatus 1 shall be attached in the vehicle.

  The CPU (central processing unit) 10 reads a program stored in the memory 14, controls and controls each part of the navigation device 1 according to the read program, and obtains it from the GPS unit 11, the acceleration sensor 12, and the magnetic sensor 13. The current position is estimated from the obtained data, and the calculation process described later (FIGS. 2 to 4) for correcting the output of the acceleration sensor 12 using the estimation result and the navigation function for guiding the current position to the user are realized. Execute information processing.

  The GPS unit 11 measures the position of the vehicle (latitude and longitude) by the GPS (Global Positioning System) method, and calculates the vehicle speed based on the latest measured position and the position immediately before it. Information indicating the position and speed is output to the CPU 10. In the GPS system, a GPS signal from a GPS satellite is received, and the position is measured by arithmetic processing using data of a reception time (timed by a time measuring unit (not shown)) when the GPS signal is received.

  The acceleration sensor 12 measures the acceleration of the vehicle (that is, the navigation device 1) and outputs acceleration information representing the measurement result to the CPU 10. The measured acceleration includes gravitational acceleration due to the vehicle leaning on a slope or the like in addition to acceleration due to the vehicle performing acceleration motion. As the acceleration sensor 12, a two-axis acceleration sensor that detects acceleration components in two different directions perpendicular to each other is used. In the present embodiment, one axis of the acceleration sensor 12 is the vehicle front-rear direction (front of the vehicle). In the positive direction of the y-axis), and the other axis is oriented in the left-right direction of the vehicle (the right side of the vehicle is the positive direction of the x-axis). The acceleration information output to the CPU 10 is an acceleration component in each axial direction, that is, an acceleration component in the front-rear direction of the vehicle and an acceleration component in the left-right direction of the vehicle. Each component includes an offset error including an offset due to the measurement of gravitational acceleration and an electrical offset of the acceleration sensor 12.

The magnetic sensor 13 measures the geomagnetism in the vehicle (that is, the navigation device 1), calculates the direction in which the predetermined reference axis defined by the magnetic sensor 13 is directed, and outputs the direction information indicating the direction to the CPU 10. . In this embodiment, the magnetic sensor 13 shall be arrange | positioned so that this reference axis may face the vehicle front direction. As the magnetic sensor 13, either a two-axis magnetic sensor that detects magnetic components in two different directions perpendicular to each other or a three-axis magnetic sensor that detects magnetic components in three different directions perpendicular to each other is used. For example, in the case of a biaxial magnetic sensor, two elements (magnetoresistive elements) for detecting the magnitude of magnetism are arranged in two different directions, and based on the magnitude of geomagnetism detected by each element. The direction of the reference axis (vehicle front) is calculated.
The y axis of the acceleration sensor 12 and the reference axis of the magnetic sensor 13 need to be parallel to each other, that is, in the same direction. However, the directions may not be the front direction of the vehicle as described above. It is good also as a direction.

The memory 14 includes a ROM (read only memory) and a RAM (anytime read / write memory). In the ROM, a program executed by the CPU 10 is stored. The RAM is used as a storage area for storing temporary data during program execution.
The external storage device 15 is a large-capacity storage device such as a hard disk drive, and stores map information and the like necessary for navigation.
The communication unit 16 is connected to the Internet, for example, by wireless communication, and acquires the latest map information and the like via the Internet. The acquired map information and the like are stored in the external storage device 15.

The display device 17 graphically displays the travel guidance by the navigation function realized by the CPU 10 in a form that shows the current position and direction of the vehicle on a map, for example.
The voice output device 18 generates voice guidance voice such as turning an intersection at a necessary timing and performs voice guidance from a speaker.

  Next, processing in which the navigation apparatus 1 estimates the current position from data acquired from the GPS unit 11, the acceleration sensor 12, and the magnetic sensor 13 and corrects the output of the acceleration sensor 12 using the estimation result will be described. FIG. 2 is a diagram showing functional blocks related to the process of the navigation device 1, and the process includes an INS system positioning unit 101 (the acceleration sensor 12 and the magnetic sensor 13 are called an INS (Internal Navigation System) system). The best value estimation unit 102, the position correction unit 103, the acceleration offset calculation unit 104, and the accuracy determination unit 105 are realized. Note that the functions of these units are realized by the CPU 10 reading and executing the program stored in the memory 14 as described above.

  An acceleration a = (ax, ay) including an offset error is input from the acceleration sensor 12 to the INS system positioning unit 101 (ax is an x-axis component, ay is a y-axis component), and the magnetic sensor 13 An azimuth θ indicating the direction in which the front is facing is input. The input of the acceleration a and the direction θ is sequentially performed at a predetermined time interval (for example, several times per second). Further, the acceleration offset value b = (bx, by) for correcting the offset error included in the acceleration a from the acceleration sensor 12 is supplied from the acceleration offset calculation unit 104 to the INS positioning unit 101 at a predetermined time interval ( It is a time interval in which the update described below is performed, and is input at, for example, every second (bx is an x-axis component, and by is a y-axis component).

The INS positioning unit 101 calculates the vehicle position P _INS and the speed V _INS according to the following equations using each of the input data, and outputs the calculation results to the best value estimation unit 102 and the position correction unit 103. .
aN = − (ax + bx) · sin θ + (ay + by) · cos θ
aE = (ax + bx) · cos θ + (ay + by) · sin θ
VN _INS = VN0 _INS + aN · Δt
VE _INS = VE0 _INS + aE · Δt
PN _INS = PN0 _INS + sin ⁻¹ [(VN _INS × Δt) / R]
PE _INS = PE0 _INS + sin ⁻¹ [(VE _INS × Δt) / {R × cos (PN0 _INS )}]
Where aN and aE are the latitude and longitude components of acceleration a + b, VN _INS and VE _INS are the latitude and longitude components of velocity V _INS , and PN _INS and PE _INS are the latitude and longitude components of position P _INS , respectively. . Further, VN0 _INS , VE0 _INS , PN0 _INS , and PE0 _INS are values of VN _INS , VE _INS , PN _INS , and PE _INS calculated previously, respectively, and Δt is an elapsed time from the previous calculation to the current calculation.

  Here, as shown in the above formula, the INS positioning unit 101 obtains the acceleration a from the acceleration sensor 12 from the acceleration offset calculation unit 104 by adding the acceleration a and the acceleration offset value b. Correction is performed using the acceleration offset value b. The acceleration offset value b used for this correction is updated in the acceleration offset calculation unit 104 every time there is a new input from the best value estimation unit 102. Therefore, the acceleration a from the acceleration sensor 12 is always appropriately corrected by the acceleration offset value b that is updated as needed.

The calculated value P _INS and the velocity V _INS are input from the INS system positioning unit 101 to the best value estimation unit 102, and the position P _GPS and the position P _GPS at predetermined time intervals (for example, every second) are input from the GPS unit 11. Speed V _GPS is input. The position P _INS and velocity V _INS calculated by the INS system positioning unit 101 and the position P _GPS and velocity V _GPS obtained from the GPS unit 11 are different from each other because their measurement principles are different. Each value has an inherent error. The best value estimation unit 102 is used to estimate the most probable value from such input values.

The best value estimation unit 102 calculates the input position difference δP ′ = P _GPS −P _INS and the input speed difference δV ′ = V _GPS −V _INS every time there is an input from the GPS unit 11. Based on the differences δP ′ and δV ′, δP and δV which are the most probable values (best values) are estimated. It is assumed that δP and δV are estimated by calculation using a Kalman filter. Details of the calculation will be described later. Best value estimation section 102 outputs best values δP and δV obtained by the estimation to position correction section 103 and acceleration offset calculation section 104.

Position correction unit 103, based on the best value δP of the difference between the position input from the position of the vehicle _{P INS} and the optimum value estimation unit 102 which is input from the INS positioning unit 101, corrects the position _{P INS} in accordance with the following formula To calculate the current most probable position P.
P = P _INS + δP
Further, the position correction unit 103 outputs the calculated position P to the navigation software (CPU 10) when the accuracy of measurement by the GPS unit 11 is sufficiently good in response to an instruction from the accuracy determination unit 105. If the accuracy of measurement by the GPS unit 11 is not sufficiently good, the position P _INS input from the INS system positioning unit 101 is used as an output to the navigation software (CPU 10) as it is.

In the navigation device 1 of the present embodiment, the (final) current position of the vehicle is obtained from the result of positioning by the INS system, and the measurement result by the GPS unit 11 is an acceleration a obtained from the INS system. And the position P _INS are used for the purpose of correcting. Therefore, as described above, when the accuracy of the GPS unit 11 is good, the position P _INS is also corrected with good accuracy, so the position P is output, and when the accuracy of the GPS unit 11 is not good, correction is not performed. The position P _INS is output as it is.

  The acceleration offset calculation unit 104 is a correction value for correcting an offset error included in the acceleration a output from the acceleration sensor 12 based on the best value δP of the position difference input from the best value estimation unit 102. An acceleration offset value b is calculated. Here, a calculation formula for obtaining the acceleration offset value b from the best position difference δP is determined based on the following assumptions.

That is, if the acceleration a from the acceleration sensor 12 has been completely corrected (that is, offset error is eliminated) last time in the INS system positioning unit 101, the best value of the position difference obtained by the best value estimating unit 102 is obtained. δP should be zero. On the other hand, if the correction of the acceleration a is incomplete (that is, an offset error remains), the position P calculated by the INS system positioning unit 101 by the incomplete amount of acceleration (referred to as b ′). _An error occurs in the _INS, and the best value δP of the position difference having a value corresponding to the error should be obtained from the best value estimation unit 102. Therefore, the output δP of the best value estimation unit 102 is a position error caused by the uniform acceleration motion at the acceleration b ′.
δP = (1/2) · b ′ · ΔT ²
Holds.

Therefore, the calculation formula for the acceleration offset calculation unit 104 to calculate the acceleration offset value b is
b = b0 + 2 · δPxy / ΔT ²
And Here, b0 is an acceleration offset value used for the previous acceleration correction in the INS positioning unit 101, and ΔT is an elapsed time from the previous acceleration correction to the current time. Further, δPxy is obtained by converting the best value δP expressed by the latitude and longitude obtained from the best value estimation unit 102 into an xy coordinate system fixed to the vehicle (rotation of the azimuth θ).

  The acceleration offset calculation unit 104 calculates the acceleration offset value b every time the best value δP is input from the best value estimation unit 102 according to the above formula, and outputs the calculated acceleration offset value b to the INS system positioning unit 101. In this way, the acceleration offset value b is updated as needed, and the acceleration a of the acceleration sensor 12 is corrected in the INS system positioning unit 101 by the updated acceleration offset value b. As a result, the correction of the acceleration a is always performed in an appropriate state.

  The accuracy determination unit 105 determines whether or not the accuracy of measurement by the GPS unit 11 is sufficiently good, and notifies the position correction unit 103 and the best value estimation unit 102 of the result. In accordance with this notification, best value estimating section 102 estimates best values δP and δV only when it is determined that the accuracy of measurement by GPS section 11 is sufficiently good, and this estimation is performed when the accuracy is not good. Cancel. By obtaining the best value δP only when the measurement accuracy of the GPS unit 11 is good and outputting it to the acceleration offset calculation unit 104, the accurate acceleration offset value b can be obtained from the accurate best value δP.

When the accuracy determination unit 105 determines the accuracy, the speed V calculated based on the position P _GPS obtained from the GPS unit 11 and the angle α in which the traveling direction changes are used. Position measurement by GPS generally involves an error of a predetermined size, but when the speed is slow, the influence of the position measurement error on the speed error is significant, whereas when the speed is fast, the influence is relatively Therefore, the accuracy determination unit 105 determines that the accuracy of measurement by the GPS unit 11 is sufficiently good when the speed V is equal to or greater than a predetermined value. In addition, when the position measurement error by GPS is large, the traveling direction obtained from the position P _GPS changes abruptly. It is determined that the accuracy of measurement by the unit 11 is sufficiently good.

  Next, a specific example of the operation of the navigation device 1 will be described. FIG. 3 is a flowchart showing an operation example of the navigation device 1.

First, the CPU 10 acquires the vehicle position P _GPS and the speed V _GPS from the GPS unit 11 and stores them in the memory 14 (step S1), and further acquires the acceleration a including an offset error from the acceleration sensor 12 and magnetically. The azimuth θ indicating the front direction of the vehicle is obtained from the sensor 13, and each is stored in the memory 14 (step S2). Next, the INS system positioning unit 101 corrects the acquired acceleration a by the acceleration offset value b held in the memory 14, and based on the corrected acceleration a + b and the acquired orientation θ, the vehicle position P _INS and The speed V _INS is calculated (step S3). The acceleration offset value b is an initial value b = (0, 0) when step S3 is executed for the first time, and is a value calculated in step S11 after executing step S11 described below. These are memorized.

Next, the CPU 10 determines whether or not the position P _GPS and the speed V _GPS have been updated (step S4). As will be described later, step S1 is repeated at time Δt _GPS intervals, and step S2 is repeated at time Δt _INS (<Δt _GPS ) intervals. When time Δt _GPS has not yet elapsed, position P _GPS and speed are repeated. Since V _GPS is not updated, the determination in step S4 is NO.

In this case, the position correction unit 103 calculates the current most probable vehicle position P by correcting the position P _INS calculated in step S3 using the latest best value δP held in the memory 14. (Step S5). The best value δP is calculated and stored in the memory 14 at the previous execution of step S10 described below. In this manner, the position correction unit 103 outputs the position P corrected from the position P _INS calculated by the INS system positioning unit 101 to the navigation software (step S6).

Subsequently, CPU 10 determines whether the elapsed time Delta] t _GPS (step S7), and the process is repeated from step S1 once again the time Delta] t _GPS has elapsed. If not elapsed time Delta] t _GPS is still, CPU 10 determines whether or not next time Delta] t _INS has elapsed (step S8), and repeats the process from step S2 again when the time Delta] t _INS has elapsed. In this way, the data from the GPS unit 11 is updated at the time Δt _GPS interval and the INS data is updated at the time Δt _INS interval until the data from the GPS unit 11 is updated. The position P is output at time Δt _INS intervals. As an example, Δt _GPS = 1 second and Δt _INS = 0.2 seconds.

When the data from the GPS unit 11 is updated, the determination in step S4 is YES. In this case, the accuracy determination unit 105 determines whether or not the accuracy of measurement by the GPS unit 11 is sufficiently good (step S9). This determination process will be described later with reference to FIG. If it is determined that the accuracy is sufficiently good, the best value estimation unit 102 calculates the difference δP ′ = P _GPS −P _INS and δV ′ = V _GPS calculated from the position and velocity obtained in steps S1 and S3. Based on −V _INS , their best values δP and δV are estimated (step S10). This estimation result is stored in the memory 14.

  Next, the acceleration offset calculation unit 104 calculates the acceleration offset value b from the obtained best value δP (step S11). Thus, every time new data with sufficiently good accuracy is obtained from the GPS unit 11, the acceleration offset value b is updated. The acceleration offset value b calculated here is stored in the memory 14 and used to correct the acceleration a in step S3 as described above. Therefore, since the correction is performed using the acceleration offset value b that is sequentially updated, the acceleration a including the offset error acquired from the acceleration sensor 12 can always be appropriately corrected.

After step S11, the position correction unit 103 calculates the current most probable vehicle position P using the best value δP estimated in step S10 (step S5). The position correction unit 103 outputs a position P obtained by correcting the position P _INS calculated by the INS system positioning unit 101 even when sufficiently accurate data is obtained from the GPS unit 11 in this way ( Step S6).

On the other hand, if it is determined in step S9 that the accuracy of measurement by the GPS unit 11 is not sufficiently good, the best values δP and δV in step S10 and the acceleration offset value b in step S11 are not calculated. The position correction unit 103 outputs the position P _INS calculated in step S3 as it is to the navigation software (step S12). For this reason, since the acceleration offset value b with poor accuracy is not obtained, the correction of the acceleration a in the INS system positioning unit 101 becomes reliable.

  Next, the specific process of step S9 described above will be described. FIG. 4 is a flowchart illustrating an example of processing executed by the accuracy determination unit 105.

First, the accuracy determination unit 105 determines whether or not the position P _GPS acquired from the GPS unit 11 is data indicating that the position is indefinite (step S91). Here, from the GPS unit 11, when GPS signals can be received, data indicating latitude and longitude is output as the position P _GPS , and when GPS signals cannot be received, data indicating that the position is indefinite is output to the position P. It shall be output as _GPS . When the position P _GPS is data indicating that the position is indefinite, the accuracy determination unit 105 clears the position P _GPS stored in the memory 14 and the measurement accuracy by the GPS unit 11 is not sufficiently good. Judgment is made (step S98).

If the position P _GPS is data indicating latitude and longitude, the accuracy determination unit 105 next determines whether or not a predetermined number (for example, three) of the position P _GPS acquired from the GPS unit 11 is stored in the memory 14. Is determined (step S92). Here, in the flowchart of FIG. 3, the position P _GPS is sequentially updated every time step S <b> 1 is executed, and the latest three positions P _GPS and the measurement time information thereof are stored in the memory 14. When the number of memories is not yet three, the accuracy determination unit 105 determines that the accuracy of measurement by the GPS unit 11 is not sufficiently good (step S98).

If the number of stored positions P _GPS is 3, the accuracy determination unit 105 determines the vector connecting the position P _GPS (2) and the position P _GPS (1), the position P _GPS (1), and the position P _GPS (0). Is calculated (step S93), and the moving speed V from the position _PGPS (1) to the position _PGPS (0) is calculated (step S94). Here, the position P _GPS (0) is the position measured in the current measurement, the position P _GPS (1) is the position measured in the previous measurement, and the position P _GPS (2) is in the previous measurement. This is the measured position. The angle α represents an angle at which the traveling direction changes. The moving speed V is obtained by dividing the moving distance from the position P _GPS (1) to the position P _GPS (0) by the difference between the respective measurement times.

  Next, the accuracy determination unit 105 determines whether or not the angle α is equal to or less than a predetermined value (for example, 90 degrees) (step S95). If the angle α is larger than the predetermined value, it means that the traveling direction is changing abruptly. Therefore, the accuracy determination unit 105 determines that the accuracy of measurement by the GPS unit 11 is not sufficiently good (step S98). . When the angle α is equal to or smaller than the predetermined value, the accuracy determining unit 105 further determines whether or not the moving speed V is equal to or higher than a predetermined value (for example, 20 km / h) (step S96). When the moving speed V is smaller than the predetermined value, the accuracy determination unit 105 determines that the accuracy of measurement by the GPS unit 11 is not sufficiently good (step S98).

  When the angle α is equal to or smaller than the predetermined value and the moving speed V is equal to or larger than the predetermined value, the accuracy determining unit 105 determines that the accuracy of measurement by the GPS unit 11 is sufficiently good (step S97).

Next, details of the calculation process in which the best value estimation unit 102 estimates the best values δP and δV will be described.
A Kalman filter is applied to the arithmetic processing of the best value estimation unit 102. In this embodiment, a measurement vector z that is an input to the Kalman filter, a state vector x that is an output thereof, and a state equation that represents a time change of the state vector x Define as follows.

Here, PN _GPS and PE _GPS are the latitude and longitude components of the position P _GPS obtained from the GPS unit 11, and VN _GPS and VE _GPS are the latitude and longitude components of the velocity V _GPS obtained from the GPS unit 11, respectively. Further, δPN and δPE are the latitude and longitude components of the best value δP, respectively, and δVN and δVE are the latitude and longitude components of the best value δV, respectively. Note that the variable of the subscript INS is of the INS system (described above).

  Under such a definition, the state vector x, which is the output of the Kalman filter, is expressed as follows. However, k (= 1, 2,...) Is a subscript indicating that the best value is estimated for the kth time, and corresponds to time.

Here, K _k is a Kalman gain and is calculated by the following equation.

  However, Δt is an elapsed time from the correction of the acceleration by the acceleration offset value b calculated based on the best value δP estimated at the previous time (k−1th time) to the current time (k time), and each matrix R, P The initial values of Q are as follows.

  The best value estimation unit 102 calculates the state vector x from the measurement vector z according to these equations. The state vector x obtained by this calculation is the best value δP (its latitude and longitude components) and δV (its latitude and longitude components).

Next, another embodiment of the present invention will be described.
FIG. 5 is a diagram showing functional blocks of the navigation device 1 according to the second embodiment of the present invention. The navigation apparatus 1 according to the second embodiment differs from the first embodiment described above in the calculation processing of the best value estimation unit 102, and accordingly, the acceleration offset calculation unit 104 and the position selection output unit 106 are different. However, since other parts are common, the description is omitted.

Optimum value estimation unit 102, a difference _{δP '= P GPS -P INS,} δV' position and velocity ₌ without calculating the V GPS _{-V INS,} these positions _{P GPS} and _{P INS} and the speed _{V GPS} and _{V INS} The best position value P and the best speed value V are estimated directly. The calculation of the Kalman filter for performing this estimation is the same as that described in the first embodiment except that the measurement vector z serving as an input and the state vector x serving as an output are defined as follows:

The acceleration offset calculation unit 104 calculates an acceleration offset value b according to the following formula based on the best value P of the position input from the best value estimation unit 102 and the position P _INS input from the INS system positioning unit 101.
b = b0 + 2 · (P−P _INS ) / ΔT ²
The position selection output unit 106 selectively outputs either the best position value P or the position P _INS in accordance with an instruction from the accuracy determination unit 105. Specifically, the position selection output unit 106 outputs the best value P of the position input from the best value estimation unit 102 to the navigation software (CPU 10) when the measurement accuracy by the GPS unit 11 is sufficiently good. If the accuracy of measurement by the GPS unit 11 is not sufficiently good, the position P _INS input from the INS system positioning unit 101 is output to the navigation software (CPU 10).

  As described above, the calculation processing method of the best value estimation unit 102 can take various forms, and the acceleration offset calculation unit 104 and the position correction unit 103 (position selection unit 103) are selected according to the best value output from the best value estimation unit 102. If the processing of the output unit 106) is appropriately corrected, the operation of the navigation device 1 such as correction of the acceleration a obtained from the acceleration sensor 12 and calculation of the current position P becomes the same as a whole.

As described above, the embodiment of the present invention has been described in detail with reference to the drawings. However, the specific configuration is not limited to the above, and various design changes and the like can be made without departing from the scope of the present invention. It is possible to
For example, the present invention is suitable for use in a portable navigation device, but is also applicable to a stationary navigation device. In the above embodiment, the vehicle includes an automobile, a motorcycle, a bicycle, and the like.

The calculation formula for the acceleration offset calculation unit 104 to calculate the acceleration offset value b is as follows:
b = b0 + 2 · δPxy / ΔT ² · r
It may be as follows. Here, r is a coefficient for adjusting the degree of change when the acceleration offset value is updated from the previous value b0, and is set as appropriate, for example, r = 0.03.
The calculation formula for the acceleration offset calculation unit 104 to calculate the acceleration offset value b is expressed by the following equation using the best speed value δV obtained from the best value estimation unit 102:
b = b0 + δVxy / ΔT
It may be as follows. However, δVxy is obtained by converting the best value δV expressed by latitude and longitude into an xy coordinate system fixed to the vehicle (rotation of azimuth θ).

  Further, instead of the magnetic sensor 13, another direction sensor such as a gyro sensor may be used.

It is a block diagram of the navigation apparatus by embodiment of this invention. It is a figure which shows the functional block of the navigation apparatus by 1st Embodiment. It is a flowchart which shows the operation example of a navigation apparatus. It is a flowchart which shows the example of the process which an precision determination part performs. It is a figure which shows the functional block of the navigation apparatus by 2nd Embodiment.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 10 ... CPU 11 ... GPS part 12 ... Acceleration sensor 13 ... Magnetic sensor 14 ... Memory 15 ... External storage device 16 ... Communication part 17 ... Display apparatus 18 ... Audio | voice output device 19 ... Bus line 101 ... INS system positioning part 102 ... Best value Estimating unit 103 ... Position correcting unit 104 ... Acceleration offset calculating unit 105 ... Accuracy determining unit 106 ... Position selection output unit

Claims (4)

First measuring means for obtaining a position by GPS and obtaining a moving speed from a plurality of positions at different times obtained;
A second measuring unit that corrects the acceleration obtained from the acceleration sensor using the acceleration offset value and obtains the moving speed and the position based on the corrected acceleration and the orientation obtained from the orientation sensor, In a navigation device that performs
The position and moving speed obtained by the first measuring means and the position and moving speed obtained by the second measuring means are input to the Kalman filter to estimate the best value regarding the current most likely position. A best value estimation means;
Based on the best value estimated by the best value estimating means, the acceleration is corrected by the offset error, assuming that the position error obtained by the second measuring means is caused by the offset error of the acceleration sensor. Acceleration offset calculating means for calculating the acceleration offset value for
Accuracy determination means for determining the accuracy of the measurement result by the first measurement means;
When the accuracy determination means determines that the accuracy is good, the position obtained by the second measurement means is corrected using the best value estimated by the best value estimation means, and the current position A position correction unit that outputs the position obtained by the second measuring means as the current position when it is determined that the accuracy is not good;
With
The acceleration offset calculating unit calculates the acceleration offset value based on the best value only when the accuracy determining unit determines that the accuracy is within a predetermined good range. . The best value estimating means includes the difference between the position obtained by the first measuring means and the position obtained by the second measuring means, the moving speed obtained by the first measuring means, and the first The navigation device according to claim 1, wherein the best value of these differences is estimated from the difference from the moving speed obtained by the two measuring means. The accuracy determining means, according to claim 1 or claim 2, moving speed obtained by the first measuring means and judging the accuracy is good when it is larger than a predetermined value The navigation device described in 1. The accuracy determining means calculates an angle of change in the traveling direction from a plurality of positions at different times obtained by the first measuring means, and the accuracy is good when the calculated angle is equal to or less than a predetermined value. The navigation device according to any one of claims 1 to 3, wherein a determination is made .

Images