JP4345779B2 - Navigation device and position detection method - Google Patents

Description

  The present invention relates to a navigation apparatus that detects the position of a moving body, and more particularly, to a navigation apparatus that evaluates positioning and a positioning error of a moving body by inertial navigation.

  In navigation devices, radio navigation that detects the position of the host vehicle based on radio waves from a GPS (Global Positioning System) satellite and inertial navigation that detects the position by a behavior sensor that detects the behavior of the host vehicle such as speed are used. .

  However, it is known that positioning by these radio navigation and inertial navigation includes large and small errors, and when performing various vehicle controls using the position of the host vehicle, the degree of error may affect. is there.

  For this reason, a method for evaluating a positioning error by radio navigation or inertial navigation is required. However, there is a conventional error evaluation method for positioning by radio navigation even if the error is small or large.

On the other hand, an error evaluation method for positioning by inertial navigation has been proposed (for example, see Patent Document 1). The error evaluation method described in Patent Document 1 is based on the travel distance and heading error of the host vehicle detected by the distance sensor / heading sensor, the calculation error when the heading is not constant, and the accumulated error. Estimate the positioning accuracy of your vehicle. This estimation is performed by a probability distribution, and is expanded at a speed according to the accuracy of the sensors. Therefore, the error of the positioning position by inertial navigation can be evaluated by the probability distribution.
Japanese Patent Laid-Open No. 10-54729

  However, the evaluation method described in Patent Document 1 only considers only the error due to the distance sensor / azimuth sensor after all, and does not sufficiently examine the factors that affect the position of the moving body. For example, a running vehicle changes its position under the influence of vehicle attributes such as the position of the center of gravity. Therefore, in order to evaluate the error of positioning by inertial navigation with an error that takes into account only the distance sensor / azimuth sensor not enough.

  In view of the above problems, an object of the present invention is to provide a navigation device that can accurately evaluate errors in positioning by inertial navigation.

In order to solve the above-mentioned problems, the present invention provides an autonomous sensor that detects movement information of a moving body and an inertial positioning position of the moving body by accumulating information detected by the autonomous sensor in a navigation device that detects the position of the moving body. Applying the inertial positioning means to detect and the error variance of the inertial positioning position at time t, the sensor error variance and the calculation error of the autonomous sensor to the update formula based on the moving model (for example, dynamics model) of the moving object, An error variance calculating means for incrementally calculating an error variance at time t + 1 , and the predetermined range based on whether or not a range in which the moving body exists with a predetermined probability in the error variance overlaps at least a part with a predetermined area. Determining means for determining whether or not the moving object is present in an area .

  It is possible to provide a navigation device that can accurately evaluate errors in positioning by inertial navigation.

  The best mode for carrying out the present invention will be described below with reference to the drawings. First, an outline of error evaluation of the position of the host vehicle will be described.

  FIG. 1 is a diagram showing an outline of error evaluation of the own vehicle position. In this embodiment, error variance is used for error evaluation. Then, the error variance at the time t, the error variance of the sensor value, and the rounding error of the calculation are applied to the update formula to calculate the error variance of the vehicle position at the time t + 1.

  Instead of simply accumulating the error of the sensor value, the error variance of the own vehicle position is derived gradually using the update formula, so that the error of the own vehicle position can be evaluated with high accuracy. Also, since the update formula is set based on the vehicle dynamics model, it is possible to derive a positioning error according to the nature of the moving object.

  FIG. 2A shows a schematic configuration diagram of the navigation device 1. The navigation device 1 is controlled by a navigation ECU (Electrical Control Unit) 10 that controls the navigation device 1. The navigation ECU 10 includes a CPU that executes a program, a storage device (hard disk drive, ROM) that stores the program, a RAM that temporarily stores data and programs, an input / output unit that inputs and outputs data, and NV (Non Volatile) − A RAM or the like is configured as a computer connected via a bus.

  The navigation ECU 10 includes a GPS receiver 11 that receives radio waves from a GPS satellite, a vehicle speed sensor 12 that detects a vehicle speed, a yaw rate sensor 13 that detects a rotational angular velocity around the vertical axis of the vehicle, and a map database that stores map data (hereinafter, referred to as a map database). (Map DB) 14, an input device 15 for operating the navigation device 1, and a display device 16 for displaying the map and the current position of the vehicle are connected. In addition, you may provide other autonomous sensors, such as a gyro sensor and a rudder angle sensor.

  The GPS receiver 11 outputs the position of the host vehicle based on the radio wave from the GPS satellite by a known method. The GPS receiver 11 calculates a distance to the GPS satellite based on arrival times of radio waves transmitted from a plurality of GPS satellites, and sets one point where the distance between the three or more GPS satellites and the own vehicle intersects the own vehicle. Position as a position.

  The vehicle speed sensor 12 is a sensor that outputs a pulse signal as the tire rotates, and detects the vehicle speed based on the number of pulses detected for each sampling time. The relationship between the number of pulses and the travel distance is calculated as a pulse coefficient from the relationship of “travel distance ÷ number of pulses” with the travel distance known in advance. However, when the tire air pressure (diameter) changes, the pulse coefficient differs, and in the low speed state, the detection of the travel distance by the pulse may cause an error.

  The yaw rate sensor 13 is a sensor that generates a voltage or the like proportional to the rotational angular velocity around the vertical axis of the vehicle, and converts the output of the yaw rate sensor 13 from the voltage or the like to the rotational angular velocity using a proportional constant. When the detection axis attached to the vehicle body and the turning axis of the vehicle do not match, the yaw rate sensor 13 outputs a rotational angular velocity including an error because a preset proportionality constant is different from an actual conversion ratio. End up. In addition, since the yaw rate sensor 13 has a detection axis arranged along a predetermined direction (for example, the direction of gravity), when the vehicle turns on an inclined surface, the conversion ratio changes to include an error.

  The map DB 14 is composed of a hard disk, CD-ROM, DVD-ROM or the like, and stores road map information such as road networks and intersections in association with latitude and longitude. In the map DB 5, information related to nodes (points where roads and roads intersect, points separated from intersections at predetermined intervals, etc.) and links (roads connecting nodes and nodes) are associated with the actual road network. It is stored in a table-like database consisting of information related to.

  The input device 15 is an interface for inputting an operation from the driver, which includes a touch panel, a push-down keyboard, buttons, a remote controller, a cross key, and the like. Further, an operation may be input by providing a microphone and recognizing a voice produced by the driver with a voice recognition circuit. When performing a route search to a destination, the driver can input the destination by an address, a place name, a landmark name, a zip code, or the like.

  The display device 16 is composed of liquid crystal, organic EL, HUD (Head Up Display), etc., and displays a road map around the host vehicle or a road map of a specified area according to a specified scale, and if necessary. The position of the vehicle and the route to the destination are displayed on the road map. In addition, the display device 16 includes a speaker, and guides a traveling direction along a route such as an intersection where the speaker turns right and left by voice.

  FIG. 2B shows a functional block diagram of the navigation ECU 10. When the CPU of the navigation ECU 10 executes the program, inertial positioning means 10a for accumulating detection information from the vehicle speed sensor 12 and yaw rate sensor 13 to detect the positioning position of the host vehicle, and error distribution of the positioning position based on the dynamics model. An error variance calculation unit 10b that derives gradually, an error ellipse calculation unit 10d that calculates a reliability error ellipse, and a determination unit 10d that determines whether or not the reliability error ellipse overlaps a predetermined region are realized. The dynamics model and Mahalanobis distance are stored in the storage device of the navigation ECU 10.

  In the present embodiment, the inertial positioning means 10a accumulates detection information from the vehicle speed sensor 12 and the yaw rate sensor 13 and detects (estimates) the positioning position of the host vehicle by a known method. Then, the error variance calculating means 10b calculates the error variance of the lateral position.

[Derivation of error variance by update formula]
Derivation of error variance by the error variance calculation means 10b will be described in detail. In this embodiment, a dynamics model for estimating the position of the vehicle is set. When the position information and sensor information of the host vehicle at time t are input to the dynamics model, the dynamics model outputs an estimated position at time t + 1. Therefore, the dynamics model is transformed to derive the error variance at the estimated position at time t + 1 from the error variance at time t. The dynamics model will be described later.

True position of the vehicle and direction X _t, the translational velocity and rotational angular U _t each represent the following (both vector quantity). The translation speed is not the vehicle speed in the traveling direction but the vehicle speed in the vehicle length direction and the vehicle width direction.

X _t = (x, y, θ)
U _t = (v _t , w _t )
If an expression for estimating the position of the own vehicle by inputting X _t and U _t (hereinafter simply referred to as a dynamic model) is f (X _t , U _t ), the position X _{t + 1} of the own vehicle at time _{t + 1} is It can be expressed as:

_{Xt + 1} = f ( _Xt , _Ut ) + _nt (1)
Here, n _t is an error caused by quantization or calculation rounding error.

Here, placing the [Delta] X _t errors of _{X t,} the error of the .DELTA.U _{t U t,} the estimated value of the X ^ _t X _t (mean value), the U ^ _t estimate of U _t (mean value), and.

  The vehicle position estimation formula is defined as follows.

X ^ _{t + 1} = f (X ^ _t , U ^ _t ) (2)
Taylor expansion is performed to linearize equation (2).

_{Xt + 1} = f ( _Xt , _Ut ) + _{nt
= F (X ^ t + ΔX} t, U ^ t + ΔU t) + n t
_{≒ f (X ^ t, U} ^ t) + J x ΔX t + J u ΔU t + n t
= X ^ _{t + 1} + ΔX _{t + 1}
However, J _x and _Ju are expressed as follows, and the relationship of Expression (2) was used from the third line to the fourth line.

また、３行目と４行目から次の関係が得られるので、この式から誤差分散を導出する更新式を求めることができる。
ΔＸ_t+1 ＝ Ｊ_xΔＸ_ｔ ＋ Ｊ_uΔＵ_ｔ ＋ ｎ_t
以上から、推定位置の誤差分散は次のように表すことができる。式（３）が誤差分散を漸化的に導出する更新式となる。但し、
Σ_Xt＝Ｅ（ΔＸ_tΔＸ_t ^T）、Σ_Ut＝Ｅ（ΔＵ_tΔＵ_t ^T）、Σ_n＝Ｅ（ΔｎΔｎ^T）
と置いた（右上のＴは転置行列を表す）。Ｅは期待値を表す記号であり、したがってΣは共分散行列を示す。

Further, since the following relationship is obtained from the third and fourth rows, an update equation for deriving the error variance can be obtained from this equation.
_{ΔX t + 1 = J x ΔX} t + J u ΔU t + n t
From the above, the error variance of the estimated position can be expressed as follows. Equation (3) is an update equation that gradually derives the error variance. However,
_{Σ Xt = E (ΔX t ΔX} t T), Σ Ut = E (ΔU t ΔU t T), Σ n = E (ΔnΔn T)
(T in the upper right represents a transposed matrix). E is a symbol representing an expected value, and therefore Σ represents a covariance matrix.

第１項〜第３項の内容はそれぞれ次のようになる。

The contents of the first to third terms are as follows.

First term: the amount of the estimated position error at time t on the estimated position error at time t + 1 Second term: sensor value error Third term: calculation rounding error, error due to other factors Therefore, as can be seen from the update equation, By determining Σ _X0, Σ _{U0, and} Σ _n at time t = 0, the error variance of the estimated position can be derived gradually.

[Dynamics model]
Next, the dynamics model will be described. FIG. 3 is a diagram showing a geometric vehicle model as an example of a dynamics model. Note that the dynamics model of the present embodiment is an example, and even if other models are appropriately set, the dynamics model can be suitably used for derivation of the error variance of the present embodiment.

  In the dynamic model of FIG. 3, the vehicle 20 is a four-wheeled vehicle, but for the sake of explanation, three wheels including a front Fc (steering wheel) and rear wheels Rl and Rr are shown. The total length L of the vehicle is L = Lf (front side) + Lr (rear side) starting from the center of gravity.

Hereinafter, the translational speed v _t and the rotational angular velocity w _t of the vehicle at the center of gravity are obtained.
A. The average vehicle speed of the four wheels detected by the vehicle speed sensor 12 is approximated to the vehicle speed Vstr.
B. The vehicle speed Vstr is disassembled in the vehicle length direction and the vehicle width direction. In order to disassemble, the steering angle α is detected by the steering angle sensor.

・ Speed in the vehicle length [m / s]
V_model_x = Vstr x cos (α)
・ Vehicle width direction speed V_model_fy = Vstr x sin (α)
C. Calculate the translational speed and rotational angular velocity of the center of gravity. Assuming that the lateral translation speed at the position of the rear wheel of the vehicle is zero, the translation speed / rotational angular speed of the center of gravity position is calculated according to the distance from the front wheel position to the center of gravity.

-Coefficient determined by the distance from the front wheel position to the center of gravity position K_wheelbase = Lr / L
・ Translation speed [m / s] of the center of gravity
V_model_x
・ Translation speed of the center of gravity in the vehicle width direction [m / s]
V_model_gy = V_model_fy x K_wheelbase
-Center of gravity angular velocity [rad / s]
yaw_model_g = V_model_fy / L
From the above, Vt and Wt are as follows.

車両の真の位置・方位Ｘ_tについては並進速度・回転角速度から算出される。すなわち、前回の位置・方位に、並進速度・回転角速度に基づく変化量を累積する。但し、Ｓ_tはサンプリング時間。

The true position and orientation X _t of the vehicle is calculated from the translational speed and rotation angular velocity. That is, the amount of change based on the translation speed / rotational angular speed is accumulated in the previous position / orientation. Where _St is the sampling time.

_{x t + 1 = x t +} V_model_x × S t
y _{t + 1} = y _t + V_model_gy × S _t
θ _{t + 1} = θ _t + w _t × S _t
As described above, by setting the dynamics model, it is possible to derive a function form (dynamics model) for calculating the true position / orientation of the vehicle and the translational speed / rotational angular speed.

  According to the present embodiment, the error variance at the time t + 1 is gradually derived by applying the error variance at time t, the error variance of the sensor value, and the rounding error of the calculation to the update formula. can do. The error can be accurately evaluated for positioning by inertial navigation by error variance.

  FIG. 4 is a diagram illustrating an example of a positioning result and error variance by inertial navigation. The host vehicle exists at the true position O and is traveling toward the intersection P. An error variance Qv indicated by a dotted line derived by an update formula is obtained for the positioning position Q measured by inertial navigation. In addition, in order to show the area | region of the intersection P, the concentric intersection area | region Pv centering on the node P of the intersection was shown.

  According to the positioning position Q, the own vehicle does not exist in the intersection area Pv. However, the positioning position Q has an error and may actually exist in the intersection area Pv.

  Therefore, for example, when the user's vehicle is present at an intersection and a warning to alert the driver is heard, the determination based only on the positioning position Q is not sufficient.

  On the other hand, in this embodiment, since the error variance Qv of the estimated position as well as the positioning location Q is derived, if the error variance Qv overlaps the intersection area Pv, there is a possibility that the host vehicle exists near the intersection. Detect that there is.

  According to this embodiment, even when the vehicle position is detected by inertial navigation, the position of the vehicle is detected in a wide range, so that accurate vehicle control is possible. In addition, since the spread of the position of the host vehicle is derived based on a dynamics model that takes into account factors that affect the position of the moving body, the positioning error of inertial navigation can be suitably evaluated.

  By the way, when the positioning position Q and the error variance Qv of the own vehicle are obtained as shown in FIG. 4, there is a risk that it is determined that the vehicle exists at the intersection even if the vehicle exists at a position far from the intersection depending on the error variance Qv.

  Therefore, in this embodiment, a reliability error ellipse based on the Mahalanobis distance D is calculated, and it is determined whether or not the host vehicle exists in the intersection area Pv based on whether or not the reliability error ellipse overlaps the intersection area Pv. .

  FIG. 5 is a diagram showing an outline of determination of the vehicle position by the reliability error ellipse. As shown in FIG. 5, in order to generate a reliability error ellipse, the “own vehicle position (positioning position)”, “own vehicle position error variance” and “own vehicle existence probability (Mahara's screw distance D)” are Use.

The calculation formula of the reliability error ellipse is as follows.
Reliability error ellipse = (X−X ^* ) ^T Σ _Xt ⁻¹ (X−X ^* ) = D (4)
X ^* is "own vehicle position (positioning position)", and Σ _Xt is "error variance of own vehicle position (first term of update formula)", so input Mahalanobis distance D assuming an appropriate existence probability Then, the error ellipse calculation means 10c can determine the reliability error ellipse.

  Further, the determination unit 10d extracts the position information of the intersection node from the map DB 14, and determines, for example, an area having a predetermined radius as the intersection area Pv according to the width.

  If the reliability error ellipse and the intersection area Pv overlap, the determination unit 10d determines that the own vehicle exists in the intersection. If the reliability error ellipse and the intersection area Pv do not overlap, the determination unit 10d determines that the own vehicle does not exist in the intersection. Can be judged.

  FIG. 6 is a diagram showing a determination result of determining whether or not the own vehicle exists at the intersection of the road actually traveled based on the reliability error ellipse.

  The host vehicle turns right at the intersection from the upper left direction (arrow direction) and travels downward in FIG. Since the navigation ECU calculates a reliability error ellipse every predetermined time during traveling, a plurality of reliability error ellipses are described. Since the reliability error ellipse indicates the error variance of the positioning position as described above, it is calculated along the locus of the positioning position.

  An experiment was conducted to determine whether or not the vehicle was determined to be present at the intersection shown by the reliability error ellipse. That is, it is determined whether or not at least a part of the reliability error ellipse overlaps with the intersection area during the time when the host vehicle actually enters the intersection until the vehicle passes the intersection. In addition, it was confirmed visually that the host vehicle actually entered the intersection.

  The test run was performed 20 times, and for the sake of comparison, the test run that was determined based on only the positioning position without considering the positioning error was similarly performed 20 times.

  When determined by the reliability error ellipse, it was determined that the vehicle entered the intersection area in all 20 of the 20 runs. On the other hand, when it is determined only by the positioning position without considering the positioning error, it is determined that 14 times out of 20 runs are in the intersection, but the remaining 6 times the own vehicle is actually at the intersection. In some cases, it was not detected. From these experimental results, it can be seen that the determination based on the reliability error ellipse is effective.

  According to the present embodiment, it can be reliably determined whether or not the host vehicle is present at a predetermined position. Since the size of the reliability error ellipse can be controlled by the Mahara's screw distance D, it is possible to minimize erroneous determinations such as determining that the reliability error ellipse exists at the intersection even though it is far from the intersection.

It is a figure which shows the outline of the error evaluation of the own vehicle position. It is a schematic block diagram of a navigation apparatus. It is a figure which shows an example of a dynamics model. It is a figure which shows an example of the positioning result by an inertial navigation, and error dispersion | distribution. It is a figure which shows the outline of the determination of the own vehicle position by a reliability error ellipse. It is a figure which shows the determination result which determined whether the own vehicle exists in the intersection of the road which actually drive | worked based on the reliability error ellipse.

Explanation of symbols

1 navigation device 10 navigation ECU
11 GPS receiver 12 Vehicle speed sensor 13 Yaw rate sensor 14 Map DB
15 Input device 16 Display device

Claims (8)

In a navigation device that detects the position of a moving object,
An autonomous sensor for detecting behavior information of the moving body;
Inertial positioning means for accumulating detection information by the autonomous sensor to detect the inertial positioning position of the moving body;
The error variance at the time t + 1 is gradually applied by applying the error variance of the inertial positioning position at time t, the sensor error variance and the calculation error of the autonomous sensor to the update formula based on the movement model of the moving object. Error variance calculating means for calculating;
Determination means for determining whether or not the moving object exists in the predetermined area based on whether or not a range where the moving object exists with a predetermined probability in the error variance overlaps at least partly with the predetermined area. When,
A navigation device comprising: Error ellipse calculation means for calculating a reliability error ellipse based on the inertial positioning position, the error variance, and the Mahalanobis distance;
The reliability error ellipse, and the predetermined region and determining means for determining whether at least a part of the movable body in the predetermined area based on whether a duplicate exists,
The navigation apparatus according to claim 1, further comprising: The moving model of the moving object is a dynamics model of the moving object.
The navigation apparatus according to claim 1 or 2, wherein The autonomous sensor is at least one of a vehicle speed sensor, a yaw rate sensor, a gyro sensor, and a rudder angle sensor.
The navigation apparatus according to claim 1 or 2, wherein Wherein the error variance calculation unit, a navigation device according to claim 1 or 2, wherein, wherein the calculating on the basis of the error dispersed in the updating formula.

In a position detection method for detecting the position of a moving object,
An autonomous sensor detecting behavior information of the moving body;
An inertial positioning means for accumulating detection information by the autonomous sensor to detect an inertial positioning position of the moving body;
The error variance calculating means applies the error variance of the inertial positioning position at time t, the sensor error variance and the calculation error of the autonomous sensor to the update formula based on the moving model of the moving object, and the error at time t + 1. Calculating the variance incrementally;
The determination means determines whether or not the moving body exists in the predetermined area based on whether or not a range in which the moving body exists with a predetermined probability in the error variance overlaps at least a part with the predetermined area. Determining step,
A position detection method comprising: An error ellipse calculating means calculating a reliability error ellipse based on the inertial positioning position, the error variance, and the Mahalanobis distance;
Determining means, said determining whether the moving body is present in a given area and at least partially based on whether overlaps said predetermined area,
The position detection method according to claim 6, further comprising : The error variance is calculated based on the following update formula,
The position detection method according to claim 6 or 7 , characterized in that

Images