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

Abstract

The own-vehicle position estimating unit 17 of the on-vehicle device 1 generates information indicating the predicted own-vehicle position X- (t), and also obtains the normal information and the size information of the feature to be measured by the rider 2 from the map DB 10. get. Then, the own vehicle position estimating unit 17 calculates the predicted own vehicle position X based on the angle difference Δθ calculated from the information on the traveling direction of the vehicle and the normal line information of the feature, and the size S indicated by the size information of the feature. Correcting (t).

Description

  The present invention relates to a self-position estimation technique.

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

JP 2013-257742 A JP 2017-72422 A

  The features, such as road signs and direction signs, to be measured in the self-position estimation process are created and installed so that the driver can easily recognize them, but not all sizes and directions are constant. , The size differs according to the display content, and the orientation differs according to the installation position. As a result, there are cases where the feature to be measured by the system for performing self-position estimation has a size or orientation that is difficult to measure, and in such a case, the measured value of the feature to be measured contains a large error. There is a possibility. When the self-position estimation is performed using the measurement value including the error as described above, the self-position estimation accuracy decreases.

  SUMMARY OF THE INVENTION The present invention has been made to solve the above-described problems, and is capable of appropriately suppressing the deterioration of the self-position estimation accuracy even when there is a possibility of a measurement error of a feature. A main object is to provide a position estimation device.

  The invention according to claim 1 is a self-position estimating device, wherein a first acquisition unit that acquires predicted position information indicating a predicted self-position, and a second acquisition unit that acquires information on a traveling direction of a moving object. A third acquisition unit that acquires normal line information and size information of a feature to be measured by a measurement unit that moves with the moving object, information on the traveling direction, the normal line information, and the size information And a correction unit that corrects the predicted self-position based on the above.

  The invention according to claim 8 is a control method executed by the self-position estimating device, wherein a first obtaining step of obtaining predicted position information indicating a predicted self-position, and obtaining information of a traveling direction of a moving body. A second acquisition step of acquiring, a third acquisition step of acquiring normal information and size information of a feature to be measured by a measurement unit that moves together with the mobile object, information of the traveling direction, and the normal information. And a correcting step of correcting the predicted self-position based on the size information.

  The invention according to claim 9 is a program executed by a computer, wherein the first acquisition unit acquires predicted position information indicating a predicted self-position, and the second acquisition unit acquires information on a traveling direction of a moving body. Unit, a third obtaining unit that obtains normal information and size information of a feature to be measured by a measuring unit that moves together with the moving body, information on the traveling direction, the normal information, and the size. The computer functions as a correction unit that corrects the predicted self-position based on the information.

It is a schematic structure figure of a driving support system. It is a block diagram which shows the functional structure of an in-vehicle apparatus. It is an example of the data structure of a map DB. FIG. 4 is a diagram illustrating a state variable vector in two-dimensional orthogonal coordinates. It is a figure which shows the schematic relationship between a prediction step and a measurement update step. 4 shows functional blocks of a vehicle position estimating unit. 4 shows an example of the positional relationship between a feature to be measured by a rider and a vehicle. FIG. 7 is a diagram for describing an example of setting a coefficient value. It is a flowchart of a self-vehicle position estimation process. FIG. 3 is a diagram illustrating an irradiated surface of a feature irradiated with laser light by a lidar.

  According to a preferred embodiment of the present invention, the self-position estimating device includes a first obtaining unit that obtains predicted position information indicating a predicted self-position, and a second obtaining unit that obtains information on a traveling direction of a moving object. A third acquisition unit that acquires normal line information and size information of a feature to be measured by a measurement unit that moves with the moving object, information on the traveling direction, the normal line information, and the size information And a correction unit that corrects the predicted self-position based on the above. According to this aspect, the self-position estimating device can appropriately correct the predicted self-position based on the direction and size of the feature with respect to the traveling direction of the moving object.

  In one aspect of the self-position estimating device, the self-position estimating device is predicted based on first distance information indicating a distance measured by the measuring unit from the moving object to the feature, and position information of the feature. A fourth acquisition unit configured to acquire second distance information indicating a distance from the moving object to the feature, wherein the correction unit determines the predicted self-position by the first distance information and the second distance. The degree of correction based on the difference value of the distance indicated by the information is determined based on the information on the traveling direction, the normal line information, and the size information. According to this aspect, the self-position estimating device preferably sets the degree of correcting the predicted self-position based on the difference value of the distance indicated by the first distance information and the second distance information based on the direction and size of the feature relative to the traveling direction of the moving object. Can be adjusted.

  In another aspect of the self-position estimating device, 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 includes information on the traveling direction. And a correction coefficient for the gain is determined based on the normal line information and the size information. According to this aspect, the self-position estimating device sets the correction coefficient for the gain that determines the degree of correcting the predicted self-position based on the difference value between the distances indicated by the first distance information and the second distance information, to the It can be determined based on the orientation and size. Preferably, the gain is a Kalman gain.

  In another aspect of the self-position estimation device, the correction unit may be configured to perform the prediction in the traveling direction as the angle difference between the traveling direction and the normal direction of the feature indicated by the normal information increases. The degree of correcting the self-position is reduced, and the smaller the angle difference is, the lower the degree of correcting the predicted self-position in the lateral direction of the moving object is. By doing so, the self-position estimating device can reduce the degree of correction of the self-position in a direction in which an error is likely to occur in the position measurement of a feature, and can appropriately improve the position estimation accuracy.

  In another aspect of the self-position estimating device, the correction unit reduces the degree of correction of the predicted self-position as the size of the feature indicated by the size information is smaller. In general, as the size of a feature to be measured is smaller, the number of measurement points for the feature decreases, and the position measurement accuracy of the feature decreases. Therefore, the self-position estimating device can appropriately suppress a decrease in position estimation accuracy in this mode.

  In another aspect of the self-position estimating device, the correction unit is configured such that, as the size of the feature measured by the measurement unit is smaller than the size of the feature indicated by the size information, the predicted self Reduce the degree of position correction. Generally, when only a part of a feature can be measured due to occlusion or the like, the position measurement accuracy of the feature decreases. Therefore, according to this aspect, the self-position estimating device reduces the degree of correction of the predicted self-position when only a part of the feature can be measured due to occlusion or the like, and preferably suppresses a decrease in the self-position estimation accuracy. Can be.

  According to another preferred embodiment of the present invention, there is provided a control method executed by the self-position estimating apparatus, wherein a first obtaining step of obtaining predicted position information indicating a predicted self-position; A second acquisition step of acquiring the information of the above, a third acquisition step of acquiring normal information and size information of a feature to be measured by a measurement unit that moves together with the moving body, and information of the traveling direction, A correcting step of correcting the predicted self-position based on the normal line information and the size information. By executing this control method, the self-position estimating device can appropriately correct the predicted self-position based on the direction and size of the feature with respect to the traveling direction of the moving object.

  According to another preferred embodiment of the present invention, a computer-executable program acquires a predicted position information indicating a predicted self-position, and obtains information on a traveling direction of a moving object. A second acquisition unit, a third acquisition unit that acquires normal line information and size information of a feature to be measured by a measurement unit that moves together with the moving object, the traveling direction information, and the normal line information. And causing the computer to function as a correction unit that corrects the predicted self-position based on the size information. By executing this program, the computer can appropriately correct the predicted self-position based on the direction and size of the feature with respect to the traveling direction of the moving object. Preferably, the program is stored in a storage medium.

Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. Incidentally, "^" or on any symbol - a letter is attached, for convenience in this specification, "A ^{^"} or "A ^-", "" expressed as ( "A" is an arbitrary character).

[Schematic configuration]
FIG. 1 is a schematic configuration diagram of the driving support system according to the present embodiment. The driving assistance system shown in FIG. 1 is mounted on a vehicle and controls the driving assistance of the vehicle. And 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 these outputs, the position of the vehicle on which the in-vehicle device 1 is mounted (“own vehicle position”). Is also estimated.) Then, the on-vehicle device 1 performs automatic driving control of the vehicle based on the estimation result of the own vehicle position so as to travel along the set route to the destination. The in-vehicle device 1 stores a map database (DB: DataBase) 10 that stores road data and landmark information that is information on landmarks provided near roads. The features that serve as the above-mentioned landmarks are, for example, features such as kiloposts, 100 m posts, delinators, traffic infrastructure equipment (for example, signs, direction signs, signals), telephone poles, street lights, and the like that are periodically arranged beside the road. Then, the on-vehicle device 1 estimates the position of the vehicle based on the terrestrial feature information by comparing the output with the rider 2 or the like. The in-vehicle device 1 is an example of the “self-position estimation device” in the present invention.

  The lidar 2 emits a pulse laser in a predetermined angle range in the horizontal direction and the vertical direction, thereby discretely measuring a distance to an object existing in the external world, and 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 irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and scan data based on a light reception signal output by the light receiving unit. And an output unit that outputs 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 above-described light reception signal. In general, the closer the distance to the object is, the higher the accuracy of the lidar distance measurement value is, and the farther the distance is, the lower the accuracy is. The rider 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5 supply output data to the vehicle-mounted device 1, respectively. The rider 2 is an example of the “measurement unit” in the present invention.

  FIG. 2 is a block diagram illustrating a functional configuration of the vehicle-mounted device 1. The in-vehicle device 1 mainly includes an interface 11, a storage unit 12, an input unit 14, a control unit 15, and an information output unit 16. These components are interconnected via a bus line.

  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 a program executed by the control unit 15 and information necessary for the control unit 15 to execute a predetermined process. In the present 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. 3, the map DB 10 includes facility information, road data, and feature information.

  The feature information is information in which information on the feature is associated with each feature. Here, a feature ID corresponding to an index of the feature, position information, normal line information, and size information are included. Including. The position information indicates the absolute position of a feature represented by latitude and longitude (and altitude). The normal line information and the size information are information provided for a feature having a planar shape such as a signboard, for example. The normal line information is information indicating the orientation of a feature, and indicates, for example, a normal vector to a surface formed on the feature. The size information is information indicating the size of the feature, and may be, for example, information indicating the area of the surface formed on the feature, and information on the vertical and horizontal widths of the surface formed on the feature. May be indicated.

  Note that the map DB 10 may be updated periodically. In this case, for example, the control unit 15 receives partial map information on an area to which the own vehicle position belongs from a server device that manages map information via a communication unit (not shown), and reflects the partial map information on the map DB 10.

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

  The control unit 15 includes a CPU that executes a program, and controls the entire vehicle-mounted device 1. In the present embodiment, the control unit 15 has a host vehicle position estimating unit 17. The control unit 15 includes a “first acquisition unit”, a “second acquisition unit”, a “third acquisition unit”, a “fourth acquisition unit”, a “correction unit”, and a “computer” that executes a program in the present invention. This is an example.

  The own-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 the angle to the feature by the rider 2 and the location information of the feature extracted from the map DB 10. The self-vehicle position estimated from the output data of No. 5 is corrected. In the present embodiment, as an example, the own vehicle position estimating unit 17 performs a prediction step of estimating the own vehicle position from output data of the gyro sensor 3, the vehicle speed sensor 4, and the like based on a state estimation method based on Bayesian estimation, The measurement updating step for correcting the estimated value of the vehicle position calculated in the prediction step is alternately executed. Various filters developed to perform Bayesian estimation can be used as the state estimation filter used in these steps, and examples thereof include an extended Kalman filter, an unscented Kalman filter, and a particle filter. As described above, various methods have been proposed for position estimation based on Bayes estimation. In the following, as an example, the vehicle position estimation using an extended Kalman filter will be briefly described.

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

FIG. 5 is a diagram showing a schematic relationship between the prediction step and the measurement update step. FIG. 6 shows an example of functional blocks of the host vehicle position estimating unit 17. As shown in FIG. 5, by repeating the prediction step and the measurement update step, the calculation and update of the estimated value of the state variable vector “X” indicating the own vehicle position are sequentially executed. As shown in FIG. 6, the vehicle position estimating unit 17 includes a position estimating unit 21 that executes a prediction step and a position estimating unit 22 that executes a measurement updating 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, reference time (ie current time) which is an object of calculation the state variable vector of "t", "X ^- (t)" or "X ^{^ (t)"} is indicated as ( "state variables Vector X (t) = (x (t), y (t), Ψ (t)) ^T ”). Here, the provisional estimated value (predicted value) estimated in the predicting step is indicated by “ ⁻ ” added to the character representing the predicted value, and the more accurate estimated value updated in the measurement updating step is updated. denoted by ^{"^"} on top of the character that represents the value to.

In the prediction step, the dead reckoning block 23 of the host vehicle position estimating unit 17 calculates the vehicle moving speed “v” and the angular speed “ω” (these are collectively referred to as “control value u (t) = (v (t), ω ( t)) ^T ”) is used to determine the moving distance and azimuth change from the previous time. The position prediction block 24 of the own vehicle position estimating unit 17 adds the obtained moving distance and azimuth change to the state variable vector X ^＾ (t−1) at time t−1 calculated in the immediately preceding measurement update step. , the predicted value of the vehicle position at time t (also referred to as "predicted vehicle position".) X - calculating a ^(t). At the same time, the predicted vehicle position X ^- covariance with ^- "(t) P", and time t-1 calculated in the previous measurement update step error covariance matrix corresponding to the distribution of (t) It is calculated from the matrix “P ^＾ (t−1)”.

In the measurement update step, the feature search / extraction block 25 of the vehicle position estimating unit 17 associates the feature position vector registered in the map DB 10 with the scan data of the rider 2. Then, the feature search / extraction block 25 of the own vehicle position estimating unit 17, when the association is established, measures the value of the associated feature by the rider 2 (called “feature measurement value”). ) and "Z (t)", the predicted vehicle position X ^- (t) and the measured estimate of the feature obtained by modeling the measurement process by the rider 2 by using the position vector of the feature that has been registered in the map DB10 (referred to as "feature predictive value".) "Z ^- (t)" and the obtaining respectively. The feature measurement value Z (t) is a two-dimensional body coordinate system of the vehicle obtained by converting the feature distance and scan angle measured by the rider 2 at time t into components having axes in the traveling direction and the lateral direction of the vehicle. Vector. Then, the position correction block 26 of the vehicle position estimating unit 17 calculates a difference value between the feature measurement value Z (t) and the feature prediction value Z ⁻ (t) as shown in the following equation (1). I do.

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

Further, the position correction block 26 of the vehicle position estimating unit 17 calculates the difference between the feature measurement value Z (t) and the feature prediction value Z ⁻ (t) as Kalman as shown in the following equation (2). multiplied by the gain "K (t)", which predicted vehicle position X ^- by adding a (t), updated state variable vector (. also referred to as "the estimated vehicle position") X ^ a ^(t) calculate.

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

Further, in the measurement update step, the position correction block 26 of the own vehicle position estimating unit 17 performs the covariance matrix P ^＾ (t) (simply equivalent to the error distribution of the estimated own vehicle position X ^＾ (t) as in the prediction step P (t)) is obtained from the covariance matrix P ⁻ (t). The parameters such as the Kalman gain K (t) can be calculated in the same manner as in a known self-position estimation technique using an extended Kalman filter, for example.

  Note that, when the position vector of the feature registered in the map DB 10 and the scan data of the rider 2 can be associated with a plurality of features, the vehicle position estimating unit 17 selects any one of the selected features. The measurement update step may be performed based on an object measurement value or the like, or the measurement update step may be performed a plurality of times based on all associated feature measurement values or the like. In the case of using a plurality of feature measurements and the like, the own vehicle position estimating unit 17 determines that the distance between the rider 2 and the feature is smaller in consideration of the fact that the lidar measurement accuracy deteriorates as the feature is farther from the rider 2. The longer the length, the smaller the weight for the feature should be.

As described above, the prediction step and the measurement update step are repeatedly performed, and the predicted own vehicle position X ⁻ (t) and the estimated own vehicle position X ^＾ (t) are sequentially calculated, so that the most probable own vehicle position is obtained. Is calculated.

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

[Details of vehicle position estimation]
Next, details of the own vehicle position estimation in the present embodiment will be described. Schematically, the vehicle position estimating unit 17 corrects the Kalman gain based on the normal information and the size information included in the feature information of the feature to be measured by the rider 2. Thereby, the vehicle position estimating unit 17 estimates the accuracy of the feature measurement values in the traveling direction and the lateral direction of the vehicle according to the direction and size of the feature to be measured, and in each direction according to the accuracy. predicted vehicle position X ^- determining accurately the amount of correction of (t).

  FIG. 7 is a diagram illustrating an example of a positional relationship between the feature 50 to be measured by the rider 2 and the vehicle. In FIG. 7, an arrow 40 indicates the traveling direction of the vehicle, and an arrow 41 indicates the normal direction of the feature 50. The measurement points “P1” to “P7” indicate the positions of the points in the point cloud data output by the rider 2. Further, “Δθ” indicates an angle difference between the traveling direction of the vehicle indicated by the arrow 40 and the normal direction of the feature 50 indicated by the arrow 41. The angle difference Δθ is in the range of 0 to 90 degrees.

  In the example of FIG. 7, the vehicle position estimating unit 17 acquires the point cloud data indicating the measurement points P1 to P7 from the rider 2, and calculates the terrestrial feature values Z in the x direction and the y direction from the acquired point cloud data. (T) is calculated. In this case, the vehicle position estimating unit 17 calculates the barycentric coordinates of the x-coordinate of the body coordinate system indicated by the measurement points P1 to P7 with respect to the feature 50 (that is, the average value of the coordinates indicated by each measurement point) in the x-direction feature It is calculated as the measured value Z (t), and the barycentric coordinates of the y coordinate of the body coordinate system indicated by the measurement points P1 to P7 with respect to the terrestrial object 50 are calculated as the terrestrial measured value Z (t) in the y direction.

  At this time, as the angle difference Δθ is smaller (ie, closer to parallel), the accuracy of the traveling direction component of the feature measurement value increases, and the accuracy of the horizontal component decreases. In other words, the greater the angle difference Δθ (ie, closer to vertical), the higher the accuracy of the horizontal component of the feature measurement value and the lower the accuracy of the traveling direction component. Further, as the size of the feature 50 increases, the number of measurement points of the rider 2 with respect to the feature 50 increases, so that the accuracy of the feature measurement value increases.

In consideration of the above, the own-vehicle position estimating unit 17 calculates the coefficient values “a _X (t)” and “a _Y ” for reducing the Kalman gain based on the angle difference Δθ and the size information of the feature to be measured. (T) ”, and the corrected Kalman gain“ K (t) ′ ”is calculated by multiplying these by the Kalman gain K (t) as shown in the following equation (3).

これにより、自車位置推定部１７は、地物計測値の精度が高い方向に対する予測自車位置Ｘ⁻（ｔ）への地物計測値Ｚ（ｔ）と地物予測値Ｚ⁻（ｔ）との差分値による補正量を相対的に大きくし、地物計測値の精度が低い方向に対する予測自車位置Ｘ⁻（ｔ）への上述の差分値による補正量を相対的に小さくする。

Accordingly, the vehicle position estimating unit 17 calculates the feature measurement value Z (t) and the feature prediction value Z ⁻ (t) to the predicted vehicle position X ⁻ (t) in the direction in which the accuracy of the feature measurement value is high. and relatively large correction amount by the difference value between the predicted vehicle position X accuracy of the feature measured value for the lower direction ^- to reduce relatively the amount of correction by the difference value mentioned above to (t).

In this case, the coefficient values a _X (t) and a _Y (t) are determined to be values from 0 to 1 and to be larger as the size of the feature to be measured is larger. Further, the coefficient a _X (t) is determined to be smaller as the angle difference Δθ is larger, and a _Y (t) is set to be larger as the angle difference Δθ is larger. In other words, the coefficient a _X (t) is determined to be larger as the angle difference Δθ is smaller, and a _Y (t) is set to be smaller as the angle difference Δθ is smaller. The coefficient values a _X (t) and a _Y (t) are set using an equation or a table. For example, if the size (eg, area) of a feature to be measured is “S”, the following equation (4) is used. Is set based on

このように算出した係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）に基づきカルマンゲインを補正することで、誤差量が多い可能性のある小さいサイズの地物（看板等）を計測対象とした場合にはカルマンゲインが小さくなり，誤差量が少ないと思われる大きいサイズの地物を計測対象とした場合にはカルマンゲインは小さくならない。また、車両から見て誤差が大きくなる方向のカルマンゲインが小さくなり、誤差が少ない方向のカルマンゲインは小さくならない。よって、計測誤差が多く含まれる可能性のあるデータの重みが相対的に小さくなり、計算される自車位置の精度が向上する。係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）は、本発明における「補正係数」の一例である。

By correcting the Kalman gain based on the coefficient values a _X (t) and a _Y (t) calculated in this way, a small-sized feature (a signboard or the like) having a large possibility of an error amount is set as a measurement target. In this case, the Kalman gain is small, and the Kalman gain is not small when a large-sized feature that is considered to have a small error amount is to be measured. Further, the Kalman gain in the direction where the error increases when viewed from the vehicle decreases, and the Kalman gain in the direction where the error decreases does not decrease. Therefore, the weight of data that may include many measurement errors is relatively reduced, and the accuracy of the calculated vehicle position is improved. The coefficient values a _X (t) and a _Y (t) are examples of the “correction coefficient” in the present invention.

Next, an example of setting the coefficient values a _X (t) and a _Y (t) will be described with reference to FIG.

  FIG. 8A is a bird's-eye view of the vehicle during which the rider 2 is performing a scan. In FIG. 8A, features 51 and 52 having different normal directions exist within the measurement range of the rider 2. FIG. 8B is an enlarged view of the feature 51 in which the measurement points “P11” to “P15” of the rider 2 are specified. FIG. 8C is an enlarged view of the feature 52 in which the measurement points “P16” to “P19” of the rider 2 are clearly shown. In FIGS. 8B and 8C, the dashed lines indicate the barycentric coordinates in the lateral direction and the traveling direction of the vehicle calculated from the position coordinates of each irradiation point. 8A to 8C, for convenience of explanation, a case where the rider 2 emits laser light along a predetermined scanning surface is illustrated, but along a plurality of scanning surfaces having different heights. A laser beam may be emitted.

As shown in FIGS. 8A and 8B, the normal direction of the feature 51 substantially coincides with the lateral direction of the vehicle. Therefore, the irradiation points P11 to P15 are arranged along the traveling direction of the vehicle, and have a large variation in the traveling direction and a small variation in the lateral direction. Therefore, in the position estimation using the feature 51, it is predicted that the accuracy of the feature value of the feature in the horizontal direction in which the variation of the irradiation points P11 to P15 is small is high. In this case, the angle difference Δθ is approximately 90 degrees, and is “cos Δθ ≒ 0” and “sin Δθ ≒ 1”. Therefore, the coefficient value a _X (t) based on the equation (4) becomes a value close to 0. , Coefficient value a _Y (t) is a value corresponding to the size S. Therefore, in this case, the predicted vehicle position X with respect to the x direction accuracy of feature measurements is low (i.e., the traveling direction) ^- can be made relatively small the amount of correction of the (t).

On the other hand, as shown in FIGS. 8A and 8C, the normal direction of the feature 52 substantially matches the traveling direction of the vehicle. Therefore, the irradiation points P16 to P19 are arranged along the lateral direction of the vehicle, and have large variations in the lateral direction and small variations in the traveling direction. Therefore, in the position estimation using the feature 52, it is predicted that the accuracy of the feature measurement value of the traveling direction component in which the variation of the irradiation points P16 to P19 is small is increased. Then, in this case, the angle difference Δθ is almost 0 degree, and is “cosΔθ ≒ 1” and “sinΔθ ≒ 0”. Therefore, the coefficient value a _X (t) based on the equation (4) depends on the size S. And the coefficient value a _Y (t) is a value close to 0. Therefore, in this case, the correction amount to the predicted own vehicle position X ⁻ (t) in the y direction (that is, the lateral direction) in which the accuracy of the feature measurement value decreases can be relatively reduced. Note that the coefficient values a _X (t) and a _Y (t) may have lower limits. In this case, if the result calculated by the equation (4) is equal to or smaller than a predetermined lower limit, the lower limit is set. As a result, it is possible to avoid a situation in which the direction that is not corrected depending on Δθ continues.

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

  First, the own vehicle position estimating unit 17 sets an initial value of the own vehicle position based on the output of the GPS receiver 5 or the like (step S101). Next, the vehicle position estimating unit 17 acquires the vehicle speed from the vehicle speed sensor 4 and the angular velocity in the yaw direction from the gyro sensor 3 (step S102). Then, the own-vehicle position estimating unit 17 calculates a moving distance of the vehicle and a change in azimuth of the vehicle based on the acquisition result of step S102 (step S103).

Then, the vehicle position estimating section 17, one time before the estimated vehicle position X ^{^ (t-1),} by adding the moving distance and direction change calculated in step S103, the predicted vehicle position X ^- (t) Is calculated (step S104). Further, the vehicle position estimating section 17, the predicted vehicle position ^X - based on the (t), referring to the feature information of the map DB 10, searches the feature as the measurement range rider 2 (step S105).

Then, the vehicle position estimating section 17, the predicted vehicle position X ^- from (t) and the position coordinates indicated by the feature information of the searched feature in the step S105, the feature estimated value Z ^- calculating a (t) ( Step S106). Further, in step S106, the host vehicle position estimating unit 17 calculates a feature measurement value Z (t) from the measurement data of the rider 2 for the feature searched for in step S105.

Then, the vehicle position estimating unit 17 refers to the size information and the normal line information of the feature information of the feature searched in step S105, and determines the size S of the irradiated surface of the feature, the traveling direction of the vehicle, and the target The angle difference Δθ from the normal vector of the feature is calculated (step S107). In this case, the vehicle position estimating section 17, the angle difference [Delta] [theta], the predicted vehicle position X ^- and (t) direction of the vehicle indicated [psi, the normal of the feature indicated by the normal vector information included in the feature information It is calculated based on the direction.

Next, the vehicle position estimating unit 17 calculates coefficient values a _X (t) and a _Y (t) based on the size S and the angle difference Δθ, for example, by using Expression (4) (Step S108). . Then, the host vehicle position estimating unit 17 multiplies the Kalman gain K (t) by the coefficient values a _X (t) and a _Y (t) based on the equation (3), thereby obtaining the Kalman gain K (t) ′. It is generated (step S109). Thereafter, the own vehicle position estimating unit 17 corrects the estimated own vehicle position X ⁻ (t) by using the generated Kalman gain K (t) ′ instead of K (t) based on Expression (2), and estimates The host vehicle position X ^＾ (t) is calculated (step S110).

As described above, the vehicle position estimating section 17 of the vehicle-mounted device 1 according to this embodiment, the predicted vehicle position X ^- and generates information indicating a (t), the feature that the rider 2 becomes measurement object The normal line information and the size information are acquired from the map DB 10. Then, the own vehicle position estimating unit 17 calculates the predicted own vehicle position X based on the angle difference Δθ calculated from the information of the traveling direction of the vehicle and the normal information of the feature and the size S indicated by the size information of the feature. ^- to correct a (t). Thereby, the vehicle position estimating unit 17 estimates the accuracy of the feature measurement values in the traveling direction and the lateral direction of the vehicle according to the direction and size of the feature to be measured, and in each direction according to the accuracy. The correction amount of the predicted own vehicle position can be accurately determined.

[Modification]
Hereinafter, a modified example suitable for the embodiment will be described. The following modifications may be combined and applied to the embodiment.

(Modification 1)
Predicted vehicle position ^X - method for adjusting a correction amount of (t) is the coefficient value _a X _(t), not limited to multiplying _a Y (t) to the Kalman gain K (t).

Instead, in the first example, the host vehicle position estimating unit 17 multiplies the difference values dx and dy shown in Expression (1) by the coefficient values a _X (t) and a _Y (t). Is also good. That is, in this case, the own vehicle position estimating unit 17 calculates the estimated own vehicle position based on the following equation (5).

なお、この場合、カルマンゲインＫ（ｔ）は後述する式（７）で生成された後、式（３）に基づく更新が行われないため、以下の一般式（６）により示される共分散行列Ｐ（ｔ）の更新に、地物の向き及びサイズに基づく地物計測値の信頼度が反映されない。すなわち、位置推定は信頼度を反映する一方、共分散行列は信頼度が反映されないため、その違いを考慮した処理が必要となる。

In this case, since the Kalman gain K (t) is not updated based on the expression (3) after being generated by the expression (7) described later, the covariance matrix represented by the following general expression (6) The update of P (t) does not reflect the reliability of the feature measurement value based on the orientation and size of the feature. That is, while the position estimation reflects the reliability, the covariance matrix does not reflect the reliability, so that a process is required in consideration of the difference.

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

In the second example, the vehicle position estimating unit 17 calculates the following diagonal components of the observation noise matrix “R (t)” used when calculating the Kalman gain K (t) by the following general formula (7). May be multiplied by coefficient values a _X (t) and a _Y (t) as shown in Expression (8).

この場合、自車位置推定部１７は、係数値ａ_Ｘ（ｔ）を、サイズＳが大きいほど小さくなり、かつ、角度差Δθが小さいほど小さい値となるように設定し、係数値ａ_Ｙ（ｔ）を、サイズＳが大きいほど小さい値となり、かつ、角度差Δθが大きいほど小さい値となるように設定する。例えば、自車位置推定部１７は、以下の式（９）に示すように係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を設定する。

In this case, the vehicle position estimating unit 17 sets the coefficient value a _X (t) to be smaller as the size S is larger and smaller as the angle difference Δθ is smaller, and the coefficient value a _Y ( t) is set such that the larger the size S, the smaller the value, and the larger the angle difference Δθ, the smaller the value. For example, the vehicle position estimating unit 17 sets coefficient values a _X (t) and a _Y (t) as shown in the following equation (9).

本変形例によっても、自車位置推定部１７は、予測自車位置Ｘ⁻（ｔ）の補正量を、サイズＳ及び角度差Δθに基づき決定した係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を用いて好適に調整することができる。

Also in the present modified example, the own vehicle position estimating unit 17 determines the correction amount of the predicted own vehicle position X ⁻ (t) as coefficient values a _X (t) and a _Y (t) determined based on the size S and the angle difference Δθ. ) Can be suitably adjusted.

(Modification 2)
The vehicle position estimating unit 17 corrects the Kalman gain in consideration of the ratio between the size of the feature to be measured specified from the size information registered in the map DB 10 and the size specified from the measurement data of the rider 2. May be.

  FIG. 10 is a diagram showing an irradiated surface of the feature 55 to which the laser light is irradiated by the rider 2. In the example of FIG. 10, measurement points on the irradiated surface of the feature 55 are clearly shown. Note that a part of the left side of the feature 55 is not irradiated with the laser beam of the rider 2 due to occlusion or the like due to an obstacle.

In this case, the vehicle position estimating section 17, based on the size information included in the feature information corresponding to the feature 55, the height direction of the width of the feature 55 "H _M" lateral width "W _M "Respectively. In addition, the vehicle position estimating unit 17 determines the width “ _HL ” in the height direction of the range (measurement range) in which the measurement points for the feature 55 are distributed, based on the point cloud data output by the rider 2, and The width “W _L ” is specified.

  Then, the vehicle position estimating unit 17 generates reliability information “a (t)” indicating the accuracy of distance measurement with respect to the feature 55 using the following equation (10).

式（１０）に示すように、信頼度情報ａ（ｔ）は、地図ＤＢ１０に基づき特定される地物のサイズ（面積）に対するライダ２の計測データに基づき特定される地物のサイズの比に相当し、０から１までの値域を有し、かつオクルージョンが少ないほど高い値となる。

As shown in Expression (10), the reliability information a (t) is a ratio of the size of the feature specified based on the measurement data of the rider 2 to the size (area) of the feature specified based on the map DB 10. Correspondingly, it has a value range from 0 to 1, and the higher the occlusion, the higher the value.

Then, the own vehicle position estimating unit 17 multiplies the calculated reliability information a (t) by the Kalman gain together with the coefficient values a _X (t) and a _Y (t), for example, to calculate the distance to the feature. It is possible to determine the correction amount of the predicted own vehicle position according to the reliability indicating the accuracy.

(Modification 3)
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 can be applied 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 unit 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 is stored in, for example, a storage unit in the vehicle, and the electronic control unit of the vehicle may receive update information of the map DB 10 from a server device (not shown).

DESCRIPTION OF SYMBOLS 1 Onboard equipment 2 Rider 3 Gyro sensor 4 Vehicle speed sensor 5 GPS receiver 10 Map DB

Claims (10)

A first acquisition unit that acquires predicted position information indicating a predicted self-position,
A second acquisition unit that acquires information on the traveling direction of the moving body,
A third acquisition unit that acquires normal information and size information of a feature to be measured by a measurement unit that moves together with the moving object,
The traveling direction information, the normal line information, based on the size information, based on the correction unit, which corrects the predicted self-position,
Self-position estimation device comprising: First distance information indicating a distance measured by the measurement unit from the moving object to the feature, and second distance information indicating a distance from the moving object to the feature predicted based on position information of the feature. And a fourth acquisition unit that acquires
The correction unit is configured to correct the degree of correcting the predicted self-position based on a difference value between the distances indicated by the first distance information and the second distance information, the traveling direction information, the normal line information, and the size. The self-position estimating device according to claim 1, which is determined based on the information. The correcting unit corrects the predicted self-position by a value obtained by multiplying the difference value by a predetermined gain,
The self-position estimation device according to claim 2, wherein the correction unit determines a correction coefficient for the gain based on the information on the traveling direction, the normal information, and the size information.   The self-position estimation device according to claim 3, wherein the gain is a Kalman gain.   The correction unit, the larger the angle difference between the traveling direction and the normal direction of the feature indicated by the normal information, the lower the degree of correcting the predicted self-position in the traveling direction, the lower the angle The self-position estimating device according to any one of claims 1 to 4, wherein the smaller the difference is, the lower the degree of correcting the predicted self-position in the lateral direction of the moving object is.   The self-position estimating device according to any one of claims 1 to 5, wherein the correction unit reduces the degree of correcting the predicted self-position as the size of the feature indicated by the size information is smaller.   The correction unit, the smaller the size of the feature measured by the measurement unit is smaller than the size of the feature indicated by the size information, to reduce the degree of correcting the predicted self-position. 7. The self-position estimating device according to any one of 6. A control method executed by the self-position estimating device,
A first acquisition step of acquiring predicted position information indicating a predicted self-position,
A second acquisition step of acquiring information on the traveling direction of the moving object;
A third acquisition step of acquiring normal line information and size information of a feature to be measured by a measurement unit that moves with the moving body,
A correction step of correcting the predicted self-position based on the traveling direction information, the normal line information, and the size information,
A control method having: A program executed by a computer,
A first acquisition unit that acquires predicted position information indicating a predicted self-position,
A second acquisition unit that acquires information on the traveling direction of the moving body,
A third acquisition unit that acquires normal information and size information of a feature to be measured by a measurement unit that moves together with the moving object,
A program that causes the computer to function as a correction unit that corrects the predicted self-position based on the traveling direction information, the normal line information, and the size information.   A storage medium storing the program according to claim 9.

Images