JP2022176322A - Self-position estimation device, control method, program, and storage medium - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a self-position estimation device capable of suitably suppressing precision of self-position estimation from decreasing even if an error occurs to one of a map or a measured value.

SOLUTION: A self-vehicle position estimation part 17 of an on-vehicle machine 1 has a position prediction part 21 and a position estimation part 22 which generate information indicative of a predicted self-vehicle position X^-(t). The position estimation part 22 calculates a feature predicted value Z^-(t) predicted based upon a feature measured value Z(t) indicative of a measured distance from a moving body to an object by a lidar 2 and feature information. The position estimation part 22 determines coefficient value a_X(t), a_Y(t) for correcting a Karman gain K(t) according to difference evaluated values Ex, Ey based upon difference values dx, dy between a feature measured value and a feature predicted value, position estimation precision σ_P and lidar measurement precision σ_L.

SELECTED DRAWING: Figure 7

COPYRIGHT: (C)2023,JPO&INPIT

Description

The present invention relates to self-location estimation technology.

2. Description of the Related Art Conventionally, there has been known a technique of detecting a feature installed in a vehicle's traveling destination using a radar or a camera and calibrating the own vehicle position based on the detection result. For example, Patent Literature 1 discloses a technique of estimating a self-position by collating the output of a measurement sensor with the position information of a feature registered in advance on a map. Further, Patent Document 2 discloses a vehicle position estimation technique using a Kalman filter.

JP 2013-257742 A JP 2017-72422 A

When estimating the self-position by comparing the output of the measurement sensor with the position information of the feature registered in advance on the map, if there is an error in either the map or the output of the measurement sensor, the self-position will be incorrect. is corrected, and the accuracy of self-localization is degraded. Situations in which an error occurs in the output of the measurement sensor include the presence of many objects with high reflection intensity, such as delineation posts and reflectors of other vehicles, and erroneously detecting them as target features. In some cases, other vehicles exist between the vehicle and the target feature, and all or part of the scan by the measurement sensor is occluded. In addition, as a situation in which an error occurs in the map, there is a case where the latest information is not reflected in the map and the position coordinates of the features are inaccurate.

SUMMARY OF THE INVENTION The present invention has been made to solve the above-described problems, and is capable of suitably suppressing a decrease in self-position estimation accuracy even when an error occurs in either a map or measurement values. The main object is to provide a self-localization device capable of

The invention according to claim 1 is a self-position estimating device, which is a first acquisition unit that acquires predicted position information indicating a predicted self-position, and indicates the distance measured by the measurement unit from the moving object to the object. a second acquisition unit that acquires first distance information and second distance information that indicates a distance from the moving object to the object predicted based on the position information of the object; and self-position accuracy of the moving object. A third acquisition unit that acquires information, and a correction unit that corrects the predicted self-position based on the first distance information, the second distance information, and the self-position accuracy information.

1 is a schematic configuration diagram of a driving support system; FIG. 3 is a block diagram showing a functional configuration of an in-vehicle device; FIG. It is an example of the data structure of map DB. It is the figure which represented the state variable vector with the two-dimensional orthogonal coordinate. FIG. 5 is a diagram showing a schematic relationship between a prediction step and a measurement update step; 3 shows functional blocks of a vehicle position estimator. It shows the positional relationship between the estimated vehicle position of the vehicle and the feature. 4 is a flowchart of self-vehicle position estimation processing; 3 shows experimental results of vehicle position estimation processing based on the present embodiment. 3 shows experimental results of vehicle position estimation processing based on the present embodiment. Fig. 10 shows experimental results of conventional vehicle position estimation processing when position coordinates of feature information of a certain feature are shifted by 80 cm in the traveling direction. FIG. 10 shows experimental results of vehicle position estimation processing based on the embodiment when position coordinates of feature information of a certain feature are shifted by 80 cm in the direction of travel. FIG.

According to a preferred embodiment of the present invention, the self-position estimation device includes a first acquisition unit that acquires predicted position information indicating a predicted self-position, and a measuring unit that indicates the measured distance from the moving body to the target a second acquisition unit that acquires first distance information and second distance information that indicates a distance from the moving object to the object predicted based on the position information of the object; and self-position accuracy of the moving object. a third acquisition unit that acquires information; and a correction unit that corrects the predicted self-position based on the distance difference value indicated by the first distance information and the second distance information and the self-position accuracy information. , provided. According to this aspect, the self-position estimation device can suitably correct the predicted self-position based on the distance difference value indicated by the first distance information and the second distance information and the self-position accuracy information. .

In one aspect of the self-position estimation device, the correction unit corrects the predicted self-position based on the difference value, the self-position accuracy information, and the measurement accuracy information of the measurement unit. In this aspect, the self-position estimation device can further take into consideration the measurement accuracy of the measurement unit and suitably correct the self-position.

In another aspect of the self-position estimation device, the correction unit calculates an evaluation value for evaluating the difference value based on the difference value and the self-position accuracy information, and determines the predicted self-position. A degree of correction based on the difference value is determined based on the evaluation value. According to this aspect, the self-position estimation device evaluates the above-described difference value based on the self-position accuracy information, thereby accurately determining the degree of correction of the predicted self-position using the above-described difference value. can. As a result, even if there is an error in the first distance information or the second distance information, it is possible to suitably suppress the deterioration of the vehicle position estimation accuracy. In this case, preferably, the correction unit corrects the predicted self-position by a value obtained by multiplying the difference value by a predetermined gain, and the correction unit adjusts the gain based on the evaluation value. A correction factor may be determined. More preferably, the gain is the Kalman gain.

In another aspect of the self-position estimation device, the second acquisition unit obtains the distance indicated by the second distance information based on the predicted position information and the position information of the object recorded in the map information. calculate. According to this aspect, the self-position estimation device can generate the second distance information based on the map information and suitably use it for self-position estimation.

According to another preferred embodiment of the present invention, there is provided a control method executed by a self-position estimation device, comprising: a first acquisition step of acquiring predicted position information indicating a predicted self-position; A second acquisition step of acquiring first distance information indicating the distance measured by the measuring unit to and second distance information indicating the distance from the moving object to the object predicted based on the position information of the object and a third acquisition step of acquiring the self-location accuracy information of the moving body, the difference value between the distances indicated by the first distance information and the second distance information, and the estimated self-location accuracy information. and a correction step of correcting the self-position. By executing this control method, the self-position estimation device can appropriately correct the predicted self-position.

According to another preferred embodiment of the present invention, there is provided a program executed by a computer, comprising: a first acquisition unit for acquiring predicted position information indicating a predicted self-position; a second acquisition unit that acquires first distance information indicating the distance measured by the object and second distance information indicating the distance from the moving object to the object predicted based on the position information of the object; a third acquisition unit that acquires self-location accuracy information of a body; and a difference value between distances indicated by the first distance information and the second distance information, and the predicted self-location based on the self-location accuracy information. The computer is caused to function as a correction section that performs correction. The computer can suitably correct the predicted self-position by executing this program. Preferably, the program is stored in a storage medium.

Preferred embodiments of the present invention will be described below with reference to the drawings. In this specification, for the sake of convenience, a character with "^" or " ^- " above any symbol is represented as "A ^{^} " or "A-" (where "A" is an arbitrary character).
[Outline configuration]

FIG. 1 is a schematic configuration diagram of a driving support system according to this embodiment. The driving support system shown in FIG. 1 is mounted on a vehicle and includes an in-vehicle device 1 that performs control related to driving support of the vehicle, a lidar (Light Detection and Ranging, or Laser Illuminated Detection And Ranging) 2, and a gyro sensor 3. , a vehicle speed sensor 4 and a GPS receiver 5 .

The in-vehicle device 1 is electrically connected to the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and based on the outputs of these, the position of the vehicle in which the in-vehicle device 1 is mounted (“vehicle position”). ) is estimated. Based on the result of estimating the position of the vehicle, the vehicle-mounted device 1 performs automatic operation control of the vehicle so that the vehicle travels along the set route to the destination. The in-vehicle device 1 stores a map database (DB: DataBase) 10 that stores road data and feature information that is information about features that serve as landmarks provided near roads. Examples of the features that serve as landmarks include features such as kilo-posts, 100-m posts, delineators, traffic infrastructure facilities (for example, signs, signboards, and signals), utility poles, and streetlights that are periodically arranged along the road. Then, the in-vehicle device 1 estimates the position of the vehicle by comparing it with the output of the rider 2 or the like based on the feature information. The in-vehicle device 1 is an example of the "self-position estimation device" in the present invention.

The lidar 2 discretely measures the distance to an object existing in the outside world by emitting a pulsed laser over a predetermined angular range in the horizontal and vertical directions, and generates a three-dimensional point indicating the position of the object. Generate group information. In this case, the lidar 2 includes an irradiation unit that irradiates laser light while changing the direction of irradiation, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and scan data based on light receiving signals output by the light receiving unit. and an output unit for outputting The scan data is generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the response delay time of the laser light specified based on the light reception signal described above. In general, the closer the distance to the target, the more accurate the lidar distance measurement, and the greater the distance, the less accurate. The rider 2 , the gyro sensor 3 , the vehicle speed sensor 4 and the GPS receiver 5 each supply output data to the onboard device 1 . The rider 2 is an example of a "measurement section" in the present invention.

FIG. 2 is a block diagram showing the functional configuration of the vehicle-mounted device 1. As shown in FIG. The in-vehicle device 1 mainly has an interface 11 , a storage section 12 , an input section 14 , a control section 15 and an information output section 16 . These elements are interconnected via bus lines.

The interface 11 acquires output data from sensors such as the rider 2 , the gyro sensor 3 , the vehicle speed sensor 4 , and the GPS receiver 5 and supplies the output data to the control unit 15 .

The storage unit 12 stores programs executed by the control unit 15 and information necessary for the control unit 15 to execute predetermined processing. In this embodiment, the storage unit 12 stores a map DB 10 including feature information. FIG. 3 shows an example of the data structure of the map DB 10. As shown in FIG. As shown in FIG. 3, the map DB 10 includes facility information, road data, and feature information. The feature information is information in which information about the feature is associated with each feature. Here, it is represented by a feature ID corresponding to the index of the feature, latitude and longitude (and altitude), etc. and position information indicating the absolute position of the feature. Note that the map DB 10 may be updated periodically. In this case, for example, the control unit 15 receives partial map information about the area to which the vehicle position belongs from a server device that manages map information via a communication unit (not shown), and reflects it in the map DB 10 .

The input unit 14 is a button, a touch panel, a remote controller, a voice input device, or the like for user operation. The information output unit 16 is, for example, a display, a speaker, or the like that outputs based on the control of the control unit 15 .

The control unit 15 includes a CPU that executes programs and the like, and controls the entire in-vehicle device 1 . In this embodiment, the controller 15 has a vehicle position estimator 17 . The control unit 15 is an example of the "first acquisition unit", the "second acquisition unit", the "third acquisition unit", the "correction unit", and the "computer" that executes the program in the present invention.

The vehicle position estimating unit 17 uses the gyro sensor 3, the vehicle speed sensor 4, and/or the GPS receiver based on the measured values of the distance and angle of the lidar 2 with respect to the feature and the position information of the feature extracted from the map DB 10. Correct the vehicle position estimated from the output data of 5. In this embodiment, as an example, the vehicle position estimating unit 17 performs a prediction step of estimating the vehicle position from the output data of the gyro sensor 3, the vehicle speed sensor 4, etc., based on a state estimation method based on Bayesian estimation, A measurement update step for correcting the estimated value of the vehicle position calculated in the prediction step is alternately executed. As the state estimation filter used in these steps, various filters developed for Bayesian estimation can be used, such as an extended Kalman filter, an unscented Kalman filter, and a particle filter. Thus, various methods have been proposed for position estimation based on Bayesian estimation. As an example, vehicle position estimation using an extended Kalman filter will be briefly described below.

FIG. 4 is a diagram showing the state variable vector x in two-dimensional rectangular coordinates. As shown in FIG. 4, the position of the vehicle on a plane defined on two-dimensional orthogonal coordinates of xy is represented by the coordinates "(x, y)" and the direction of the vehicle "Ψ". Here, the azimuth Ψ is defined as the angle between the direction of travel of the vehicle and the x-axis. The coordinates (x, y) indicate an absolute position, for example a combination of latitude and longitude.

FIG. 5 is a diagram showing a schematic relationship between the prediction step and the measurement update step. 6 shows an example of functional blocks of the vehicle position estimation unit 17. As shown in FIG. As shown in FIG. 5, by repeating the prediction step and the measurement update step, the estimated value of the state variable vector "X" indicating the vehicle position is sequentially calculated and updated. Further, as shown in FIG. 6, the vehicle position estimation unit 17 has a position prediction unit 21 that executes a prediction step and a position estimation unit 22 that executes a measurement update step. The position prediction unit 21 includes a dead reckoning block 23 and a position prediction block 24 , and the position estimation unit 22 includes a feature search/extraction block 25 and a position correction block 26 . In FIG. 5, the state variable vector of the reference time (that is, current time) “t” to be calculated is expressed as “X ⁻ (t)” or “X ^{^} (t)” (“state variable vector X(t)=(x(t), y(t), ψ(t)) ^T ”). Here, the provisional estimated value (predicted value) estimated in the prediction step is marked with " ^- " above the character representing the predicted value, and the more accurate estimated value updated in the measurement update step is attached with “ ^{^} ” above the character representing the value.

In the prediction step, the dead reckoning block 23 of the vehicle position estimator 17 calculates the moving speed "v" and the angular velocity "ω" of the vehicle (collectively, "control value u(t)=(v(t), ω( t))) ^T ”. The position prediction block 24 of the vehicle position estimator 17 adds the obtained movement distance and direction change to the state variable vector X ^{^} (t-1) at time t-1 calculated in the immediately preceding measurement update step, and obtains , the predicted value of the vehicle position at time t (also referred to as “predicted vehicle position”) X ⁻ (t). At the same time, the covariance matrix "P- ⁽ t)" corresponding to the error distribution of the predicted vehicle position X- ⁽ t) is changed to the covariance at time t-1 calculated in the previous measurement update step. It is calculated from the matrix “P ^{^} (t-1)”.

In the measurement update step, the feature search/extraction block 25 of the vehicle position estimation unit 17 associates the feature position vector registered in the map DB 10 with the scan data of the rider 2 . When the feature search/extraction block 25 of the vehicle position estimating unit 17 is able to establish this association, the measured value of the associated feature by the rider 2 (referred to as the "feature measurement value"). ) "Z(t)", the predicted vehicle position X- ⁽ t), and the position vector of the feature registered in the map DB 10 are used to model the measurement processing by the rider 2 to obtain the measured estimated value of the feature. (referred to as "feature prediction value"). Obtain " ^Z- (t)" respectively. The feature measurement value Z(t) is a two-dimensional value in the vehicle body coordinate system that is converted from the feature distance and scan angle measured by the rider 2 at time t into components with axes in the traveling direction and the lateral direction of the vehicle. is a vector. Then, the position correction block 26 of the vehicle position estimation unit 17 calculates the difference value between the feature measurement value Z(t) and the feature prediction value Z ⁻ (t) as shown in the following equation (1). do.

また、自車位置推定部１７の位置補正ブロック２６は、以下の式（２）に示すように、地物計測値Ｚ（ｔ）と地物予測値Ｚ^－（ｔ）との差分値にカルマンゲイン「Ｋ（ｔ）」を乗算し、これを予測自車位置Ｘ^－（ｔ）に加えることで、更新された状態変数ベクトル（「推定自車位置」とも呼ぶ。）Ｘ^＾（ｔ）を算出する。

Further, the position correction block 26 of the vehicle position estimator 17 calculates the difference value between the feature measurement value Z(t) and the feature prediction value Z ⁻ (t) as shown in the following equation (2). By multiplying the gain "K(t)" and adding it to the predicted vehicle position X- ⁽ t), the updated state variable vector (also called "estimated vehicle position") X ^{^} (t) is obtained as calculate.

また、計測更新ステップでは、自車位置推定部１７の位置補正ブロック２６は、予測ステップと同様、推定自車位置Ｘ^＾（ｔ）の誤差分布に相当する共分散行列Ｐ^＾（ｔ）（単にＰ（ｔ）とも表記する）を共分散行列Ｐ^－（ｔ）から求める。カルマンゲインＫ（ｔ）等のパラメータについては、例えば拡張カルマンフィルタを用いた公知の自己位置推定技術と同様に算出することが可能である。

In addition, in the measurement update step, the position correction block 26 of the vehicle position estimator 17 updates the covariance matrix P ^{^ (} t) (simply P(t)) is obtained from the covariance matrix P ⁻ (t). Parameters such as the Kalman gain K(t) can be calculated in the same manner as known self-position estimation techniques using an extended Kalman filter, for example.

Note that the vehicle position estimating unit 17 can associate the position vectors of the features registered in the map DB 10 with the scan data of the rider 2 for a plurality of features. The measurement update step may be performed based on the object measurement values or the like, or the measurement update step may be performed multiple times based on all the associated feature measurement values or the like. Note that when a plurality of feature measurement values are used, the vehicle position estimation unit 17 takes into account that the accuracy of the lidar measurement deteriorates as the feature is farther from the rider 2, and the distance between the rider 2 and the feature is The longer the feature, the smaller the weighting for the feature.

In this way, the prediction step and the measurement update step are repeatedly performed, and the predicted vehicle position X ⁻ (t) and the estimated vehicle position X ^{^} (t) are sequentially calculated, thereby obtaining the most probable vehicle position is calculated.

In the above description, the feature measured value Z(t) is an example of the "first distance information" of the present invention, and the feature predicted value ^Z- (t) is an example of the "second distance information" of the present invention. An example.

[Details of vehicle position estimation]
Next, details of vehicle position estimation in this embodiment will be described. Schematically, the vehicle position estimation unit 17 obtains an evaluation value (also referred to as a “difference evaluation value”) for evaluating the difference value between the feature measurement value Z(t) and the feature prediction value Z ⁻ (t). is calculated, and the Kalman gain K(t) is corrected according to the difference evaluation value. As a result, the vehicle position estimating unit 17 preferably suppresses deterioration of the vehicle position estimation accuracy even when an error occurs in either the map DB 10 or the measurement value of the rider 2 .

In EKF (Extended Karman Filter) position estimation using road signs using lidar, as shown in the above equation (2), the feature measured value Z(t) and the feature predicted value Z ⁻ (t) The vehicle position is calculated using the difference value. As can be understood from the equation (2), the larger the difference value shown in the equation (1), the larger the correction amount for the predicted vehicle position X ⁻ (t). Here, the reason why the difference value becomes large is due to one of the following causes (A) to (C).

(A) There is an error in the feature measurement value.
For example, if the measurement accuracy of the lidar is low, or if the measurement accuracy of the lidar is not low, but there is a moving object such as another vehicle between the feature to be measured and the vehicle at the time of measurement, the measurement accuracy is reduced. becomes low (that is, occlusion occurs).

(B) The estimated vehicle position used to obtain the feature prediction value is deviated.
This is the case when the predicted vehicle position X ⁻ (t) or the estimated vehicle position ^X̂ (t) has a large error.

(C) The position coordinates of the features in the map data used to obtain the feature prediction values are deviated.
For example, after the map data is created, the position of the feature is changed due to the inclination of the sign, which is the feature, due to a vehicle collision, etc., or the removal or movement of the structure, which is the feature. This is a state in which the positional coordinates of the feature in the data do not match the actual positional coordinates of the feature. In other words, there is a deviation between the position of the feature on the map data and the position of the feature in the real environment.

If the estimated vehicle position ^X̂ (t) is calculated based on the equation (2) when the difference value is large due to the above (A) or (C), the vehicle position estimation accuracy decreases. Therefore, when either (A) or (C) above occurs, it is preferable to make the degree of correction for the predicted vehicle position X ⁻ (t) as low as possible.

FIG. 7 shows the positional relationship between the estimated vehicle position of the vehicle and the features. The coordinate system shown in FIG. 7 is a body coordinate system with the vehicle as a reference, the x-axis indicates the traveling direction of the vehicle, and the y-axis indicates the direction perpendicular thereto (lateral direction of the vehicle). In FIG. 7, the estimated vehicle position has an error range indicated by an error ellipse 40. In FIG. The error ellipse 40 is defined by the position estimation accuracy "σ _P (x)" in the x direction and the position estimation accuracy "σ _P (y)" in the y direction. Here, the position estimation accuracy σ _P is obtained by transforming the covariance matrix P(t) into the body coordinate system using the Jacobian matrix "H(t)" according to the following equation (3).

式（３）において、「σ_Ｐ（ｘ）^２」はｘ方向の分散、「σ_Ｐ（ｙ）^２」はｙ方向の分散であり、これらの平方根としてｘ方向の位置推定精度σ_Ｐ（ｘ）とｙ方向の位置推定精度σ_Ｐ（ｙ）が得られる。こうして、位置推定精度σ_Ｐ（σ_Ｐ（ｘ），σ_Ｐ（ｙ））が得られる。

In equation (3), “σ _P (x) ² ” is the variance in the x direction, and “σ _P (y) ² ” is the variance in the y direction _. ) and the position estimation accuracy σ _P (y) in the y direction are obtained. Thus, the position estimation accuracy σ _P (σ _P (x), σ _P (y)) is obtained.

In FIG. 7, the feature prediction value ^Z- (t) is obtained by modeling the measurement processing by the rider 2 using the predicted vehicle position X- ⁽ t) and the feature position vector registered in the map DB 10. It is. Further, the feature measurement value Z(t) is a measurement value of the feature by the lidar 2 when the position vector of the feature registered in the map DB 10 and the scan data of the lidar 2 are associated with each other. The feature measurement value Z(t) has an error range indicated by an error ellipse 43 . The error ellipse 43 is defined by the x-direction measurement accuracy “σ _L (x)” and the y-direction measurement accuracy “σ _L (y)”. In general, the measurement error by the lidar increases in proportion to the square of the distance between the lidar and the feature to be measured. By calculating the error ellipse 43, the rider measurement accuracy σ _L (σ _L (x), σ _L (y)) is obtained. Also, the difference value between the feature predicted value Z ⁻ (t) and the feature measured value Z(t) is “dx” in the x direction and “dy” in the y direction, as shown in Equation (1).

The position estimation accuracy σ _P is an example of the "self-position accuracy information" of the present invention, and the rider measurement accuracy σ _L is an example of the "measurement accuracy information" of the present invention.

Next, a method of calculating an evaluation value for evaluating a difference value between the feature measured value Z(t) and the feature predicted value Z ⁻ (t) will be described. Using the position estimation accuracy σ _P (σ _P (x), σ _P (y)) and the rider measurement accuracy σ _L (σ _L (x), σ _L (y)), the vehicle position estimation unit 17 A difference value between the feature measurement value Z(t) and the feature prediction value Z ⁻ (t) is evaluated. In other words, the vehicle position estimator 17 determines the validity of the difference value based on the ratio of the difference value to the error range 40 and the rider error range 43 of the estimated vehicle position. Specifically, the vehicle position estimation unit 17 calculates difference evaluation values “Ex” and “Ey” using the following evaluation formula (4). In this case, the absolute values of the difference values dx and dy are used.

この場合、差分評価値Ｅｘ、Ｅｙが高いほど、上記の（Ａ）又は（Ｃ）のいずれかが生じている可能性が高いことが推定される。よって、自車位置推定部１７は、差分評価値Ｅｘ、Ｅｙの少なくとも一方が所定値より大きい場合、差分値が大きくなった原因として上記の（Ａ）又は（Ｃ）が生じていると判定する。よって、この場合、自車位置推定部１７は、差分評価値Ｅｘ、Ｅｙに応じて、カルマンゲインを小さくするための係数値「ａ_Ｘ（ｔ）」、「ａ_Ｙ（ｔ）」を算出し、カルマンゲインに乗じる。この場合、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）は、０から１までの値となり、かつ、差分評価値Ｅｘ、Ｅｙが大きいほど小さい値になるように、式又はテーブルを用いて設定される。例えば、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）は、以下の式（５）に基づき設定される。

In this case, it is estimated that the higher the difference evaluation values Ex and Ey, the higher the possibility that either (A) or (C) above occurs. Therefore, when at least one of the difference evaluation values Ex and Ey is larger than a predetermined value, the vehicle position estimation unit 17 determines that (A) or (C) above is the cause of the increase in the difference value. . Therefore, in this case, the vehicle position estimator 17 calculates coefficient values “a _X (t)” and “a _Y (t)” for reducing the Kalman gain according to the difference evaluation values Ex and Ey. , multiplied by the Kalman gain. In this case, the coefficient values _{aX(t) and aY (} t) are values from 0 to 1, and the larger the difference evaluation values Ex and Ey are, the smaller the values are. set. For example, coefficient values _{aX(t) and aY (} t) are set based on the following equation (5).

そして、自車位置推定部１７は、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を用いて、以下の式（６）に基づき更新後のカルマンゲイン「Ｋ（ｔ）´」を算出する。

Then, the vehicle position estimation unit 17 uses the coefficient values _{aX(t) and aY (} t) to calculate the updated Kalman gain "K(t)'" based on the following equation (6). .

このようにカルマンゲインを設定することで、自車位置推定部１７は、地物予測値の信頼度が低い場合に，その信頼度に応じてカルマンゲインを小さくし、予測自車位置Ｘ^－（ｔ）に対する補正量を少なくすることができる。よって、この場合，不正確な補正を防止できるため、推定自車推定精度が向上する。係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）は、本発明における「補正係数」の一例である。

By setting the Kalman gain in this way, when the reliability of the feature prediction value is low, the vehicle position estimation unit 17 reduces the Kalman gain according to the reliability, and the predicted vehicle position X ⁻ ( t) can be reduced. Therefore, in this case, since inaccurate correction can be prevented, the estimated own vehicle estimation accuracy is improved. The coefficient values _{aX(t) and aY (} t) are examples of "correction coefficients" in the present invention.

FIG. 8 is a flowchart of vehicle position estimation processing performed by the vehicle position estimation unit 17 of the vehicle-mounted device 1 . The in-vehicle device 1 repeatedly executes the processing of the flowchart of FIG.

First, the vehicle position estimator 17 sets the initial value of the vehicle position based on the output of the GPS receiver 5 (step S101). Next, the vehicle position estimator 17 acquires the vehicle body speed from the vehicle speed sensor 4 and acquires the angular velocity in the yaw direction from the gyro sensor 3 (step S102). Then, the vehicle position estimator 17 calculates the moving distance of the vehicle and the change in the orientation of the vehicle based on the result obtained in step S102 (step S103).

After that, the vehicle position estimating unit 17 adds the movement distance and the azimuth change calculated in step S103 to the estimated vehicle position X ^{^} (t-1) one time ago, and calculates the predicted vehicle position X ^- (t). is calculated (step S104). Further, in step S104, the vehicle position estimator 17 obtains the position estimation accuracies σ _P (x) and σ _P (y) from the covariance matrix using Equation (3). Further, the vehicle position estimating unit 17 refers to the feature information in the map DB 10 based on the predicted vehicle position X ⁻ (t), and searches for a feature that is the measurement range of the rider 2 (step S105).

Then, the vehicle position estimating unit 17 predicts the feature in each of the traveling direction and the lateral direction of the vehicle from the predicted vehicle position X ⁻ (t) and the position coordinates indicated by the feature information of the feature searched in step S105. A position (that is, a feature prediction value Z ⁻ (t)) is calculated (step S106). After that, the vehicle position estimating unit 17 calculates the measured distances (that is, the measured feature values Z(t)) to the features in each of the traveling direction and the lateral direction of the vehicle from the scan data of the rider 2, and from the measured distances The rider measurement accuracies σ _L (x) and σ _L (y) are calculated (step S107).

Next, the vehicle position estimation unit 17 calculates the difference values dx and dy between the feature measured value and the feature predicted value based on Equation (1) (step S108). Then, the vehicle position estimation unit 17 calculates the difference evaluation values Ex and Ey based on the formula (4) (step S109).

Then, when the difference evaluation values Ex and Ey are larger than the predetermined values (step S110; Yes), the vehicle position estimation unit 17 calculates the coefficient values _aX (t) and _aY (t) from the difference evaluation values Ex and Ey. Generate (step S111). For example, the vehicle position estimation unit 17 calculates the coefficient values _{aX(t) and aY (} t) based on the equation (5), so that the difference evaluation values Ex and Ey are The coefficient values _{aX(t) and aY (} t) are calculated so that the larger the value, the smaller the value. If there is a difference evaluation value that does not exceed a predetermined value, the vehicle position estimation unit 17 determines the coefficient value corresponding to the difference evaluation value (the coefficient value a _X (t )) may be set to 1 regardless of equation (5).

On the other hand, when the difference evaluation values Ex and Ey are equal to or less than the predetermined values (step S110; No), the vehicle position estimation unit 17 sets the coefficient values _{aX(t) and aY (} t) to "1" ( step S112). That is, the coefficient value _aX (t) is set by comparing the difference evaluation value Ex and a predetermined value, and the coefficient value _aY (t) is set by comparing the difference evaluation value Ey and a predetermined value.

Next, the vehicle position estimator 17 multiplies the Kalman gain K(t) by the coefficient values _aX (t) and _aY (t) based on Equation (6) to obtain the Kalman gain K(t)' is generated (step S113). Thereafter, the vehicle position estimation unit 17 uses the generated Kalman gain K(t)' instead of K(t) based on Equation (2) to correct the predicted vehicle position X ⁻ (t) and estimate The host vehicle position ^X̂ (t) is calculated (step S114).

[Concrete example]
Next, the effects of the vehicle position estimation processing based on this embodiment will be described.

FIG. 9A is a graph showing changes in position in the x-direction and y-direction when the vehicle is driven while executing the vehicle position estimation process shown in FIG. 8 on a running test course. In FIG. 9A, the vehicle position (also referred to as "reference position") measured with high accuracy using RTK-GPS and the estimated vehicle position calculated by the vehicle position estimation process shown in FIG. and are drawn respectively, but they overlap to such an extent that they cannot be visually distinguished.

FIG. 9(B) is an enlarged view of the data in the traveling section within the frame 70 of FIG. 9(A). In addition, in FIG. 9B, the positions of the features to be measured are plotted with circles. FIG. 10A is a graph showing transition of the difference value dx in the travel section framed 70, and FIG. FIG. 10(C) shows the difference in the traveling direction from the reference position, and FIG. 10(C) shows the difference in the lateral direction between the estimated vehicle position based on the embodiment and the reference position in the traveling section of frame 70, and FIG. 10(D). indicates the azimuth difference between the estimated vehicle position based on the embodiment and the reference position in the traveling section of the frame 70 .

As shown in FIG. 9(B), the vehicle position estimating unit 17 estimates the estimated vehicle position based on the feature information for features present at irregular intervals. The predicted vehicle position is corrected based on the difference value shown in . As a result, as shown in FIGS. 10(B) to 10(D), the difference between the reference position and the estimated vehicle position in the traveling direction and lateral direction of the vehicle is within approximately 0.1 m. The difference is also within about 0.1 degree.

FIGS. 11A to 11D show the Kalman gain K(t) when the position coordinates of the feature information for the feature indicated by the plot 71 in FIG. 9B are shifted by 80 cm in the traveling direction. Experimental results similar to FIGS. 10(A)-(D) when used without correction by _X (t) and a _Y (t) are shown.

As shown in FIG. 11(A), the position coordinates of the feature information of the feature indicated by the plot 71 are changed due to the deviation of the position coordinates of the feature information with respect to the feature indicated by the plot 71 of FIG. 9(B). The difference value dx (see the circled frame 72) calculated using the position coordinates of the other feature information has a significantly larger amount of deviation than the difference value dx calculated using the position coordinates of the other feature information. As a result, as shown in FIG. 11B, the estimated vehicle position calculated using the position coordinates of the feature information for the feature indicated by the plot 71 in FIG. There is a deviation (see circle frame 73). The error in the estimated vehicle position thus generated continues until the next detection of the feature used for vehicle position estimation (see plot 74 in FIG. 9).

FIGS. 12A to 12D show the Kalman gain K(t) when the position coordinates of the feature information for the feature indicated by plot 71 in FIG. Experimental results similar to FIGS. 11(A)-(D) when corrected by _X (t) and a _Y (t) are shown.

In this case, the difference value dx, the position estimation accuracy σ _P (x), and the lidar measurement accuracy σ _L (x) at the time when the feature indicated by the plot 71 in FIG. 9B is detected are as follows.
dx = -0.749
σ _P (x)=0.048
σ _L (x)=0.058

Therefore, substituting these values into the coefficient value _aX (t) shown in equation (5), the coefficient value _aX (t) is
0.854×10 ⁻³ ≈0
becomes. In this case, the Kalman gain K(t)' is calculated as shown in the following equation (7).

よって、この場合、車両の進行方向については、予測自車位置Ｘ^－（ｔ）の補正が行われないため、精度の良い位置推定が維持されることになる。

Therefore, in this case, since the predicted vehicle position X ⁻ (t) is not corrected with respect to the traveling direction of the vehicle, accurate position estimation is maintained.

As described above, the vehicle position estimation unit 17 of the vehicle-mounted device 1 according to the present embodiment includes the position prediction unit 21 that generates information indicating the predicted vehicle position X ⁻ (t) and the position estimation unit 22. have. The position estimator 22 calculates a feature measured value Z(t) indicating the distance measured by the lidar 2 from the moving object to the object and a feature predicted value Z ⁻ (t) predicted based on the feature information. Then, the position estimating unit 22 calculates the difference values dx and dy between the feature measured value and the feature predicted value, the position estimation accuracy σ _P and the lidar measurement accuracy σ _L according to the difference evaluation values Ex and Ey. , the coefficient values a _X (t) and a _Y (t) for correcting the Kalman gain K(t) are determined. As a result, even if an error occurs in either the map DB 10 or the measurement value of the rider 2, the vehicle-mounted device 1 can suitably suppress the deterioration of the vehicle position estimation accuracy.

[Modification]
Modifications suitable for the embodiment will be described below. The following modifications may be combined and applied to the embodiment.

(Modification 1)
A method of reducing the correction amount of the predicted vehicle position _X ⁻ (t) based on the feature information judged to be unreliable is to use the Kalman gain K ₍ t ) is not limited to multiplication.

Alternatively, in the first example, the vehicle position estimation unit 17 may multiply the difference values dx and dy by the coefficient values _aX (t) and _aY (t). That is, in this case, the vehicle position estimator 17 calculates the estimated vehicle position based on the following equation (8).

なお、この場合、カルマンゲインＫ（ｔ）は式（１０）で生成された後、式（６）に基づく更新が行われないため、以下の一般式（９）により示される共分散行列Ｐ（ｔ）の更新には、信頼度が低い地物情報を用いたということが反映されない。すなわち、位置推定は信頼度を反映する一方、共分散行列は信頼度が反映されないため、その違いを考慮した処理が必要となる。

In this case, the Kalman gain K(t) is not updated based on Equation (6) after being generated by Equation (10), so the covariance matrix P ( The update of t) does not reflect that feature information with low reliability is used. That is, since the position estimation reflects the reliability, but the covariance matrix does not reflect the reliability, it is necessary to consider the difference.

第２の例では、自車位置推定部１７は、以下の一般式（１０）によりカルマンゲインＫ（ｔ）を算出する際に用いる観測雑音行列「Ｒ（ｔ）」の対角成分に、以下の式（１１）に示すように係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を乗じてもよい。

In the second example, the vehicle position estimation unit 17 adds the following to the diagonal component of the observation noise matrix "R(t)" used when calculating the Kalman gain K(t) by the following general expression (10): may be multiplied by coefficient values a _X (t) and a _Y (t) as shown in equation (11).

この場合、自車位置推定部１７は、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を、式（４）に示される評価値Ｅｘ、Ｅｙが大きいほど大きくなるように設定する。例えば、自車位置推定部１７は、以下の式（１２）に示すように係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を設定する。

In this case, the vehicle position estimator 17 sets the coefficient values _{aX(t) and aY (} t) so that they increase as the evaluation values Ex and Ey shown in Equation (4) increase. For example, the vehicle position estimation unit 17 sets coefficient values _{aX(t) and aY (} t) as shown in the following equation (12).

この場合、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）の取り得る範囲は、１から無限大まであるため、上限値を定めるなどのオーバーフローを回避する処理が必要となる。

In this case, the possible range of the coefficient values _{aX(t) and aY (} t) is from 1 to infinity, so processing to avoid overflow, such as setting upper limits, is required.

(Modification 2)
The configuration of the driving assistance system shown in FIG. 1 is an example, and the configuration of the driving assistance system to which the present invention is applicable is not limited to the configuration shown in FIG. For example, in the driving support system, instead of having the vehicle-mounted device 1 , the electronic control device of the vehicle may execute the processing of the vehicle position estimation unit 17 of the vehicle-mounted device 1 . In this case, the map DB 10 may be stored, for example, in a storage unit within the vehicle, and the electronic control device of the vehicle may receive update information for the map DB 10 from a server device (not shown).

1 In-Vehicle Device 2 Rider 3 Gyro Sensor 4 Vehicle Speed Sensor 5 GPS Receiver 10 Map DB

Claims (1)

a first acquisition unit that acquires predicted position information indicating the predicted self-position;
Acquisition of first distance information indicating a distance measured by a measuring unit from a moving object to an object, and second distance information indicating a distance from the moving object to the object predicted based on position information of the object. a second acquisition unit for
a third acquisition unit that acquires the self-position accuracy information of the moving object;
a correction unit that corrects the predicted self-position based on the first distance information, the second distance information, and the self-position accuracy information;
A self-localization device comprising:

Images