JP2009236532A - Method for geolocation, program, and apparatus for geolocation - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To propose a new technique for correcting an error contained in the results of detection of a sensor for inertial navigation and to achieve more accurate geolocation of a current location. <P>SOLUTION: In a car navigation apparatus 1, an estimated absolute attitude which is an estimate of an absolute attitude of the sensor 60 for inertial navigation in relation to the earth is calculated by integrating the results of detection of a gyroscope sensor 61 for car navigation. Then, a coordinate transformation matrix of a moving body coordinate system and a normal coordinate system is calculated based on the estimated absolute attitude. By using this coordinate transformation matrix, the results of detection of an acceleration sensor 63 for car navigation is transformed into the normal coordinate system, while a component of gravity acceleration is subtracted, and thereby a movement vector of a car in the normal coordinate system is calculated. Using this movement vector, geolocation of the current location is performed. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

The present invention relates to a positioning method for positioning a current position of a moving body to which an inertial navigation sensor, in which a gyro sensor and an acceleration sensor are integrally configured, is fixed, using a detection result of the inertial navigation sensor.

As a positioning system using an artificial satellite, a GPS (Global Positioning System) is widely known, and is used in a positioning device built in a mobile phone, a car navigation device, or the like. In GPS, the values of four parameters, the three-dimensional coordinate value indicating the position of the aircraft and the clock error, are calculated based on information such as the positions of a plurality of GPS satellites and the pseudoranges from each GPS satellite to the aircraft. The current position of the aircraft is measured by performing the required positioning calculation.

However, in environments where GPS satellite signals cannot be received, such as in tunnels or indoors, G
Since positioning by PS cannot be performed, a technique for positioning the current position by performing inertial navigation calculation processing using detection results of inertial navigation sensors such as a gyro sensor and an acceleration sensor is widely used (for example, patents) Reference 1).
JP-A-10-132589

By the way, the detection results of the gyro sensor and the acceleration sensor, which are inertial navigation sensors, contain errors due to various error factors, and therefore do not always indicate the correct motion state or movement state of the moving body. Absent. For example, a gravitational acceleration component is included in the acceleration detected by the acceleration sensor due to the inclination of the traveling road surface and the attachment inclination of the acceleration sensor.

Further, since the angular velocity detected by the gyro sensor includes an error, an error due to the angular velocity is accumulated in the posture angle obtained by integrating the angular velocity. If the inertial navigation calculation process is performed based on the detection result of the inaccurate inertial navigation sensor as described above, there is a problem that the accurate position of the moving body cannot be obtained.

The present invention has been made in view of the above-described problems, and its object is to propose a new method for correcting an error included in the detection result of the inertial navigation sensor, and to provide a more accurate method. It is to realize the positioning of the current position.

According to a first aspect of the present invention for solving the above problems, the current position of a moving body to which an inertial navigation sensor in which a gyro sensor and an acceleration sensor are integrally formed is fixed, and a detection result of the inertial navigation sensor is obtained. A positioning method that uses the gyro sensor to integrate the detection result to calculate an estimated absolute attitude that is an estimated value of the absolute attitude of the inertial navigation sensor with respect to the earth; A coordinate transformation matrix between a moving body coordinate system and a reference coordinate system, which is a coordinate system based on the reference coordinate system, based on the estimated absolute posture, and using the coordinate transformation matrix, the detection result of the acceleration sensor is calculated Calculating the movement vector of the moving body in the reference coordinate system by converting to the reference coordinate system and subtracting the gravitational acceleration component; and using the movement vector The method comprising positioning a position, a positioning method comprising.

According to another aspect of the present invention, a computer calculates a position of a current position of a moving body to which an inertial navigation sensor, in which a gyro sensor and an acceleration sensor are integrated, is fixed, using a detection result of the inertial navigation sensor. For calculating an estimated absolute attitude, which is an estimated value of the absolute attitude of the inertial navigation sensor with respect to the earth, by integrating the detection results of the gyro sensor, and based on the moving body itself Calculating a coordinate transformation matrix between the moving body coordinate system and the reference coordinate system, which is a coordinate system, based on the estimated absolute posture, and using the coordinate transformation matrix, the detection result of the acceleration sensor is calculated as the reference coordinate Calculating a movement vector of the moving body in the reference coordinate system by converting into a system and subtracting a gravitational acceleration component; and And to measure the current position by using, may also constitute a program for causing the computer to perform.

Furthermore, as another invention, there is provided a positioning device for positioning a current position of a moving body to which an inertial navigation sensor, in which a gyro sensor and an acceleration sensor are integrally formed, is fixed, using a detection result of the inertial navigation sensor. Then, by integrating the detection result of the gyro sensor, an absolute posture estimation unit that calculates an estimated absolute posture that is an estimated value of the absolute posture of the inertial navigation sensor with respect to the earth, and the moving body itself as a reference A matrix calculation unit that calculates a coordinate transformation matrix between a moving body coordinate system, which is a coordinate system, and a reference coordinate system based on the estimated absolute attitude; and the detection result of the acceleration sensor using the coordinate transformation matrix A movement vector calculation unit for calculating a movement vector of the moving body in the reference coordinate system by converting to a coordinate system and subtracting a gravitational acceleration component; A positioning unit for positioning a current position using the Le, may constitute a positioning device provided with a.

According to the first aspect of the present invention, the estimated absolute attitude, which is an estimated value of the absolute attitude of the inertial navigation sensor with respect to the earth, is calculated by integrating the detection results of the gyro sensor. Then, a coordinate transformation matrix between the moving body coordinate system and the reference coordinate system is calculated based on the estimated absolute posture, and using the coordinate transformation matrix, the detection result of the acceleration sensor is converted to the reference coordinate system, and the gravitational acceleration is calculated. Is subtracted, the movement vector of the moving body in the reference coordinate system is calculated.
Then, the current position is measured using this movement vector.

Since the acceleration detected by the acceleration sensor is an acceleration in a moving body coordinate system based on the moving body itself, it is necessary to convert the acceleration into an acceleration in the reference coordinate system in order to correct an error caused by gravity. Therefore, in the first invention, a coordinate conversion matrix is calculated based on the estimated absolute attitude of the inertial navigation sensor, and the acceleration detected by the acceleration sensor is converted into an acceleration in the reference coordinate system using the coordinate conversion matrix. At the same time, the gravitational acceleration is subtracted from the converted acceleration. As a result, it is possible to obtain an acceleration in which an error due to the gravity of the earth is corrected, and as a result, more accurate positioning of the current position can be realized.

Further, as a second invention, the positioning method according to the first invention, wherein it is detected that the moving body is stopped or traveling at a constant speed, and the moving body is stopped or traveling at a constant speed. Is detected based on the component of gravitational acceleration that appears in the detection result of the acceleration sensor,
You may comprise the positioning method including calculating the absolute attitude | position of the said sensor for inertial navigation with respect to the earth, and correct | amending the said estimated absolute attitude | position with the calculated absolute attitude | position.

According to the second aspect of the invention, when it is detected that the moving body is stopped or traveling at a constant speed, the inertial navigation sensor for the earth is detected based on the gravitational acceleration component that appears in the detection result of the acceleration sensor. An absolute posture is calculated, and the estimated absolute posture is corrected with the calculated absolute posture.

Although the details will be described later, when the moving body is stopped or traveling at a constant speed, the acceleration sensor detection result includes only the gravitational acceleration component, and therefore the acceleration sensor detection result and the moving from the reference coordinate system. By using a conversion matrix (conversion formula) to the body coordinate system, the absolute attitude of the inertial navigation sensor with respect to the earth can be calculated. Then, by correcting the estimated absolute posture obtained by integrating the gyro sensor detection results using the calculated absolute posture, it is possible to appropriately correct an error included in the estimated absolute posture.

The third invention is the positioning method of the first or second invention, wherein the current position is intermittently measured by performing a predetermined positioning calculation based on a positioning signal transmitted from a positioning satellite. And calculating the horizontal movement distance of the moving body in a predetermined period from the positioning result of the positioning calculation, and in the same period as the predetermined period based on the detection result of the atmospheric pressure sensor for detecting the external atmospheric pressure. Calculating a moving distance in the altitude direction of the moving body, calculating an inclination angle of a road surface on which the moving body is located based on the moving distance in the horizontal direction and the moving distance in the altitude direction; Using the acceleration detected by the acceleration sensor, the acceleration detected by the second acceleration sensor installed on the moving body so as to have the same posture as the moving body, and the gravitational acceleration, A pitch angle of the inertial navigation sensor using the inclination angle of the road surface and the inclination angle of the inertial navigation sensor with respect to the movable body. And a correction method for estimating the error by including a predetermined estimation operation using a pitch angle of the inertial navigation sensor to estimate an error included in the estimated absolute attitude. May be configured.

According to the third aspect of the invention, the moving distance in the horizontal direction of the moving object calculated from the positioning result of the positioning calculation based on the positioning signal and the moving distance in the altitude direction of the moving object calculated based on the detection result of the atmospheric pressure sensor. Based on the above, the inclination angle of the road surface on which the moving body is located is calculated. An inertial navigation sensor using the acceleration detected by the acceleration sensor, the acceleration detected by the second acceleration sensor installed on the moving body so as to have the same posture as the moving body, and the gravitational acceleration. The tilt angle of the inertial navigation sensor is calculated using the inclination angle of the road surface and the tilt angle of the inertial navigation sensor with respect to the mobile object. Then, a predetermined estimation operation using the calculated pitch angle of the inertial navigation sensor is performed, an error included in the estimated absolute attitude is estimated, and the estimated absolute attitude is corrected.

The detection result of the acceleration sensor includes a component of gravitational acceleration caused by the inclination angle of attachment of the inertial navigation sensor to the moving body. For example, when a navigation device is assumed as a positioning device with a built-in inertial navigation sensor, the navigation device may be arranged obliquely with respect to the moving body. Therefore, the acceleration detected by the acceleration sensor is used as it is. The acceleration cannot be made.

Therefore, in the third invention, after calculating the mounting inclination angle of the inertial navigation sensor with respect to the moving body, an estimation calculation using the correct pitch angle of the inertial navigation sensor is performed to estimate an error included in the estimated absolute posture. Thus, the estimated absolute posture is corrected. Since the estimated absolute posture after correction is considered to be an accurate posture of the inertial navigation sensor in consideration of the mounting inclination angle, the coordinate transformation matrix calculated based on this accurate posture is also accurate. . Therefore, coupled with the first invention, it is possible to obtain the acceleration of the moving body that is as close as possible to the true value.

Hereinafter, an example of an embodiment suitable for the present invention will be described with reference to the drawings. In the following, a car navigation device mounted on an automobile, which is a kind of mobile body, will be described as an example of a positioning device, and a case where a GPS (Global Positioning System) is used as a positioning system will be described. Possible embodiments are not limited to these.

1. Functional Configuration FIG. 1 is a block diagram showing a functional configuration of a car navigation apparatus 1 in the present embodiment. The car navigation device 1 includes a GPS antenna 10, a GPS receiving unit 20, a host CPU (Central Processing Unit) 30, an operation unit 40, a display unit 50, an inertial navigation sensor 60, an atmospheric pressure sensor 70, and a ROM. (Read Only Memory) 80 and RAM (Rand
om Access Memory) 90.

The GPS antenna 10 is a radio frequency (Ra) including a GPS satellite signal transmitted from a GPS satellite.
dio Frequency) is an antenna that receives a signal, and outputs the received signal to the GPS receiver 20. The GPS satellite signal is a PRN (Pseudo R) which is a kind of spreading code that differs for each satellite.
andom noise) code 1.57542 [G modulated directly by spread spectrum system
Hz] communication signal. The PRN code is a pseudo-random noise code having a repetition period of 1 ms with a code length of 1023 chips as one PN frame.

The GPS receiving unit 20 is a positioning circuit that measures the current position of the car navigation device 1 based on a signal output from the GPS antenna 10, and is a functional block corresponding to a so-called GPS receiver. The GPS receiving unit 20 includes an RF (Radio Frequency) receiving circuit unit 21,
And a baseband processing circuit unit 23. The RF receiving circuit unit 21 and the baseband processing circuit unit 23 can be manufactured as separate LSIs (Large Scale Integration) or as a single chip.

The RF receiving circuit unit 21 is an RF signal processing circuit block, and generates an oscillation signal for RF signal multiplication by dividing or multiplying a predetermined local oscillation signal. Then, by multiplying the generated oscillation signal by the RF signal output from the GPS antenna 10, the RF signal is down-converted to an intermediate frequency signal (hereinafter referred to as an "IF (Intermediate Frequency) signal"), After the IF signal is amplified, it is converted into a digital signal by an A / D converter and output to the baseband processing circuit unit 23.

The baseband processing circuit unit 23 performs correlation processing or the like on the IF signal output from the RF reception circuit unit 21 to capture and extract GPS satellite signals, decodes the data, and extracts navigation messages, time information, and the like. This is a circuit unit that performs positioning calculation. The baseband processing circuit unit 23
It comprises a CPU as a processor and ROM and RAM as memories.
As the positioning calculation, for example, a known technique such as positioning calculation using the least square method can be applied.

The host CPU 30 is a processor that comprehensively controls each unit of the car navigation apparatus 1 according to various programs such as a system program stored in the ROM 80. In particular, in the present embodiment, the host CPU 30 determines an output position that is a position to be displayed on the navigation screen according to the positioning program 803. Then, according to the navigation program 801, a navigation screen in which the output position is plotted is generated and displayed on the display unit 50. As a position to be displayed on the navigation screen, it is preferable to plot on the road closest to the output position by further map matching the output position.

The operation unit 40 is an input device configured by a touch panel, a button switch, or the like, for example, and outputs a pressed icon or button signal to the host CPU 30. This operation unit 40
With this operation, various instructions such as a destination input and a navigation screen display request are input.

The display unit 50 is composed of an LCD (Liquid Crystal Display) or the like, and the host CPU 3
It is a display device that performs various displays based on display signals input from zero. The display unit 50 includes
A navigation screen or the like is displayed.

The inertial navigation sensor 60 is a sensor used for inertial navigation calculation processing. The car navigation gyro sensor 61 and the car navigation acceleration sensor 63 are integrated (packaged), and are built in and fixed to the main body of the car navigation apparatus 1. ing.

The car navigation gyro sensor 61 is a sensor (angular velocity sensor) that detects the rotation of the car navigation device 1 by detecting angular velocities around the three orthogonal axes, and outputs the detected angular velocities of the three axes to the host CPU 30. .

The car navigation acceleration sensor 63 is a sensor that detects the movement state of the car navigation device 1 by detecting the accelerations in the axial directions of the three orthogonal axes, and outputs the detected three-axis acceleration to the host CPU 30.

The atmospheric pressure sensor 70 is a sensor that detects the external atmospheric pressure of the car navigation device 1, and outputs the detected external atmospheric pressure to the host CPU 30.

The ROM 80 is a read-only storage device, and stores a system program for the host CPU 30 to control the car navigation apparatus 1 and various programs and data for realizing a navigation function.

The RAM 90 is a readable / writable storage device, and forms a work area for temporarily storing a system program executed by the host CPU 30, various processing programs, data being processed in various processing, processing results, and the like.

In addition to the car navigation apparatus 1, the automobile is equipped with an acceleration sensor 100, and the detection result is output to the host CPU 30. The acceleration sensor 100 is an orthogonal three-axis sensor that detects a moving state of the automobile, and includes suspension control for body shake control, torque distribution control for front and rear wheels, ABS (Anti-lock Brak).
e System) is used to perform control and the like, and is preinstalled in the automobile for controlling the automobile.

Here, a coordinate system and the like used in the present embodiment will be described. As shown in FIG. 2, the traveling direction of the vehicle is “X ^b axis” (the straight traveling direction is the positive direction), and the left and right direction toward the X ^b axis positive direction is “Y ^b
Axis "(the right forward direction)," Z ^b axis "a right-handed orthogonal coordinate system (the downward direction the positive direction) and the height direction with respect to X ^b-axis is defined as a" mobile coordinate system ". Further, a right-handed orthogonal coordinate system in which the north direction is “X ⁿ axis”, the east direction is “Y ⁿ axis”, and the gravity direction by the earth is “Z ⁿ axis” is defined as “reference coordinate system”.

In addition, the inclination and the inclination angle of the inertial navigation sensor 60 with respect to the automobile are referred to as “attachment inclination” and “attachment inclination angle”, respectively. Further, the attitude and attitude angle of the inertial navigation sensor 60 with respect to the earth are referred to as “absolute attitude” and “absolute attitude angle”, respectively, and the rotation angle “φ” around the ^Xb axis is referred to as “absolute roll angle”, Y The rotation angle “θ” around the ^b axis is referred to as “absolute pitch angle”, and the rotation angle “ψ” around the Z ^b axis is referred to as “absolute yaw angle”. Further, the absolute attitude and the absolute attitude angle of the inertial navigation sensor 60 with respect to the earth obtained by integrating the detection results of the car navigation gyro sensor 61 are referred to as “estimated absolute attitude” and “estimated absolute attitude angle”, respectively. .

Due to the inclination of the vehicle and the inclination of the inertial navigation sensor 60, the car navigation acceleration sensor 6
Since the acceleration detected by 3 includes a gravitational acceleration component, the accurate acceleration of the automobile cannot be obtained unless this gravitational component is removed. Therefore, in this embodiment, a coordinate transformation matrix from the moving body coordinate system to the reference coordinate system is calculated based on the estimated absolute posture. Then, using the calculated coordinate conversion matrix, the detection result of the car navigation acceleration sensor 63 is converted into the reference coordinate system, and the acceleration of the vehicle in the earth coordinate system is obtained by subtracting the gravitational acceleration component.

However, since the angular velocity detected by the car navigation gyro sensor 61 includes an error, the estimated absolute posture obtained by integrating the detection result of the car navigation gyro sensor 61 does not necessarily match the absolute posture. In order to solve this problem, in this embodiment, the inclination angle and the attachment inclination angle of the road surface on which the automobile is traveling (hereinafter referred to as “traveling road surface”) are calculated, and based on these, the inertial navigation sensor 60 is calculated. The absolute pitch angle of is calculated. Then, an error included in the estimated absolute posture is estimated by performing Kalman filter processing (estimation calculation) using the calculated absolute pitch angle as an observation value, and the estimated absolute posture is corrected.

FIG. 3 is a diagram for explaining the principle of calculating the mounting inclination angle. Here, the mounting inclination angle is
The description will be made assuming that “α” and the inclination angle of the road surface are “β”. In a state where the automobile is stopped, the traveling direction of the automobile detected by the car navigation acceleration sensor 63 and the acceleration sensor 100 (X
^The acceleration with respect to the ( ^b- axis direction) is only the component with respect to the direction of gravity acceleration.

Specifically, when the gravitational acceleration is “g”, accelerations “a _x1 ” and “a _x2 ” detected by the car navigation acceleration sensor 63 and the acceleration sensor 100 in the ^Xb- axis direction are expressed by the following equations (1), respectively. And (2).

By eliminating “β” from the equations (1) and (2), the mounting inclination angle “α” is calculated as the following equation (3).

2. Data Configuration FIG. 4 is a diagram illustrating an example of data stored in the ROM 80. The ROM 80 includes data of a navigation program 801 read out by the host CPU 30 and executed as navigation processing, a positioning program 803 executed as positioning processing (see FIG. 7), and map information for generating a navigation screen. Some map data 805 is stored. Further, the positioning program 803 includes an estimated absolute posture correction program 8031 executed as estimated absolute posture correction processing (see FIG. 8) as a subroutine.

In the navigation processing, the host CPU 30 performs map matching processing for correcting the output position determined by the positioning processing on the road using the map information stored in the map data 805, and plots the corrected position. In this process, a navigation screen is generated and displayed on the display unit 50. Since the map matching process is known,
Detailed description is omitted.

With the positioning process, the host CPU 30 performs the GPS positioning process to determine the current position if positioning by GPS is possible, and performs the inertial navigation calculation process to determine the current position if positioning by GPS is not possible. This is a process of measuring the position. The positioning process will be described later in detail using a flowchart.

The estimated absolute attitude correction process is a process in which the host CPU 30 performs a Kalman filter process considering the mounting inclination angle to estimate an error included in the estimated absolute attitude and correct the estimated absolute attitude. The mounting inclination angle is calculated using detection results of the car navigation acceleration sensor 63 and the acceleration sensor 100 when the automobile is stopped. The estimated absolute posture correction process will also be described in detail later using a flowchart.

FIG. 5 is a diagram illustrating an example of data stored in the RAM 90. The RAM 90 stores measurement history data 901, correction absolute attitude angle data 903, and attachment inclination angle data 905.

FIG. 6 is a diagram illustrating an example of the data configuration of the measurement history data 901. Measurement history data 9
01 includes a triaxial angular velocity 9012 detected by the car navigation gyro sensor 61 and a triaxial acceleration 9013 detected by the car navigation acceleration sensor 63 at each time 9011.
And an estimated absolute attitude angle 9015 obtained by integrating the external atmospheric pressure 9014 detected by the atmospheric pressure sensor 70, the angular velocity 9012, the velocity 9017 obtained by the GPS positioning process or the inertial navigation calculation process, and the GPS positioning process. The obtained GPS positioning position 9018 and the inertial navigation calculation position 9019 obtained by the inertial navigation calculation process are stored in association with each other.
The measurement history data 901 is updated by the host CPU 30 in the positioning process.

The correction absolute posture angle data 903 is absolute posture angle data calculated based on the detection result of the car navigation acceleration sensor 63 and the gravitational acceleration, and is used to correct the estimated absolute posture. The absolute posture angle data for correction 903 is the host CP in the estimated absolute posture correction process.
Updated by U30.

The attachment inclination angle data 905 is data of the attachment inclination angle “α”, and is updated by the host CPU 30 in the estimated absolute posture correction process.

3. Processing Flow FIG. 7 is a flowchart showing a flow of positioning processing executed in the car navigation device 1 by the host CPU 30 reading and executing the positioning program 803 stored in the ROM 80.

The positioning process is a process of starting execution when the host CPU 30 detects that a positioning start instruction is operated on the operation unit 40. The car navigation device 1 is turned on.
/ OFF and GPS activation / stop may be linked to start the positioning process when a power-on operation of the car navigation device 1 is detected.

Further, although not specifically described, the RF by the GPS antenna 10 is performed during the following positioning process.
It is assumed that signal reception, down-conversion to an IF signal by the RF reception circuit unit 21, acquisition / tracking of a GPS satellite signal by the baseband processing circuit unit 23, and the like are performed at any time. Further, according to the detection results of the car navigation gyro sensor 61, the car navigation acceleration sensor 63, and the atmospheric pressure sensor 70, the host CPU 30 performs measurement history data 9 in the RAM 90.
01 is updated as needed.

First, the host CPU 30 determines whether positioning by GPS is possible (step A1). Specifically, in the case of three-dimensional positioning (positioning including altitude), the number of GPS satellites currently captured by the baseband processing circuit unit 23 (hereinafter referred to as “captured satellites”) is “four or more. ", It is determined that positioning by GPS is possible. In the case of two-dimensional positioning (positioning not including altitude), it is determined that GPS positioning is possible when the number of captured satellites is “three or more”.

If it is determined that GPS positioning is possible (step A1; Yes),
The host CPU 30 performs GPS positioning processing (step A3). Specifically, host CP
U30 causes the CPU of the baseband processing circuit unit 23 to execute a positioning calculation using, for example, the least square method based on the satellite information and time information of the captured satellite, and calculates the speed and position of the car navigation apparatus 1. . Then, the calculated speed and position (GPS positioning position) are stored in the RAM 9
0 measurement history data 901 is stored.

Thereafter, the host CPU 30 determines the GPS positioning position obtained by the GPS positioning process as the output position (step A5), and shifts the process to step A17.

On the other hand, when it is determined in step A1 that GPS positioning is impossible (step A1; No), the host CPU 30 uses the latest estimated absolute attitude angle stored in the measurement history data 901 of the RAM 90, Coordinate transformation matrix from moving body coordinate system to reference coordinate system
C ⁿ _b ”is calculated (step A7). The coordinate transformation matrix “C ⁿ _b ” is the absolute pitch angle “θ”,
3 × 3 including trigonometric functions with absolute roll angle “φ” and absolute yaw angle “ψ” as variables
This is a known matrix.

但し、ベクトル表記の「ａⁿ」、「ａ^b」、「ｇⁿ」は、それぞれ基準座標系における自
動車の加速度ベクトル、移動体座標系における自動車の加速度ベクトル、基準座標系にお
ける重力加速度ベクトルをそれぞれ示している。 Then, the host CPU 30 uses the coordinate transformation matrix “C ⁿ _b ” calculated in step A7 and the detection result of the car navigation acceleration sensor 63 to calculate the acceleration of the vehicle in the reference coordinate system according to the following equation (4). (Step A9).

However, "a ^n" in vector notation, "a ^b", "g ^n" is an automobile of the acceleration vector at each reference frame, the acceleration vector of the vehicle in the mobile coordinate system, the gravitational acceleration vector in the reference coordinate system, respectively Show.

Thereafter, the host CPU 30 calculates the speed of the vehicle (it can be said that it is a speed including a direction, not just a speed) based on the acceleration calculated in step A9 (step A11), and the measurement history of the RAM 90 The data 901 is stored. Then, the position of the vehicle is calculated using the calculated speed and the output position of the previous time (one time before) to obtain an inertial navigation calculation position (step A13), and stored in the measurement history data 901 of the RAM 90. Then, the calculated inertial navigation calculation position is determined as the output position (step A15).

Next, the host CPU 30 calculates the estimated absolute posture angle by integrating the detection result of the car navigation gyro sensor 61 (step A17), and the measurement history data 90 of the RAM 90 is calculated.
1 is stored. Then, the estimated absolute attitude correction program 80 stored in the ROM 80
By reading and executing 31, an estimated absolute posture correction process for correcting the estimated absolute posture angle calculated in step A 17 is performed (step A 19).

After performing the estimated absolute posture correction process, the host CPU 30 determines whether or not to end the process (step A21). If it is determined that the process has not ended yet (step A21; No), the host CPU 30 returns to step A1. Moreover, when it determines with complete | finishing (step A21; Yes), a positioning process is complete | finished.

FIG. 8 is a flowchart showing the flow of the estimated absolute posture correction process.
First, the host CPU 30 determines whether or not the automobile is stopped or traveling at a constant speed (
Step B1). The determination as to whether or not the vehicle is stopped is made by determining whether or not the variance value of the angular velocity that is the output value of the car navigation gyro sensor 61 is equal to or less than a predetermined threshold value. Further, whether or not the automobile is traveling at a constant speed is determined by, for example, an acceleration sensor 6 for car navigation.
This is performed by determining whether or not the acceleration variance value, which is the output value of 3, is less than or equal to a predetermined threshold value.

When it is determined in step B1 that the vehicle is stopped or traveling at a constant speed (step B1; Yes), the host CPU 30 determines whether or not the vehicle is stopped (step B3). If it is determined that the vehicle is not stopped, that is, is traveling at a constant speed (step B3; No), the process proceeds to step B7.

If it is determined that the vehicle is stopped (step B3; Yes), the host CP
U30 uses the acceleration “a _x1 ” in the ^Xb- axis direction detected by the car navigation acceleration sensor 63 and the acceleration “a _x2 ” in the ^Xb- axis direction detected by the acceleration sensor 100 to obtain an equation (3
) To calculate the mounting inclination angle “α” (step B5). Then, the attachment inclination angle data 905 in the RAM 90 is updated with the calculated attachment inclination angle “α”.

Next, the host CPU 30 calculates the absolute pitch angle “θ” and the absolute roll angle “φ” of the inertial navigation sensor 60 using the detection result of the car navigation acceleration sensor 63 (step B7).
), The absolute posture angle data for correction 903 in the RAM 90 is stored.

More specifically, the acceleration vector of the automobile in the moving body coordinate system detected by the car navigation acceleration sensor 63 is represented by “a ^b ” in vector notation, and the coordinate transformation matrix from the reference coordinate system to the moving body coordinate system is represented by “C ^b _n ”. ”, The gravitational acceleration vector in the reference coordinate system is expressed as“ g
ⁿ ", the following equation (5) is established.

Each component of the three-dimensional equation (5) _{"(a x, a y, a} z) 'Expanding the following equation (6-1
) To (6-3).

From the equations (6-1) to (6-3), the absolute pitch angle “θ” and the absolute roll angle “φ”
Is obtained by the following equations (7-1) and (7-2).

Next, the host CPU 30 performs Kalman filter processing using the absolute pitch angle “θ” and the absolute roll angle “φ” calculated in step B7 as observation values, and the estimation calculated in step A17 and stored in the measurement history data 901. The absolute posture angle is corrected (step B9). Specifically, an estimation calculation based on the theory of the Kalman filter is performed using an error included in the estimated absolute attitude angle as a state vector and using the absolute pitch angle “θ” and the absolute roll angle “φ” as external observation amounts.
Then, the estimated absolute posture angle is corrected by adding the estimated error to the estimated absolute posture angle calculated based on the detection result of the car navigation gyro sensor 61.

On the other hand, when it is determined in step B1 that the vehicle is not stopped or traveling at a constant speed (step B1; No), the host CPU 30 determines whether or not the current positioning timing is GPS positioning (step B11). And when it determines with this positioning timing being GPS positioning (step B11; Yes), it is determined whether the last positioning timing was also GPS positioning (step B13).

If it is determined that the previous positioning timing is also GPS positioning (step B1
3) Yes, the host CPU 30 determines the horizontal direction of the vehicle between the previous positioning timing and the current positioning timing from the previous GPS positioning position and the current GPS positioning position stored in the measurement history data 901 of the RAM 90. The movement distance is calculated (step B15).
Further, the host CPU 30 calculates the moving distance in the altitude direction of the vehicle between the previous positioning timing and the current positioning timing based on the detection result of the atmospheric pressure sensor 70 (step B17).

Next, the host CPU 30 calculates the inclination angle “β” of the traveling road surface using the horizontal movement distance calculated in step B15 and the altitude movement distance calculated in step B17 (step B19). Then, the absolute pitch angle “θ” is calculated by adding the mounting tilt angle “α” stored in the mounting tilt angle data 905 of the RAM 90 (step B21).
The correction absolute attitude angle data 903 for AM 90 is updated.

Then, the host CPU 30 performs Kalman filter processing using the absolute pitch angle “θ” calculated in step B21 as an observation value, and calculates in step A17 to calculate measurement history data 901.
The estimated absolute attitude angle stored in (1) is corrected (step B23). Specifically, an estimation calculation based on the theory of the Kalman filter is performed using an error included in the estimated absolute attitude angle as a state vector and an absolute pitch angle “θ” as an external observation amount. Then, the estimated absolute posture angle is corrected by adding the estimated error to the estimated absolute posture angle calculated based on the detection result of the car navigation gyro sensor 61.

After correcting the estimated absolute attitude angle in step B9 or B23, the host CPU 30
Ends the estimated absolute posture correction process. If it is determined in step B11 that the current positioning timing is not GPS positioning (step B11; No), or step B11
Even when it is determined in step 13 that the previous positioning timing is not GPS positioning (step B13; No), the host CPU 30 ends the estimated absolute posture correction process.

3. Effects According to the present embodiment, in the car navigation device 1, the estimated absolute attitude, which is an estimated value of the absolute attitude of the inertial navigation sensor 60 with respect to the earth, is calculated by integrating the detection results of the car navigation gyro sensor 61. The Then, a coordinate transformation matrix between the moving body coordinate system and the reference coordinate system is calculated based on the estimated absolute attitude, and the detection result of the car navigation acceleration sensor 63 is converted into the reference coordinate system using the coordinate transformation matrix. By subtracting the gravitational acceleration component, the vehicle movement vector in the reference coordinate system is calculated. Then, the current position is measured using this movement vector.

Since the acceleration detected by the car navigation acceleration sensor 63 is an acceleration in a moving body coordinate system based on the automobile itself, it is necessary to convert the acceleration into an acceleration in the reference coordinate system in order to correct the influence of the gravitational acceleration. . Therefore, in this embodiment, the inertial navigation sensor 60 is used.
The coordinate transformation matrix is calculated based on the estimated absolute posture of the vehicle, and the acceleration detected by the car navigation acceleration sensor 61 is converted into the acceleration in the reference coordinate system using the coordinate transformation matrix. The acceleration is subtracted. As a result, it is possible to obtain acceleration in which the component due to the gravity of the earth is corrected, and as a result, more accurate positioning of the current position can be realized.

4). Modified example 4-1. Electronic Device The present invention can be applied to any electronic device provided that it has a positioning device. For example, the present invention can be similarly applied to a notebook personal computer, a PDA (Personal Digital Assistant), and the like.

4-2. Mobile Object The mobile object is not necessarily limited to an automobile, and can be similarly applied to mobile objects such as buses and trains.

4-3. Satellite positioning system In the above-described embodiment, the GPS has been described as an example of the satellite positioning system.
AS (Wide Area Augmentation System), QZSS (Quasi Zenith Satellite System)
Other satellite positioning systems such as GLONASS (GLObal NAvigation Satellite System) and GALILEO may be used.

4-4. Differentiation of processing Part or all of the processing performed by the host CPU 30 is performed by the CPU of the baseband processing circuit unit 23.
You may decide to do. Specifically, for example, the CPU of the baseband processing circuit unit 23 performs positioning processing. Then, the host CPU 30 performs a map matching process or the like on the output position obtained by the positioning process to generate a navigation screen, and performs a navigation process for displaying the generated navigation screen on the display unit 50. Further, the CPU may perform all the processes performed by the host CPU 30 including the navigation process.

4-5. Calculation of the inclination angle of the traveling road surface In the embodiment described above, based on the moving distance in the horizontal direction of the vehicle determined from the GPS positioning result and the moving distance in the height direction of the vehicle determined from the detection result of the atmospheric pressure sensor 70, Although it has been described that the inclination angle “β” of the traveling road surface is calculated, the inclination angle “β” of the traveling road surface may be calculated from only the GPS positioning result without using the detection result of the atmospheric pressure sensor 70.

In other words, in 3D positioning, the latitude, longitude and altitude of the car can be obtained.
The inclination angle “β” of the traveling road surface can be calculated by using the horizontal movement distance obtained from the change in latitude and longitude and the movement distance in the altitude direction obtained from the change in altitude.
In this case, it is not necessary to provide the atmospheric pressure sensor 70 in the car navigation device 1.

In the above-described embodiment, the road surface inclination angle “β” is calculated based on the travel distance during one time. However, this is calculated based on the travel distance for a predetermined period (for example, 10 hours). Of course, it is also possible to do.

4-6. Correction of Estimated Absolute Posture In the above-described embodiment, it has been described that the estimated absolute posture is corrected by performing the Kalman filter process to estimate the error included in the estimated absolute posture angle. However, the following may be performed. . That is, in step B9 of the estimated absolute posture correction process of FIG. 8, without performing Kalman filter processing, the absolute pitch angle and the absolute roll angle included in the estimated absolute posture angle are calculated using the absolute pitch angle “θ” calculated in step B7 and A process of replacing each with an absolute roll angle “φ” is performed. Similarly, in step B23, the Kalman filter process is not performed, and the absolute pitch angle included in the estimated absolute posture angle is replaced with the absolute pitch angle “θ” calculated in step B21.

The block diagram which shows the function structure of a car navigation apparatus. The figure which shows an example of a mobile body coordinate system. Explanatory drawing of the principle of attachment inclination-angle calculation. The figure which shows an example of the data stored in ROM. The figure which shows an example of the data stored in RAM. The figure which shows an example of a data structure of measurement log | history data. The flowchart which shows the flow of a positioning process. The flowchart which shows the flow of an estimated absolute attitude | position correction process.

Explanation of symbols

1 car navigation device, 10 GPS antenna, 20 GPS receiver,
21 RF receiving circuit section, 23 baseband processing circuit section, 30 host CPU,
40 operation units, 50 display units, 60 inertial navigation sensors,
61 Gyro sensor for car navigation, 63 Acceleration sensor for car navigation,
70 barometric pressure sensor, 80 ROM, 90 RAM, 100 acceleration sensor

Claims (5)

A positioning method for positioning a current position of a moving body to which an inertial navigation sensor, in which a gyro sensor and an acceleration sensor are integrally configured, is fixed, using a detection result of the inertial navigation sensor,
Calculating an estimated absolute attitude that is an estimated value of the absolute attitude of the inertial navigation sensor with respect to the earth by integrating the detection result of the gyro sensor;
Calculating a coordinate transformation matrix between a moving body coordinate system and a reference coordinate system, which is a coordinate system based on the moving body itself, based on the estimated absolute attitude;
Using the coordinate transformation matrix to convert a detection result of the acceleration sensor into the reference coordinate system and subtracting a gravitational acceleration component to calculate a movement vector of the moving body in the reference coordinate system;
Positioning the current position using the movement vector;
Positioning method including. Detecting that the moving body is stopped or traveling at a constant speed;
When it is detected that the moving body is stopped or traveling at a constant speed, the absolute attitude of the inertial navigation sensor with respect to the earth is calculated based on a gravitational acceleration component appearing in the detection result of the acceleration sensor. And
Correcting the estimated absolute posture with the calculated absolute posture;
The positioning method according to claim 1, comprising: Performing a predetermined positioning calculation based on a positioning signal transmitted from a positioning satellite, and intermittently positioning the current position;
Calculating a horizontal movement distance of the moving body in a predetermined period from a positioning result of the positioning calculation;
Calculating a moving distance in the altitude direction of the moving body in the same period as the predetermined period based on a detection result of an atmospheric pressure sensor that detects an external atmospheric pressure;
Calculating an inclination angle of a road surface on which the moving body is located based on the horizontal moving distance and the altitude moving distance;
Using the acceleration detected by the acceleration sensor, the acceleration detected by the second acceleration sensor installed on the moving body so as to have the same posture as the posture of the moving body, and the gravitational acceleration, Calculating an inclination angle of attachment of the inertial navigation sensor to the moving body;
Calculating a pitch angle of the inertial navigation sensor using an inclination angle of the road surface and an attachment inclination angle of the inertial navigation sensor with respect to the moving body;
Performing a predetermined estimation calculation using a pitch angle of the inertial navigation sensor to estimate an error included in the estimated absolute attitude and correcting the estimated absolute attitude;
The positioning method according to claim 1 or 2, comprising: A program for causing a computer to calculate a position of a current position of a moving body to which an inertial navigation sensor, in which a gyro sensor and an acceleration sensor are integrally configured, is fixed, using a detection result of the inertial navigation sensor,
Calculating an estimated absolute attitude that is an estimated value of the absolute attitude of the inertial navigation sensor with respect to the earth by integrating the detection result of the gyro sensor;
Calculating a coordinate transformation matrix between a moving body coordinate system and a reference coordinate system, which is a coordinate system based on the moving body itself, based on the estimated absolute attitude;
Using the coordinate transformation matrix to convert a detection result of the acceleration sensor into the reference coordinate system and subtracting a gravitational acceleration component to calculate a movement vector of the moving body in the reference coordinate system;
Positioning the current position using the movement vector;
For causing the computer to execute. A positioning device for positioning a current position of a moving body to which an inertial navigation sensor, in which a gyro sensor and an acceleration sensor are integrally configured, is fixed, using a detection result of the inertial navigation sensor,
An absolute attitude estimation unit that calculates an estimated absolute attitude that is an estimated value of the absolute attitude of the inertial navigation sensor with respect to the earth by integrating the detection result of the gyro sensor;
A matrix calculation unit that calculates a coordinate transformation matrix between a moving body coordinate system and a reference coordinate system, which is a coordinate system based on the moving body itself, based on the estimated absolute attitude;
Using the coordinate transformation matrix, a movement vector calculation unit that converts the detection result of the acceleration sensor into the reference coordinate system and calculates a movement vector of the moving body in the reference coordinate system by subtracting a gravitational acceleration component. When,
A positioning unit for positioning the current position using the movement vector;
Positioning device equipped with.

Images