JP2004125689A - Position calculation system for self-contained navigation - Google Patents

Abstract

In a navigation system of self-contained navigation, a correct direction is calculated even when a moving body runs on an inclined road, a case is attached with an inclination, or an angular velocity detecting device is inclined.
An angular velocity detecting means for detecting angular velocities in a yaw direction and a pitch direction, an acceleration detecting means for detecting at least two orthogonal accelerations, a distance detecting means for detecting a moving distance, and a moving body. Inclination detecting means 12 for detecting the inclination of the housing, housing mounting angle calculating means 13 for detecting the mounting angle of the housing based on the detection result of the tilt and the acceleration detected by the acceleration detecting means, and the housing mounting angle Tilt angle calculating means 4 for correcting the output of the detected angular velocity and acceleration based on the detected angular velocity, detecting the tilt angle of the moving object based on the corrected angular velocity, corrected acceleration, and moving distance; And an angular velocity correcting means 5 for correcting the azimuth of the angular velocity using the above.
[Selection diagram] Fig. 4

Description

[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a navigation system mounted on a moving body such as an automobile, and more particularly to a position calculation system for self-contained navigation that calculates a direction and a position with high accuracy by correcting a deviation of a detection axis of an angular velocity sensor. .
[0002]
[Prior art]
2. Description of the Related Art In recent years, in navigation systems for automobiles and the like, there is an increasing demand for accurate route guidance and display of a vehicle position on narrow streets, complicated crossing roads, parking lots, and the like. In order to meet such demands, highly accurate position positioning technology is always required. Positioning technology in current navigation systems is based on a radio navigation system that calculates the absolute position of a car based on information from GPS satellites and the like, and a self-contained navigation system that calculates the position based on information from various sensors attached to a moving object. It is made up. Among them, radio navigation has been improved in accuracy by launching new satellites and using reference point data. However, in the case of radio navigation, there is a problem that it is not possible to calculate an accurate position of a moving body in places where radio waves cannot be received, such as indoors, tunnels, and buildings in a city center. Therefore, it is necessary to improve the accuracy of the position calculation system using the self-contained navigation.
[0003]
For self-contained navigation, it is necessary to know the direction and speed of the moving object. When calculating the azimuth by detecting the yaw direction angular velocity of the moving body with the angular velocity sensor, when the angular velocity sensor installed on the moving body is tilted, or when the moving body is tilted due to the gradient of the road surface or running state In addition, the angular velocity detection surface of the yaw direction angular velocity sensor is inclined, and as a result, the calculated azimuth is shifted. In the related art for solving this problem, there is a technology in which the inclination of the yaw direction angular velocity sensor is calculated using three acceleration sensors and the output is corrected (for example, see Patent Document 1).
[0004]
In this prior art, as shown in FIG. 10, the inclination of the yaw direction angular velocity sensor from the vertical direction is obtained by three acceleration sensors installed on the same plane as the yaw direction angular velocity sensor, and the yaw direction angular velocity sensor is rotated about two different axes. It is to be corrected.
[0005]
[Patent Document 1]
JP-A-6-324066
[Problems to be solved by the invention]
However, in the correction of the conventional angular velocity sensor, since the yaw angular velocity is corrected using only the output of the acceleration sensor, the correction for the instantaneous change of the angular velocity detection surface is insufficient and sufficient accuracy cannot be obtained.
[0007]
Further, as another problem, when there is an inclination at the time of mounting the housing in a car navigation or the like, an error occurs if the output of each sensor is used as it is. In the current navigation system, this problem is avoided by providing a small restriction on the method of installing the housing, but the burden on the user is increased.
[0008]
The present invention accurately corrects the azimuth and position of a moving object by accurately correcting an output error of an angular velocity caused when a detection surface of a sensor is inclined due to a gradient of a road surface, an inclination of a moving object, and a deviation at the time of initial mounting. It is an object of the present invention to provide a self-contained navigation position calculation system capable of detecting well and eliminating restrictions when the mobile terminal is attached to a moving body.
[0009]
[Means for Solving the Problems]
In order to solve the above problems, the first invention is an angular velocity detecting means for detecting an angular velocity of the moving body in the yaw direction and the pitch direction, and at least one of a longitudinal axis, a lateral axis, and a vertical axis of the moving body. Acceleration detecting means for detecting two-axis acceleration, distance detecting means for detecting a moving distance of the moving object, angular velocity detected by the angular velocity detecting means, acceleration detected by the acceleration detecting means, and moving distance detected by the distance detecting means. An inclination angle calculating means for calculating an inclination angle of the moving body with respect to a certain arbitrary coordinate system, and an angular velocity correcting means for correcting the angular velocity detected by the angular velocity detecting means based on the inclination angle obtained by the inclination angle calculating means. A position calculation system for self-contained navigation, comprising:
[0010]
According to the first aspect of the invention, even when the moving body runs on a gradient road or when the moving body is tilted due to motion, the correct direction of the moving body can be calculated, and as a result, the accurate position of the moving body can be calculated. It becomes possible to know.
[0011]
A second invention is an angular velocity detecting means for detecting angular velocities of a moving body in a yaw direction and a pitch direction, and an acceleration detecting means for detecting acceleration of at least two axes among a longitudinal axis, a lateral axis, and a vertical axis of the moving body. Means, a housing mounting angle calculating means for calculating an mounting angle of the housing fixing the angular velocity detecting means and the acceleration detecting means to the moving body based on the acceleration detected by the acceleration detecting means, and a moving distance of the moving body. The distance detecting means to be detected, and the angular velocity detected by the angular velocity detecting means and the acceleration detected by the acceleration detecting means are corrected based on the mounting angle of the housing obtained by the housing mounting angle calculating means, and the corrected angular velocity and An inclination angle calculating means for calculating an inclination angle of the moving body with respect to a certain arbitrary coordinate system based on the corrected acceleration and the moving distance detected by the distance detecting means; It is a free-standing position calculation system for navigation, characterized in that a velocity correction means for correcting the angular velocity detected by the angular velocity detecting means by using an oblique angle.
[0012]
According to the second aspect, it is possible to know an accurate position of the moving body even when the housing is attached to the moving body at an angle. As a result, the degree of freedom regarding the installation is increased, and the burden on the user can be reduced.
[0013]
BEST MODE FOR CARRYING OUT THE INVENTION
(Embodiment 1)
Hereinafter, specific embodiments of the first invention will be described with reference to the drawings. FIG. 1 shows a system configuration of a position calculation system for self-contained navigation, 1 is an angular velocity detecting means for detecting an angular velocity of a moving body, 2 is an acceleration detecting means for detecting an acceleration of the moving body, and 3 is a traveling distance of the moving body. Detecting means 4 for calculating the inclination angle of the moving body based on the outputs of the angular velocity detecting means 1, acceleration detecting means 2 and distance detecting means 3; Angular velocity correcting means for correcting the value of the angular velocity obtained by the angular velocity detecting means 1 based on the inclination angle of the body.
[0014]
FIG. 2 is a perspective view showing the configuration of each sensor of the position calculation system for self-contained navigation. The angular velocity detecting means 1 detects the angular velocities of the moving body in the yaw direction and the pitch direction as indicated by reference numerals 6 and 7, respectively. It is constituted by an angular velocity sensor, and is attached so that the detection axes of the angular velocity are orthogonal to each other. As an example of the angular velocity sensors 6 and 7, a crystal-type vibrating gyroscope generally used for car navigation may be used. The acceleration detecting means 2 is composed of acceleration sensors for detecting the acceleration of the moving body in the vertical direction, the left-right direction, and the front-rear direction, as indicated by 8, 9, and 10, respectively. As an example of the acceleration sensor, a small capacitance type acceleration sensor or the like may be used.
[0015]
The distance detecting means 3 is attached so as to detect the moving distance of the moving body in the traveling direction. When a moving object is a car, a vehicle speed pulse generally used in a car navigation system may be used as the distance detecting means. The vehicle speed pulse has a small error and a stable output can be obtained. The configuration includes a pulse generator and a pulse detector, and the number of pulses output by a generator attached to an axle or a wheel axle is read by a detector to calculate a moving distance of the moving body. FIG. 2 shows only the pulse detector 11 for simplicity.
[0016]
FIG. 3 is a flowchart showing the overall processing flow of the self-contained navigation position calculation system configured as described above. When the system starts, each data is acquired from the angular velocity sensors 6, 7, the acceleration sensors 8, 9, 10 and the pulse detector 11 at regular intervals (Process 1). Using the obtained angular velocity data, acceleration data, and distance data, the inclination angle of the moving object is calculated (Process 2). Next, the yaw direction angular velocity is corrected from the angular velocity data obtained in the processing 1 using the calculated inclination angle (processing 3). Then, the azimuth of the moving object is calculated by time-integrating the corrected value of the angular velocity, and the position of the moving object is calculated together with the distance data (process 4).
[0017]
Hereinafter, the above-described processes 1 to 4 will be specifically described. The following symbols are defined for explanation.
θ ₁ : yaw angle of the moving body θ ₂ : pitch angle of the moving body θ ₃ : roll angle of the moving body p: angular velocity of the moving body in the roll direction q: output of the angular velocity sensor 7 for detecting angular velocity in the pitch direction r: yaw The output gx of the angular velocity sensor 6 for detecting the angular velocity in the direction: the output gy of the acceleration sensor 10 for detecting the acceleration in the front-back direction: the output gz of the acceleration sensor 9 for detecting the acceleration in the left-right direction: the acceleration sensor for detecting the acceleration in the vertical direction Output D 8: Distance data Ax obtained from the pulse detector 11: Traveling direction acceleration Ay on the reference plane (pitch and roll values are 0): Centrifugal acceleration Az: Vertical acceleration G: Gravitational acceleration α: Reference plane In the yaw direction angular velocity processing 1, the data detected by the angular velocity sensors 6, 7, the acceleration sensors 8, 9, 10, and the pulse detector 11 are converted to the processing capacity of the arithmetic unit. To get in Flip sampling interval. The acquired data is subjected to preprocessing such as noise removal, and the distance data is converted into distance data D by multiplying the distance data by a distance coefficient.
[0018]
In process 2, the inclination angle is calculated using the distance data D, the acceleration data gx, gy, gz, and the angular velocity data p, q among the data obtained in process 1. Now, the moving body theta ₂ in the pitch _direction, the case where moving with an inclination of theta ₃ in the roll direction. FIG. 6 is a diagram showing the relationship between the inclination in the pitch direction and the acceleration applied to the moving object. From FIG.
(D ² D / dt ² ) = gx + Gsin θ ₂ (1)
Holds. The acceleration component Ax on the reference plane is
Ax = (d ² D / dt ² ) · cos θ ₂ (2)
And the vertical component Az of the acceleration is
Az = (d ² D / dt ² ) · sin θ ₂ (3)
Given by
[0019]
Next, FIG. 7 is a diagram illustrating a relationship between the inclination in the roll direction and the acceleration applied to the moving body.
[0020]
From FIG.
Ay = (gy−Gcos θ ₂ sin θ ₃ ) / cos θ ₃ (4)
G = (Aysin θ ₃ -gz) / cos θ ₂ cos θ ₃ (5)
Further, the centrifugal acceleration Ay of the equation (4) is expressed as follows using the yaw direction angular velocity α on the reference plane.
Ay = (dD / dt) · α (6)
Here, the correction method of the yaw direction angular velocity sensor in the processing 3 will be described first before the processing for calculating the inclination angle of the moving body in the processing 2. In Figure 8, a plane represented by x0-y0-z0 coordinate system as a reference plane, a plane can be rotated theta ₁ the reference plane around z0 axis and x1-y1-z1 plane. x1-y1-z1 plane a plane can be rotated theta ₂ to about y1 axis x2-y2-z2 plane and finally a plane the x2-y2-z2 plane can be theta ₃ rotates about x2 axis x3-y3 −Z3 plane. The plane x3-y3-z3 thus formed is the plane on which the moving object exists, and the correction can be performed by converting the value of the angular velocity obtained on this plane into the angular velocity on the reference plane. The matrices representing the rotational transformations of θ _n (n = 1, 2, 3) are T ₁ , T ₂ , and T _3, and the rotational angular velocity vector about the z0 axis on the reference plane is (0, 0, α), x1 If the rotation angular velocity vector around the y1 axis in the −y1-z1 plane is (0, β, 0) and the rotation angular velocity vector around the x2 axis in the x2-y2-z2 plane is (γ, 0, 0),
(P, q, r) ^T = T ₃ T ₂ T ₁ (0,0, α) ^T + T ₃ T ₂ (0, β, 0) ^T
+ T ₃ (γ, 0, 0) ^T (7)
Holds. Here, the value to be calculated is α, and the following correction formula is obtained by modifying the above formula (7).
α = (− qsin θ ₃ + rcos θ ₃ ) / cos θ ₂ (8)
Next, the method of calculating the inclination angle of the moving body in the process 2 is described. When a two-axis acceleration sensor in the front-rear direction and the left-right direction of the moving body is used as an example, the above equations (1), (4), ( By solving 6) and (8), the inclination angles θ ₃ and θ ₂ of the moving body can be obtained. As another example, when using a two-axis acceleration sensor in the left-right direction and the vertical direction of the moving body, θ ₃ and θ ₂ are calculated from the above equations (1), (4), (5), and (8). It is possible to ask.
[0021]
In process 3, the value of α is calculated from equation (8) using the inclination angles θ ₃ and θ ₂ obtained in this manner. In the processing 4, the position (x, y, z) of the moving body is calculated as follows using the angular velocity α and the moving distance D of the moving body corrected in the processing 3. However, the position before the movement is (x0, y0, z0). First, the azimuth on the reference plane of the moving object is
θ ₁ = ∫αdt (9)
The moving distance D 'on the reference plane is given by the following equation (2).
D ′ = ∬Axdt = D · cos θ ₂ (10)
Therefore, from equations (3), (9), and (10),
x = x0 + D · cos θ ₁ (11)
y = y0 + D · sin θ ₁ (12)
z = z0 + ∬Azdt = z0 + D · sin θ ₂ (13)
From the above equations (11), (12), and (13), the accurate position of the moving body on the reference plane can be known.
[0022]
If accurate position information of the moving object can be obtained, for example, in a car navigation system, even when traveling on a spiral multi-story parking lot with a complicated gradient or traveling on a road with a height difference, The position can be displayed correctly.
[0023]
(Embodiment 2)
Next, a specific embodiment of the second invention will be described with reference to FIGS. FIG. 4 is a configuration diagram of a position calculation system for self-contained navigation provided with a housing attachment angle calculation unit 13. In FIG. 4, the inclination detection unit 12 determines whether or not the road surface on which the moving object exists has an inclination. Device. For example, the use of a left and right vehicle height sensor, a pendulum-type inclinometer, or altitude information in map data is conceivable. Here, a case where a vehicle height sensor is used will be described as an example.
[0024]
FIG. 5 is a flowchart showing the flow of processing in the position calculation system for self-contained navigation provided with the housing mounting angle calculation means 13. In processing 1, the distance detection means 3, acceleration detection means 2, and angular velocity detection means 1 Acquire data at regular time intervals. At the same time, the inclination of the moving body is measured from the load applied to the four wheels by the vehicle height sensor. If there is a gradient on the road surface, or if the moving body has a slope due to curve running, etc., the casing mounting angle calculation processing of the processing 2 is not performed, and the inclinations of the casing in the yaw direction, the pitch direction, and the roll direction are all determined. The processing shifts to processing 3 as 0 deg. If the moving body has no inclination, the output of the distance detecting means is checked, and the housing mounting angle calculation processing of processing 2 is performed. Hereinafter, details of the processing will be described.
[0025]
First, when the output of the distance detecting means is 0, that is, when the moving body is stopped, the following equations (14), (15), (16), and (17) hold. However, as shown in FIG. 9, the initial housing mounting angle with respect to the traveling direction,
Y ₀ : Defined as the tilt angle in the yaw direction R ₀ : The tilt angle in the roll direction P ₀ : The tilt angle in the pitch direction.
[0026]
When D = 0,
gx = −GsinP ₀ (14)
gy = GcosP ₀ sinR ₀ (15)
gz = −GcosR ₀ cosP ₀ (16)
Is satisfied, and R ₀ and R ₀ are obtained by using two equations according to the installation condition of the acceleration sensor.
[0027]
Next, when D is not 0,
gx + GsinP ₀ = (d ² D / dt ² ) cosY ₀ (17)
It is holds, obtaining _{Y 0} From this equation.
[0028]
In the housing mounting angle calculation process of process 2, after the system is started, the output of the vehicle height sensor and the distance sensor first performs the process only once that satisfies the above condition, and thereafter uses the calculated mounting angle.
[0029]
In the process 3, first, the input of the angular velocity sensor and the acceleration sensor is used to calculate the output of the angular velocity sensor and the acceleration sensor by the following equation (using the case mounting angle obtained by the case mounting angle calculating means). 18) to (22). However, the corrected output is defined by the following symbols.
r ′: Output of yaw direction angular velocity sensor 6 after correction by initial mounting angle q ′: Output of pitch direction angular velocity sensor 7 after correction by initial mounting angle gx ′: Front-back acceleration sensor 10 after correction by initial mounting angle Output gy ′: output gz ′ of the lateral acceleration sensor 9 after correction by the initial mounting angle gz ′: output r ′ = r / cosR ₀ cosP ₀ of the vertical acceleration sensor 8 after correction by the initial mounting angle (18)
q ′ = q / cosY ₀ cosR ₀ (19)
gx ′ = (gx + GsinP ₀ ) / cosY ₀ cosP ₀ (20)
gy ′ = (gy−GcosP ₀ sinR ₀ ) / cosY ₀ cosP ₀ (21)
gz ′ = gz / cosR ₀ cosP ₀ (22)
After correcting the output of each sensor by the above equation, the inclination angle is calculated using the corrected sensor output, and the azimuth correction processing is performed on q ′. Finally, the position of the moving object is calculated using the corrected value of the angular velocity and the distance obtained by the distance detecting means 3.
[0030]
The calculation of the inclination angle, the correction of the angular velocity, and the calculation of the position can be performed in the same manner as in the first embodiment.
[0031]
As described above, according to the second aspect, it is possible to eliminate an error in position calculation during traveling due to an initial mounting angle of the housing. Therefore, in the navigation system using the present invention, it is possible to reduce restrictions when mounting the housing, and it is possible to calculate an accurate position of the moving object even when the housing is distorted. Useful for
[0032]
【The invention's effect】
According to the position calculation system for self-contained navigation according to the present invention, even when an angular velocity sensor for calculating the azimuth of a moving object in self-contained navigation has a tilt, an accurate correction is always performed by performing a correction according to the tilt angle. The angular velocity can be maintained, and as a result, it is possible to calculate the accurate azimuth and position of the moving object. Therefore, if the present invention is applied to the self-contained navigation of the car navigation, it is possible to eliminate the positional deviation due to the azimuth error even when traveling on a spiral road or a sloped road.
[Brief description of the drawings]
FIG. 1 is a configuration diagram of a position calculation system for self-contained navigation according to Embodiment 1 of the present invention. FIG. 2 is a perspective view illustrating a configuration of each sensor of the position calculation system for self-contained navigation according to Embodiment 1 of the present invention. 3 is a flowchart showing a processing flow of a position calculation system for self-contained navigation according to the first embodiment of the present invention. FIG. 4 is a block diagram showing a configuration of a self-contained navigation position calculation system according to a second embodiment of the present invention. FIG. 6 is a flowchart showing a processing flow of the self-contained navigation position calculation system according to the second embodiment of the present invention. FIG. 6 is a diagram showing a relationship between the inclination of the moving body in the pitch direction and the acceleration applied to the moving body according to the first embodiment of the present invention. FIG. 7 is a diagram showing the relationship between the inclination of the moving body in the roll direction and the acceleration applied to the moving body according to the first embodiment of the present invention. FIG. 8 is the Euler angle according to the first embodiment of the present invention. Figure 10 shows a plan view of a yaw direction angular velocity correction system in the prior art showing the housing initial mounting angle according to the second embodiment of FIG. 9 the invention showing a coordinate transformation using [EXPLANATION OF SYMBOLS]
Reference Signs List 1 angular velocity detecting means 2 acceleration detecting means 3 distance detecting means 4 tilt angle calculating means 5 angular velocity correcting means 6 angular velocity sensor 7 detecting angular velocity in yaw direction 7 angular velocity sensor detecting angular velocity in pitch direction 8 detecting acceleration in vertical axis Acceleration sensor 9 Acceleration sensor 10 for detecting acceleration in the left-right direction axis Acceleration sensor 11 for detecting acceleration in the front-rear direction axis Pulse detector 12 Tilt detecting means 13 Housing mounting angle calculating means

Claims (6)

Angular velocity detecting means for detecting the angular velocity of the moving body in the yaw direction and the pitch direction,
Acceleration detecting means for detecting acceleration of at least two axes of the longitudinal axis, the lateral axis, and the vertical axis of the moving body,
Distance detecting means for detecting a moving distance of the moving body,
An inclination angle calculating unit that calculates an inclination angle of the moving body with respect to an arbitrary coordinate system based on the angular velocity detected by the angular velocity detecting unit, the acceleration detected by the acceleration detecting unit, and the moving distance detected by the distance detecting unit; ,
An angular velocity correction means for correcting an angular velocity detected by the angular velocity detection means based on the inclination angle obtained by the inclination angle calculation means. Angular velocity detecting means for detecting the angular velocity of the moving body in the yaw direction and the pitch direction,
Acceleration detecting means for detecting acceleration of at least two axes of the longitudinal axis, the lateral axis, and the vertical axis of the moving body,
A housing attachment angle calculation unit that calculates an attachment angle of the housing that fixes the angular velocity detection unit and the acceleration detection unit to the moving body based on the acceleration detected by the acceleration detection unit,
Distance detecting means for detecting a moving distance of the moving body,
The angular velocity detected by the angular velocity detecting means and the acceleration detected by the acceleration detecting means are corrected based on the mounting angle of the housing determined by the housing mounting angle calculating means, and the corrected angular velocity and the corrected Inclination angle calculating means for calculating an inclination angle of the moving body with respect to an arbitrary coordinate system based on the acceleration of the moving distance detected by the distance detecting means,
An angular velocity correction means for correcting the angular velocity detected by the angular velocity detection means using the inclination angle calculated by the inclination angle calculation means. 3. The position calculation system for self-contained navigation according to claim 1, wherein the arbitrary coordinate system is defined by a right-handed coordinate system in which a Z-axis is a gravity direction and an X-axis and a Y-axis are each orthogonal to the Z-axis. . The housing mounting angle calculating means includes an inclination detecting means for detecting whether the moving body is inclined, and the moving body is not inclined according to a detection result of the inclination detecting means, and the distance calculation is performed. 3. The self-contained navigation position according to claim 2, wherein when it is determined from the output of the means that the moving body is not moving, the mounting angle of the housing is calculated based on the output of the acceleration detecting means. Calculation system. 3. The self-supporting apparatus according to claim 1, wherein the angular velocity correction unit corrects a value of the yaw direction angular velocity detected by the angular velocity detection unit based on the inclination angle detected by the inclination angle calculation unit. Navigational position calculation system. 3. The position calculation system for self-contained navigation according to claim 1, wherein the position of the moving object is calculated from the angular velocity corrected by the angular velocity correction unit and the moving distance detected by the distance detection unit.

Images