JP2023101820A - Self-location estimation device, control method, program, and storage medium - Google Patents

Abstract

To provide a self-location estimation device capable of suitably suppressing deterioration of self-location estimation precision even in the case of a possible feature measurement error.SOLUTION: An own vehicle location estimation part 17 of an on-vehicle machine 1 generates information for showing a predicted own vehicle location X-(t), and acquires normal line information and size information of a feature as a measurement object from a map DB10 by a lidar 2. The own vehicle location estimation part 17 corrects the predicted own vehicle location X-(t) on the basis of an angle difference Δθ calculated from information on a progress direction of a vehicle and normal line information of a feature, and a size S shown by size information of a feature.SELECTED DRAWING: Figure 7

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

Features such as road signs and direction signboards to be measured in the self-localization process are manufactured and installed so that the driver can easily recognize them, but not all of them have the same size and orientation. As a result, there are cases where the size and orientation of the features to be measured by the system that performs self-localization are difficult to measure, and in such cases, there is a possibility that the measurement values of the features to be measured include large errors. If self-position estimation is performed using measurement values that contain errors in this way, the accuracy of self-position estimation will decrease.

The present invention has been made in order to solve the above problems, and the main object of the present invention is to provide a self-position estimation device that can appropriately suppress deterioration of self-position estimation accuracy even when there is a possibility of a measurement error of a feature.

The invention according to claim 1 is a self-position estimation device, comprising: a first acquisition unit that acquires predicted position information indicating a predicted self-position; a second acquisition unit that acquires information on a traveling direction of a moving object; a third acquisition unit that acquires normal information and size information of a feature to be measured by a measuring unit that moves with the moving object;

According to an eighth aspect 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 information of a traveling direction of a moving object; a third acquiring step of acquiring normal line information and size information of a feature to be measured by a measuring unit that moves together with the moving object; and a correcting step of correcting the predicted self-position based on the traveling direction information, the normal line information, and the size information.

According to a ninth aspect of the present invention, there is provided a program executed by a computer, comprising: 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 a moving object; a third acquisition unit that acquires normal information and size information of a feature to be measured by a measuring unit that moves together with the moving object; and a correcting unit that corrects the predicted self-position based on the information on the traveling direction, the normal information, and the size 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. 3 illustrates an example of a positional relationship between a feature to be measured by a rider and a vehicle. FIG. 10 is a diagram for explaining an example of setting coefficient values; 4 is a flowchart of self-vehicle position estimation processing; FIG. 4 is a diagram showing an irradiated surface of a feature irradiated with laser light from a lidar;

According to a preferred embodiment of the present invention, a self-position estimation device includes: a first acquisition unit that acquires predicted position information indicating a predicted self-position; a second acquisition unit that acquires information on a traveling direction of a moving object; a third acquisition unit that acquires normal information and size information of a feature to be measured by a measuring unit that moves with the moving object; According to this aspect, the self-position estimation device can appropriately correct the predicted self-position based on the orientation and size of the feature with respect to the traveling direction of the moving body.

In one aspect of the self-position estimation device, the self-position estimation device further comprises a fourth acquisition unit that acquires first distance information indicating the distance measured by the measurement unit from the mobile object to the feature, and second distance information indicating the distance from the mobile object to the feature predicted based on the position information of the feature, and the correction unit corrects the degree of correction of the predicted self-position based on the difference value between the distances indicated by the first distance information and the second distance information, the traveling direction information, and the normal line information. and the size information. According to this aspect, the self-position estimation device can suitably adjust 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 based on the orientation and size of the feature with respect to the traveling direction of the moving object.

In another aspect of the self-position estimation 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 determines a correction coefficient for the gain based on the traveling direction information, the normal information, and the size information. According to this aspect, the self-position estimation device can determine 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, based on the orientation and size of the feature with respect to the traveling direction of the moving object. Preferably, said gain is the Kalman gain.

In another aspect of the self-position estimation device, the correction unit reduces the degree of correcting the predicted self-position in the traveling direction as the angular difference between the traveling direction and the normal direction of the feature indicated by the normal information is larger, and lowers the degree of correcting the predicted self-position in the lateral direction of the moving object as the angular difference is smaller. By doing so, the self-position estimation device can lower the degree of correction of the self-position in the direction in which the position measurement of the feature tends to cause an error, and can preferably improve the position estimation accuracy.

In another aspect of the self-position estimation device, 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. In general, the smaller the size of the feature to be measured, the fewer the number of measurement points for the feature, and the lower the accuracy of position measurement of the feature. Therefore, according to this aspect, the self-position estimation device can suitably suppress deterioration in position estimation accuracy.

In another aspect of the self-position estimation device, the correction unit reduces the degree of correcting the predicted self-position as the size of the feature measured by the measurement unit is smaller than the size of the feature indicated by the size information. In general, when only a part of a feature can be measured due to occlusion or the like, the position measurement accuracy of the feature is low. Therefore, according to this aspect, the self-position estimation device can reduce 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 can suitably suppress the deterioration of the self-position estimation accuracy.

According to another preferred embodiment of the present invention, there is provided a control method executed by a self-localization estimating device, comprising: a first acquiring step of acquiring predicted position information indicating a predicted self-position; a second acquiring step of acquiring information of a moving direction of a moving body; a third acquiring step of acquiring normal line information and size information of a feature to be measured by a measuring unit that moves with the moving body; and a correcting step of correcting the predicted self-position based on the moving direction information, the normal line information, and the size information. . By executing this control method, the self-position estimation device can appropriately correct the predicted self-position based on the orientation and size of the feature relative to the traveling direction of the moving body.

According to another preferred embodiment of the present invention, there is provided a program executed by a computer, comprising: 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 a moving object; a third acquisition unit that acquires normal information and size information of a feature to be measured by a measuring unit that moves with the moving object; By executing this program, the computer can appropriately correct the predicted self-position based on the orientation and size of the feature with respect to the traveling direction of the moving object. 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 on-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, 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 (also referred to as “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 emits a pulsed laser over a predetermined angular range in the horizontal and vertical directions to discretely measure the distance to an object existing in the external world and generate three-dimensional point cloud information indicating the position of the object. In this case, the lidar 2 has an irradiating section that irradiates laser light while changing the irradiation direction, a light receiving section that receives reflected light (scattered light) of the irradiated laser light, and an output section that outputs scan data based on the light receiving signal output by the light receiving section. 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 shorter the distance to the object, the higher the accuracy of the distance measurement value of the lidar, and the longer the distance, the lower the accuracy. 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 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, and includes a feature ID corresponding to the index of the feature, position information, normal line information, and size information. The positional information indicates the absolute position of a feature represented by latitude and longitude (and altitude). Normal line information and size information are information provided for a feature having a planar shape such as a signboard, for example. The normal line information is information representing the orientation of the feature, and indicates, for example, a normal vector or the like with respect to a surface formed on the feature. The size information is information representing the size of the feature, and may be, for example, information indicating the area of the surface formed on the feature, or information indicating the vertical and horizontal widths of the surface formed on 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 related to 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 “fourth acquisition unit”, the “correction unit”, and the “computer” that executes the program in the present invention.

The vehicle position estimation unit 17 corrects the vehicle position estimated from the output data of the gyro sensor 3, the vehicle speed sensor 4, and/or the GPS receiver 5, based on the distance and angle measured by the rider 2 with respect to the feature and the position information of the feature extracted from the map DB 10. In this embodiment, as an example, the vehicle position estimation unit 17 alternately executes a prediction step of estimating the vehicle position from output data of the gyro sensor 3, the vehicle speed sensor 4, etc., and a measurement update step of correcting the estimated value of the vehicle position calculated in the previous prediction step, based on a state estimation method based on Bayesian estimation. 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 at the reference time (that is, the current time) “t” to be calculated is expressed as “ ^X− (t)” or “X ^{^} (t)” (expressed as “state variable vector X(t)=(x(t), y(t), Ψ(t)) ^T ”). Here, the provisional estimated value (predicted value) estimated in the prediction step is attached 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 estimating unit 17 uses the moving speed “v” and the angular velocity “ω” of the vehicle (these are collectively written as “control value u(t)=(v(t), ω(t)) ^T ”) to obtain the moving distance and direction change from the previous time. The position prediction block 24 of the vehicle position estimator 17 calculates the predicted value of the vehicle position at time t (also referred to as the “predicted vehicle position”) X ⁻ (t) by adding the calculated moving distance and direction change to the state variable vector X ^{^} (t−1) at time t−1 calculated in the immediately preceding measurement update step. At the same time, the covariance matrix “P ⁻ (t)” corresponding to the error distribution of the predicted vehicle position X ⁻ (t) is calculated from the covariance matrix “P ^{^} (t−1)” at time t−1 calculated in the immediately preceding measurement update step.

In the measurement update step, the feature search/extraction block 25 of the vehicle position estimation unit 17 associates the position vector of the feature registered in the map DB 10 with the scan data of the rider 2 . When this association is established, the feature search/extraction block 25 of the vehicle position estimating unit 17 generates "Z(t)", the value of the associated feature measured by the rider 2 (referred to as "feature measurement value"), and the measured feature value (referred to as "predicted feature value") " ^Z- (t)" of the feature obtained by modeling the measurement processing by the rider 2 using the predicted vehicle position ^X- (t) and the position vector of the feature registered in the map DB 10. respectively. The feature measurement value Z(t) is a two-dimensional vector in the vehicle body coordinate system obtained by converting the distance and scan angle of the feature measured by the rider 2 at time t into components with axes in the traveling direction and the lateral direction of the vehicle. Then, the position correction block 26 of the vehicle position estimator 17 calculates the difference value between the feature measured value Z(t) and the feature predicted value Z ⁻ (t) as shown in the following equation (1).

In addition, the position correction block 26 of the vehicle position estimation unit 17 calculates an updated state variable vector (also referred to as "estimated vehicle position") X ^{^} (t) by multiplying the difference value between the feature measured value Z(t) and the feature predicted value ^Z- (t) by the Kalman gain "K(t)" and adding this to the predicted vehicle position X- ⁽ t), as shown in the following equation (2).

Further, in the measurement update step, the position correction block 26 of the vehicle position estimation unit 17 obtains the covariance matrix P ^{^} (t) (also simply written as P(t)) corresponding to the error distribution of the estimated vehicle position X ^{^} (t) from the covariance matrix ^P- (t), as in the prediction step. 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.

When the position vector of the feature registered in the map DB 10 and the scan data of the rider 2 are associated with a plurality of features, the vehicle position estimation unit 17 may perform the measurement update step based on any one selected feature measurement value or the like, or may perform the measurement update step a plurality of times based on all the associated feature measurement values or the like. When using a plurality of feature measurement values, etc., the vehicle position estimation unit 17 takes into consideration that the accuracy of the lidar measurement deteriorates as the feature is farther from the rider 2. Therefore, the longer the distance between the rider 2 and the feature, the smaller the weighting of the feature.

In this way, the prediction step and the measurement update step are repeated, and the predicted vehicle position X ⁻ (t) and the estimated vehicle position ^X̂ (t) are sequentially calculated, thereby calculating the most probable vehicle position.

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.

[Details of vehicle position estimation]
Next, details of vehicle position estimation in this embodiment will be described. Schematically, the vehicle position estimation unit 17 corrects the Kalman gain based on the normal line information and size information included in the feature information of the feature to be measured by the rider 2 . As a result, 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 orientation and size of the feature to be measured, and accurately determines the correction amount of the predicted vehicle position X ⁻ (t) in each direction according to the accuracy.

FIG. 7 is a diagram showing an example of the 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 . Also, measurement points “P1” to “P7” indicate the positions of points in the point cloud data output by the rider 2 . Furthermore, “Δθ” indicates the angular difference between the traveling direction of the vehicle indicated by arrow 40 and the normal direction of feature 50 indicated by arrow 41 . Note that the angle difference Δθ is assumed to be in the range of 0 to 90 degrees.

In the example of FIG. 7, the vehicle position estimation unit 17 acquires point cloud data indicating the measurement points P1 to P7 from the rider 2, and calculates feature measurement values Z(t) in the x and y directions from the acquired point cloud data. In this case, the vehicle position estimation unit 17 calculates the barycentric coordinates of the x coordinates of the body coordinate system indicated by the measurement points P1 to P7 for the feature 50 (that is, the average value of the coordinates indicated by the respective measurement points) as the feature measurement value Z(t) in the x direction, and calculates the barycentric coordinates of the y coordinates of the body coordinate system indicated by the measurement points P1 to P7 for the feature 50 as the feature measurement value Z(t) in the y direction.

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

In consideration of the above, the vehicle position estimation unit 17 calculates the coefficient values “a _X (t)” and “a _Y (t)” for reducing the Kalman gain based on the angle difference Δθ and the size information of the feature to be measured, and multiplies these by the Kalman gain K(t) as shown in the following equation (3) to calculate the Kalman gain “K(t)′” after correction.

As a result, the vehicle position estimation unit 17 relatively increases the correction amount based on the difference value between the feature measurement value Z(t) and the feature prediction value Z ⁻ (t) for the predicted vehicle position X ⁻ (t) in the direction in which the feature measurement value is highly accurate, and relatively decreases the correction amount based on the above-described difference value for the predicted vehicle position X ⁻ (t) in the direction in which the feature measurement value is low in accuracy.

In this case, the coefficient values a _X (t) and a _Y (t) are values between 0 and 1, and are determined to increase as the size of the feature to be measured increases. Furthermore, the coefficient a _X (t) is determined to have a smaller value as the angle difference Δθ increases, and the coefficient a _Y (t) has a greater value as the angle difference Δθ increases. In other words, the smaller the angular difference Δθ, the larger the coefficient a _X (t), and the smaller the smaller the angular difference Δθ, the smaller the coefficient a _Y (t). The coefficient values _aX (t) and _aY (t) are set using formulas or tables. For example, if the size (for example, area) of the feature to be measured is "S", it is set based on the following formula (4).

By correcting the Kalman gain based on the coefficient values _aX (t) and _aY (t) calculated in this manner, the Kalman gain becomes small when a small-sized feature (such as a signboard) that may have a large amount of error is measured, but does not decrease when a large-sized feature that is likely to have a small amount of error is targeted for measurement. Also, the Kalman gain in the direction in which the error is large as viewed from the vehicle becomes small, and the Kalman gain in the direction in which the error is small does not become small. Therefore, the weight of data that may contain many measurement errors is relatively small, and the accuracy of the calculated vehicle position is improved. The coefficient values _aX (t) and _aY (t) are examples of "correction coefficients" in the present invention.

Next, setting examples of the coefficient values _aX (t) and _aY (t) will be described with reference to FIG.

FIG. 8A shows a bird's-eye view of the vehicle during scanning by the lidar 2. FIG. In FIG. 8A, features 51 and 52 having different normal directions are present within the measurement range of the rider 2 . FIG. 8B is an enlarged view of the feature 51 clearly indicating the measurement points "P11" to "P15" of the rider 2. FIG. FIG. 8(C) is an enlarged view of the feature 52 clearly indicating the measurement points "P16" to "P19" of the rider 2. As shown in FIG. In FIGS. 8B and 8C, the coordinates of the center of gravity of the vehicle in the lateral direction and the traveling direction calculated from the positional coordinates of each irradiation point are indicated by dashed lines. In FIGS. 8A to 8C, for convenience of explanation, the case where the lidar 2 emits laser light along a predetermined scanning plane is exemplified, but laser light may be emitted along a plurality of scanning planes having different heights.

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 large variations in the traveling direction and small variations in the lateral direction. Therefore, in the position estimation using the feature 51, it is expected that the accuracy of the feature measurement value of the horizontal component with small variations in the irradiation points P11 to P15 will be high. In this case, since the angular difference Δθ is approximately 90 degrees, and “cos Δθ≈0” and “sin Δθ≈1”, the coefficient value a _X (t) based on Equation (4) is a value close to 0, and the coefficient value a _Y (t) is a value corresponding to the size S. Therefore, in this case, it is possible to relatively reduce the amount of correction to the predicted vehicle position X ⁻ (t) in the x direction (that is, traveling direction) in which the accuracy of the feature measurement value is low.

On the other hand, as shown in FIGS. 8(A) and 8(C), the normal direction of the feature 52 substantially coincides with 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 expected that the accuracy of the feature measurement value of the traveling direction component with small variations in the irradiation points P16 to P19 will be high. In this case, since the angle difference Δθ is approximately 0 degrees, and “cos Δθ≈1” and “sin Δθ≈0”, the coefficient value a _X (t) based on Equation (4) is a value corresponding to the size S, and the coefficient value a _Y (t) is a value close to 0. Therefore, in this case, the amount of correction to the predicted vehicle position X ⁻ (t) in the y direction (that is, lateral direction) where the accuracy of the feature measurement value is low can be made relatively small. A lower limit value may be provided for the coefficient values a _X (t) and a _Y (t). In that case, if the result calculated by equation (4) is equal to or less 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 by Δθ continues.

FIG. 9 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 moving distance and the azimuth change calculated in step S103 to the estimated vehicle position X ^{^} (t-1) one time ago to calculate the predicted vehicle position X ⁻ (t) (step S104). 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 estimator 17 calculates the feature prediction value Z ⁻ (t) from the predicted vehicle position X ⁻ (t) and the position coordinates indicated by the feature information of the feature searched in step S105 (step S106). Further, in step S106, the vehicle position estimation unit 17 calculates the feature measurement value Z(t) from the measurement data of the rider 2 for the feature searched in step S105.

Then, the vehicle position estimation unit 17 refers to the size information and normal line information of the feature information of the feature searched in step S105, and calculates the size S of the illuminated surface of the feature and the angle difference Δθ between the traveling direction of the vehicle and the normal vector of the target feature (step S107). In this case, the vehicle position estimator 17 calculates the angle difference Δθ based on the vehicle orientation Ψ indicated by the predicted vehicle position X ⁻ (t) and the normal direction of the feature indicated by the normal information included in the feature information.

Next, the own vehicle position estimator 17 calculates the coefficient values _aX (t) and _aY (t) based on the size S and the angle difference ?? using, for example, Equation (4) (step S108). Then, the vehicle position estimation unit 17 multiplies the Kalman gain K(t) by the coefficient values _aX (t) and _aY (t) based on the equation (3) to generate the Kalman gain K(t)' (step S109). Thereafter, the vehicle position estimator 17 corrects the predicted vehicle position X ⁻ (t) using the generated Kalman gain K(t)′ instead of K(t) based on Equation (2), and calculates the estimated vehicle position X ^{^} (t) (step S110).

As described above, the vehicle position estimation unit 17 of the vehicle-mounted device 1 according to the present embodiment generates information indicating the predicted vehicle position X ⁻ (t), and acquires the normal line information and size information of the feature to be measured by the rider 2 from the map DB 10. Then, the vehicle position estimator 17 corrects the predicted vehicle position X ⁻ (t) based on the angle difference Δθ calculated from the vehicle traveling direction information and the normal line information of the feature, and the size S indicated by the size information of the feature. As a result, the vehicle position estimating unit 17 can estimate the accuracy of the feature measurement values in the traveling direction and the lateral direction of the vehicle according to the orientation and size of the feature to be measured, and can accurately determine the correction amount of the predicted vehicle position in each direction according to the accuracy.

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

(Modification 1)
The method of adjusting the correction amount of the predicted vehicle position X ⁻ (t) is not limited to multiplying the Kalman gain K(t) by the coefficient values a _X (t) and a _Y (t).

Alternatively, in the first example, the vehicle position estimator 17 may multiply the difference values dx and dy shown in Equation (1) 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 (5).

In this case, the Kalman gain K(t) is not updated based on Equation (3) after being generated by Equation (7), which will be described later. Therefore, the reliability of the feature measurement values based on the direction and size of the feature is not reflected in the updating of the covariance matrix P(t) shown by General Equation (6) below. 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 a second example, the vehicle position estimation unit 17 may multiply the diagonal components of the observation noise matrix "R(t)" used when calculating the Kalman gain K(t) by the following general formula (7) by the coefficient values _aX (t) and _aY (t) as shown in the following formula (8).

In this case, the vehicle position estimator 17 sets the coefficient value _aX (t) to be smaller as the size S is larger and to be smaller as the angle difference Δθ is smaller, and the coefficient value _aY (t) is set to be smaller as the size S is larger and smaller as the angle difference Δθ is larger. For example, the vehicle position estimator 17 sets coefficient values _aX (t) and _aY (t) as shown in the following equation (9).

Also according to this modification, the vehicle position estimation unit 17 can appropriately adjust the correction amount of the predicted vehicle position X ⁻ (t) using the coefficient values a _X (t) and a _Y (t) determined based on the size S and the angle difference Δθ.

(Modification 2)
The vehicle position estimation unit 17 may correct 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.

FIG. 10 is a diagram showing an irradiated surface of a feature 55 irradiated with laser light from the lidar 2. As shown in FIG. In the example of FIG. 10, measurement points on the illuminated surface of the feature 55 are clearly shown. A portion of the left side of the feature 55 is not irradiated with the laser beam of the rider 2 due to occlusion caused by an obstacle or the like.

In this case, the vehicle position estimating unit 17 identifies the height-direction width “H _M ” and the horizontal-direction width “W _M ” of the feature 55 based on the size information included in the feature information corresponding to the feature 55 . In addition, the vehicle position estimating unit 17, based on the point cloud data output by the rider 2, specifies the height direction width “H _L ” and the width direction width “W _L ” of the range (measurement range) in which the measurement points for the feature 55 are distributed.

Then, the vehicle position estimator 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 equation (10), the reliability information a(t) corresponds to the ratio of the size (area) of the feature identified based on the measurement data of the rider 2 to the size (area) of the feature identified based on the map DB 10, has a value range from 0 to 1, and has a higher value the less occlusion there is.

Then, the vehicle position estimator 17 multiplies the calculated reliability information a(t) together with the coefficient values _aX (t) and _aY (t) by the Kalman gain, for example, to determine the correction amount of the predicted vehicle position according to the reliability indicating the accuracy of the distance measurement to the feature.

(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 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;
a second acquisition unit that acquires information about the traveling direction of the 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 together with the moving object;
a correcting unit that corrects the predicted self-position based on the traveling direction information, the normal line information, and the size information;
A self-localization device comprising: