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

Abstract

PROBLEM TO BE SOLVED: 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

COPYRIGHT: (C)2024,JPO&INPIT

Description

The present invention relates to self-location estimation technology.

Conventionally, there are known techniques for detecting features installed in the vehicle's travel direction using radar or a camera, and calibrating the vehicle's position based on the detection results. For example, Patent Document 1 discloses a technique for estimating the vehicle's position by comparing the output of a measurement sensor with position information of features registered in advance on a map. Patent Document 2 discloses a vehicle position estimation technique using a Kalman filter.

JP 2013-257742 A JP 2017-72422 A

The features that are the subject of measurement in the self-location estimation process, such as road signs and direction signs, are created and installed so that they are easy for drivers to recognize, but they are not all the same size or orientation; the size varies depending on the display content, and the orientation varies depending on the installation location. As a result, there are cases where the size or orientation of the features that are the subject of measurement by the system performing self-location estimation makes it difficult to measure, and in such cases, the measurement values of the features that are the subject of measurement may contain large errors. If self-location estimation is performed using measurement values that contain such errors, the accuracy of the self-location estimation will decrease.

The present invention has been made to solve the above problems, and its main objective is to provide a self-location estimation device that can effectively suppress deterioration of the self-location estimation accuracy even in the case where there is a possibility of measurement errors of features.

The invention described in claim 1 is a self-location estimation device that includes a first acquisition unit that acquires predicted location information indicating a predicted self-location, a second acquisition unit that acquires information on the traveling direction of a 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 with the moving body, and a correction unit that corrects the predicted self-location based on the traveling direction information, the normal information, and the size information.

The invention described in claim 8 is a control method executed by a self-location estimation device, and includes a first acquisition step of acquiring predicted location information indicating a predicted self-location, a second acquisition step of acquiring information on the traveling direction of a moving body, a third acquisition step of acquiring normal information and size information of a feature to be measured by a measurement unit moving together with the moving body, and a correction step of correcting the predicted self-location based on the traveling direction information, the normal information, and the size information.

The invention described in claim 9 is a program executed by a computer, which causes the computer to function as a first acquisition unit that acquires predicted position information indicating a predicted self-position, a second acquisition unit that acquires information on the moving direction of a 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 with the moving body, and a correction unit that corrects the predicted self-position based on the moving direction information, the normal information, and the size information.

FIG. 1 is a schematic configuration diagram of a driving assistance system. FIG. 2 is a block diagram showing a functional configuration of an in-vehicle device. 4 is an example of a data structure of a map DB. FIG. 2 is a diagram showing state variable vectors in two-dimensional orthogonal coordinates. FIG. 1 illustrates a schematic relationship between a prediction step and a measurement update step. 3 shows functional blocks of a vehicle position estimation unit. 1 illustrates an example of the positional relationship between a feature that is a measurement target of a lidar and a vehicle. FIG. 11 is a diagram for explaining an example of setting coefficient values. 4 is a flowchart of a vehicle position estimation process. FIG. 2 is a diagram showing a surface of an object on the ground that is irradiated with laser light by a lidar.

According to a preferred embodiment of the present invention, the self-location estimation device includes a first acquisition unit that acquires predicted location information indicating a predicted self-location, a second acquisition unit that acquires information on the traveling direction of a 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 with the moving body, and a correction unit that corrects the predicted self-location based on the traveling direction information, the normal information, and the size information. With this aspect, the self-location estimation device can suitably correct the predicted self-location based on the orientation and size of the feature relative to the traveling direction of the moving body.

In one aspect of the self-location estimation device, the self-location estimation device further includes a fourth acquisition unit that acquires first distance information indicating a distance measured by the measurement unit from the moving body to the feature, and second distance information indicating a distance from the moving body to the feature predicted based on the location information of the feature, and the correction unit determines a degree to which the predicted self-location is corrected by a difference value between the distance indicated by the first distance information and the second distance information, based on the information on the traveling direction, the normal information, and the size information. With this aspect, the self-location estimation device can suitably adjust a degree to which the predicted self-location is corrected by a difference value between the distance indicated by the first distance information and the second distance information, based on the orientation and size of the feature relative to the traveling direction of the moving body.

In another aspect of the self-location estimation device, the correction unit corrects the predicted self-location 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 information on the traveling direction, the normal information, and the size information. With this aspect, the self-location estimation device can determine the correction coefficient for the gain that determines the degree to which the predicted self-location is corrected by the difference value of the distance indicated by the first distance information and the second distance information, based on the orientation and size of the feature relative to the traveling direction of the moving body. Preferably, the gain is a Kalman gain.

In another aspect of the self-location estimation device, the correction unit reduces the degree of correction of the predicted self-location in the traveling direction as the angular difference between the traveling direction and the normal direction of the feature indicated by the normal information increases, and reduces the degree of correction of the predicted self-location in the lateral direction of the moving body as the angular difference decreases. In this way, the self-location estimation device can reduce the degree of correction of the self-location in a direction in which errors are likely to occur in the position measurement of the feature, and can suitably improve the position estimation accuracy.

In another aspect of the self-location estimation device, the correction unit reduces the degree to which the predicted self-location is corrected as the size of the feature indicated by the size information becomes smaller. In general, the smaller the size of the feature to be measured, the fewer the measurement points for that feature, and the lower the accuracy of measuring the location of that feature. Therefore, with this aspect, the self-location estimation device can effectively suppress the decrease in location estimation accuracy.

In another aspect of the self-location estimation device, the correction unit reduces the degree to which the predicted self-location is corrected the smaller the size of the feature measured by the measurement unit is compared to the size of the feature indicated by the size information. In general, when only a portion of a feature can be measured due to occlusion or the like, the accuracy of measuring the position of the feature is low. Therefore, with this aspect, the self-location estimation device can preferably suppress a decrease in the accuracy of self-location estimation by reducing the degree of correction of the predicted self-location when only a portion of the feature can be measured due to occlusion or the like.

According to another preferred embodiment of the present invention, a control method executed by a self-location estimation device includes a first acquisition step of acquiring predicted location information indicating a predicted self-location, a second acquisition step of acquiring information on the traveling direction of a moving body, a third acquisition step of acquiring normal information and size information of a feature to be measured by a measurement unit moving with the moving body, and a correction step of correcting the predicted self-location based on the traveling direction information, the normal information, and the size information. By executing this control method, the self-location estimation device can appropriately correct the predicted self-location 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, a program executed by a computer causes the computer to function as 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 body, a third acquisition unit that acquires normal information and size information of a feature to be measured by a measurement unit that moves with the moving body, and a correction unit that corrects the predicted self-position based on the traveling direction information, the normal information, and the size information. By executing this program, the computer 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. Preferably, the above program is stored in a storage medium.

Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. For the sake of convenience, any symbol followed by "^" or "-" will be expressed as "A ^{^} " or " ^A- " (where "A" is any character) in this specification.

[General Configuration]
Fig. 1 is a schematic diagram of a driving assistance system according to the present embodiment. The driving assistance system shown in Fig. 1 includes an on-board device 1 that is mounted on a vehicle and controls driving assistance for 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 vehicle-mounted device 1 is electrically connected to the lidar 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and estimates the position of the vehicle on which the vehicle-mounted device 1 is mounted (also referred to as the "vehicle position") based on the outputs of these sensors. Then, based on the estimation result of the vehicle position, the vehicle-mounted device 1 performs automatic driving control of the vehicle so that the vehicle travels along a route to a set destination. The vehicle-mounted device 1 stores a map database (DB: DataBase) 10 that stores road data and feature information, which is information on landmark features provided near the road. The above-mentioned landmark features are, for example, kilometer posts, 100-meter posts, delineators, traffic infrastructure facilities (e.g., signs, direction signs, traffic signals), utility poles, street lights, and other features that are periodically lined up on the side of the road. Then, the vehicle-mounted device 1 estimates the vehicle position based on this feature information by collating it with the output of the lidar 2, etc. The vehicle-mounted device 1 is an example of a "self-position estimation device" in the present invention.

The lidar 2 emits a pulsed laser in a predetermined angular range in the horizontal and vertical directions to discretely measure the distance to an object 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 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 an output unit that outputs scan data based on the light receiving signal output by the light receiving unit. 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-mentioned light receiving signal. In general, the closer the distance to the object, the higher the accuracy of the distance measurement value of the lidar, and the farther the distance, the lower the accuracy. The lidar 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5 each supply output data to the vehicle-mounted device 1. The lidar 2 is an example of a "measurement unit" in the present invention.

Figure 2 is a block diagram showing the functional configuration of the vehicle-mounted device 1. The vehicle-mounted device 1 mainly has an interface 11, a memory unit 12, an input unit 14, a control unit 15, and an information output unit 16. These elements are interconnected via a bus line.

The interface 11 acquires output data from sensors such as the lidar 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 memory unit 12 stores programs executed by the control unit 15 and information necessary for the control unit 15 to execute predetermined processes. In this embodiment, the memory unit 12 stores a map DB 10 that includes feature information. Figure 3 shows an example of the data structure of the map DB 10. As shown in Figure 3, the map DB 10 includes facility information, road data, and feature information.

The feature information is information in which information about each feature is associated with the feature, and here includes a feature ID equivalent to an index of the feature, location information, normal information, and size information. The location information indicates the absolute location of the feature expressed by latitude and longitude (and altitude), etc. The normal information and size information are information provided for features that have a planar shape, such as a signboard. The normal information is information that indicates the orientation of the feature, and indicates, for example, a normal vector for a surface formed on the feature. The size information is information that indicates the size of the feature, and may be, for example, information indicating the area of a surface formed on the feature, or information indicating the vertical and horizontal widths of a surface formed on the feature.

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 the information in the map DB 10.

The input unit 14 is a button, a touch panel, a remote controller, a voice input device, etc. that the user operates. The information output unit 16 is, for example, a display, a speaker, etc. that outputs information based on 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 this embodiment, the control unit 15 has a vehicle position estimation unit 17. The control unit 15 is an example of the "first acquisition unit," "second acquisition unit," "third acquisition unit," "fourth acquisition unit," "correction unit," and "computer" that executes a 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 lidar 2 relative to the feature and the location 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 in which the vehicle position is estimated from the output data of the gyro sensor 3, the vehicle speed sensor 4, etc. based on a state estimation method based on Bayesian estimation, and a measurement update step in which the estimated value of the vehicle position calculated in the immediately preceding prediction step is corrected. As the state estimation filter used in these steps, various filters developed to perform 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. Below, a simple explanation is given of vehicle position estimation using an extended Kalman filter as an example.

Figure 4 shows the state variable vector x in two-dimensional Cartesian coordinates. As shown in Figure 4, the vehicle's position on a plane defined on the two-dimensional Cartesian coordinates of xy is represented by the coordinates "(x, y)" and the vehicle's orientation "Ψ". Here, the orientation Ψ is defined as the angle between the vehicle's traveling direction and the x-axis. The coordinates (x, y) indicate an absolute position that corresponds to, 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. FIG. 6 shows an example of a functional block of the vehicle position estimation unit 17. As shown in FIG. 5, the prediction step and the measurement update step are repeated to sequentially calculate and update the estimated value of the state variable vector "X" indicating the vehicle position. As shown in FIG. 6, the vehicle position estimation unit 17 has a position prediction unit 21 that executes the prediction step and a position estimation unit 22 that executes the measurement update step. The position prediction unit 21 includes a dead reckoning block 23 and a position prediction block 24, and the position estimation unit 22 includes a feature search/extraction block 25 and a position correction block 26. In FIG. 5, the state variable vector of the reference time (i.e., 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, a provisional estimate (predicted value) estimated in the prediction step is indicated by adding a " ^- " above the character representing the predicted value, and a more accurate estimate updated in the measurement update step is indicated by adding a " ^{^} " above the character representing the value.

In the prediction step, the dead reckoning block 23 of the vehicle position estimation unit 17 uses the vehicle's moving speed "v" and angular velocity "ω" (collectively referred to as "control value u(t) = (v(t), ω(t)) ^T ") to determine the moving distance and azimuth change from the previous time. The position prediction block 24 of the vehicle position estimation unit 17 adds the determined 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 to calculate a predicted value of the vehicle position at time t (also referred to as the "predicted vehicle position") X-(t). At the same time, the covariance matrix " ^{P- (} t)" corresponding to the error distribution of the predicted vehicle position ^X- (t) is 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 LIDAR 2. When the feature search/extraction block 25 of the vehicle position estimation unit 17 has achieved this association, it acquires the measurement value (referred to as the "feature measurement value") "Z(t)" of the feature with which the association has been achieved by the LIDAR 2, and the measurement estimate value (referred to as the "feature predicted value") "Z(t)" of the feature obtained by modeling the measurement process by the LIDAR 2 using the ^predicted vehicle position ^X- (t) and the position vector of the feature registered in the map DB 10. The feature measurement value Z(t) is a two-dimensional vector in the vehicle's body coordinate system that is converted from the distance and scan angle of the feature measured by the LIDAR 2 at time t into components with axes in the traveling direction and lateral direction of the vehicle. Then, the position correction block 26 of the vehicle position estimation unit 17 calculates the difference between the feature measurement value Z(t) and the feature prediction value Z ⁻ (t) as shown in the following equation (1).

Furthermore, the position correction block 26 of the vehicle position estimation unit 17 multiplies the difference between the feature measurement value Z(t) and the feature prediction value Z ⁻ (t) by a Kalman gain “K(t)” as shown in the following equation (2), and adds this to the predicted vehicle position X ⁻ (t), thereby calculating an updated state variable vector (also referred to as “estimated vehicle position”) X ^{^} (t).

In the measurement update step, the position correction block 26 of the vehicle position estimation unit 17 obtains a 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) in the same manner as in the prediction step. 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.

When the vehicle position estimation unit 17 is able to associate the position vectors of multiple features registered in the map DB 10 with the scan data of the LIDAR 2, the vehicle position estimation unit 17 may perform a measurement update step based on any one selected feature measurement value, or may perform the measurement update step multiple times based on all the associated feature measurement values. When using multiple feature measurement values, the vehicle position estimation unit 17 should take into account that the LIDAR measurement accuracy deteriorates the further the feature is from the LIDAR 2, and should reduce the weighting for the feature the longer the distance between the LIDAR 2 and the feature.

In this manner, the prediction step and the measurement update step are repeatedly performed to sequentially calculate the predicted vehicle position X ^- (t) and the estimated vehicle position X ^{^} (t), thereby calculating the most probable vehicle position.

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

[Details of vehicle position estimation]
Next, the vehicle position estimation in this embodiment will be described in detail. In summary, the vehicle position estimation unit 17 corrects the Kalman gain based on normal information and size information included in the feature information of the feature to be measured by the LIDAR 2. As a result, the vehicle position estimation unit 17 estimates the accuracy of the feature measurement values in the vehicle travel direction and lateral direction according to the orientation and size of the feature to be measured, and appropriately determines the correction amount of the predicted vehicle position ^X- (t) in each direction according to the accuracy.

Figure 7 is a diagram showing an example of the positional relationship between the vehicle and a feature 50 that is the target of measurement by the LIDAR 2. In Figure 7, arrow 40 indicates the traveling direction of the vehicle, and arrow 41 indicates the normal direction of the feature 50. Measurement points "P1" to "P7" indicate the positions of each point of the point cloud data output by the LIDAR 2. Furthermore, "Δθ" indicates the angular difference between the traveling direction of the vehicle indicated by arrow 40 and the normal direction of the feature 50 indicated by arrow 41. The angular 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 measurement points P1 to P7 from the rider 2, and calculates each of the 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 center of gravity coordinate of the x coordinate of the body coordinate system indicated by the measurement points P1 to P7 for the feature 50 (i.e. the average value of the coordinates indicated by each measurement point) as the feature measurement value Z(t) in the x direction, and calculates the center of gravity coordinate of the y coordinate 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 Δθ (i.e., closer to parallel), the higher the accuracy of the travel direction component of the feature measurement value and the lower the accuracy of the lateral component. In other words, the larger the angle difference Δθ (i.e., closer to perpendicular), the higher the accuracy of the lateral component of the feature measurement value and the lower the accuracy of the travel direction component. In addition, the larger the size of the feature 50, the more measurement points the lidar 2 has on the feature 50, and therefore the higher the accuracy of the feature measurement value.

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

As a result, the vehicle position estimation unit 17 relatively increases the amount of correction to the predicted vehicle position X-(t) in a direction where the accuracy of the feature measurement value is high, using the difference value between the feature measurement value Z(t) and the feature predicted value ^Z- (t), and relatively decreases the amount of correction to the predicted vehicle position ^X- (t ⁾ in a direction where the accuracy of the feature measurement value is low, using the above-mentioned difference value.

In this case, the coefficient values _aX (t) and _aY (t) are set to values between 0 and 1, and to be larger as the size of the feature to be measured increases. Furthermore, the coefficient _aX (t) is set to be smaller as the angular difference Δθ increases, and the coefficient _aY (t) is set to be larger as the angular difference Δθ increases. In other words, the coefficient _aX (t) is set to be larger as the angular difference Δθ decreases, and the coefficient _aY (t) is set to be smaller as the angular difference Δθ decreases. The coefficient values _aX (t) and _aY (t) are set using an equation or a table, and for example, when the size (e.g., area) of the feature to be measured is "S", they are set based on the following equation (4):

By correcting the Kalman gain based on the coefficient values _aX (t) and _aY (t) calculated in this manner, the Kalman gain becomes smaller when a small-sized feature (such as a signboard) that may have a large amount of error is measured, and the Kalman gain does not become smaller when a large-sized feature that is likely to have a small amount of error is measured. Furthermore, the Kalman gain becomes smaller in a direction in which the error is large as seen from the vehicle, and does not become smaller in a direction in which the error is small. Therefore, the weight of data that may contain a large amount of measurement error becomes relatively small, and the accuracy of the calculated vehicle position improves. The coefficient values _aX (t) and _aY (t) are an example of a "correction coefficient" in the present invention.

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

Figure 8 (A) shows an overhead view of a vehicle undergoing scanning by the lidar 2. In Figure 8 (A), features 51 and 52 with different normal directions are present within the measurement range of the lidar 2. Figure 8 (B) is an enlarged view of feature 51 showing the measurement points "P11" to "P15" of the lidar 2. Also, Figure 8 (C) is an enlarged view of feature 52 showing the measurement points "P16" to "P19" of the lidar 2. In Figures 8 (B) and (C), the center of gravity coordinates in the lateral direction and the traveling direction of the vehicle calculated from the position coordinates of each irradiation point are shown by dashed lines. Note that, for the sake of convenience, Figures 8 (A) to (C) show an example in which the lidar 2 emits laser light along a predetermined scanning plane, but the laser light may be emitted along multiple scanning planes at different heights.

As shown in FIG. 8(A) and FIG. 8(B), the normal direction of the feature 51 is approximately the same as the lateral direction of the vehicle. Therefore, the irradiation points P11 to P15 are aligned along the traveling direction of the vehicle, and the variation in the traveling direction is large, and the variation in the lateral direction is small. Therefore, in the position estimation using the feature 51, it is predicted that the accuracy of the feature measurement value of the lateral component with small variation of the irradiation points P11 to P15 will be high. In this case, the angle difference Δθ is approximately 90 degrees, and "cos Δθ ≒ 0" and "sin Δθ ≒ 1", so the coefficient value a _X (t) based on the formula (4) is close to 0, and the coefficient value a _Y (t) is a value according to the size S. Therefore, in this case, the correction amount to the predicted vehicle position X ⁻ (t) in the x direction (i.e., the traveling direction) where the accuracy of the feature measurement value is low can be relatively small.

On the other hand, as shown in Fig. 8(A) and Fig. 8(C), the normal direction of the feature 52 is approximately the same as the traveling direction of the vehicle. Therefore, the irradiation points P16 to P19 are aligned along the lateral direction of the vehicle, and the variation in the lateral direction is large, while the variation in the traveling direction is small. 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 with small variation of the irradiation points P16 to P19 will be high. In this case, the angle difference Δθ is almost 0 degrees, and "cos Δθ ≒ 1" and "sin Δθ ≒ 0", so the coefficient value a _X (t) based on the formula (4) is a value according to 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 vehicle position X ^- (t) in the y direction (i.e., the lateral direction) where the accuracy of the feature measurement value is low can be relatively small. It should be noted that the coefficient values _aX (t) and _aY (t) may have lower limits. In this case, if the result calculated by the formula (4) is equal to or smaller than a predetermined lower limit, the lower limit is set. This makes it possible to avoid a situation in which a direction that is not corrected by Δθ continues.

Figure 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 process of the flowchart in Figure 9.

First, the vehicle position estimation unit 17 sets an initial value of the vehicle position based on the output of the GPS receiver 5, etc. (step S101). Next, the vehicle position estimation 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 vehicle position estimation unit 17 calculates the travel distance of the vehicle and the change in the vehicle's orientation based on the results acquired in step S102 (step S103).

Thereafter, the vehicle position estimation unit 17 adds the travel distance and direction change calculated in step S103 to the estimated vehicle position X ^{^} (t-1 ⁾ one time before to calculate a predicted vehicle position ^X- (t) (step S104). Furthermore, the vehicle position estimation unit 17 refers to the feature information in the map DB 10 based on the predicted vehicle position X-(t) to search for features within the measurement range of the LIDAR 2 (step S105).

Then, the vehicle position estimation unit 17 calculates a feature predicted value ^Z- (t) from the predicted vehicle position ^X- (t) and the position coordinates indicated by the feature information of the feature searched for in step S105 (step S106). Furthermore, in step S106, the vehicle position estimation unit 17 calculates a feature measurement value Z(t) from the measurement data of the LIDAR 2 for the feature searched for in step S105.

Then, the vehicle position estimation unit 17 refers to the size information and normal information of the feature information of the feature searched for in step S105, and calculates the size S of the illuminated surface of the feature and the angular difference Δθ between the vehicle's traveling direction and the normal vector of the target feature (step S107). In this case, the vehicle position estimation unit 17 calculates the angular difference Δθ based on the vehicle's 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 vehicle position estimation unit 17 calculates coefficient values _aX (t) and _aY (t) based on the size S and the angle difference Δθ, for example, by using equation (4) (step S108). Then, the vehicle position estimation unit 17 generates a Kalman gain K(t)' by multiplying the Kalman gain K(t) by the coefficient values _aX (t) and _aY (t) based on equation (3) (step S109). After that, the vehicle position estimation unit 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 this embodiment generates information indicating the predicted vehicle position ^X- (t), and acquires normal line information and size information of a feature to be measured by the LIDAR 2 from the map DB 10. The vehicle position estimation unit 17 then corrects the predicted vehicle position X-(t ⁾ based on the angle difference Δθ calculated from information on the vehicle's traveling direction and the normal line information of the feature, and the size S indicated by the size information of the feature. This allows the vehicle position estimation unit 17 to estimate the accuracy of the feature measurement values in the vehicle's traveling direction and lateral directions according to the orientation and size of the feature to be measured, and to accurately determine the correction amount of the predicted vehicle position in each direction according to the accuracy.

[Modification]
Preferred modified examples of the embodiment will be described below. The following modified examples may be applied to the embodiment in combination.

(Variation 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 a first example, the vehicle position estimation unit 17 may multiply the difference values dx and dy shown in equation (1) by coefficient values _aX (t) and _aY (t). That is, in this case, the vehicle position estimation unit 17 calculates the estimated vehicle position based on the following equation (5).

In this case, since the Kalman gain K(t) is generated by equation (7) described below and then not updated based on equation (3), the reliability of the feature measurement value based on the orientation and size of the feature is not reflected in the update of the covariance matrix P(t) shown by the following general equation (6). In other words, while the position estimation reflects the reliability, the covariance matrix does not reflect the reliability, and therefore processing that takes this difference into account is required.

In a second example, the vehicle position estimation unit 17 may multiply the diagonal elements of the observation noise matrix “R(t)” used in calculating the Kalman gain K(t) by the following general formula (7) by coefficient values _aX (t) and _aY (t) as shown in the following formula (8).

In this case, the vehicle position estimation unit 17 sets the coefficient value _aX (t) to be smaller as the size S increases and as the angular difference Δθ decreases, and sets the coefficient value _aY (t) to be smaller as the size S increases and as the angular difference Δθ increases. For example, the vehicle position estimation unit 17 sets the coefficient values _aX (t) and _aY (t) as shown in the following equation (9).

Even in this modified example, the vehicle position estimating unit 17 can suitably 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 angular difference Δθ.

(Variation 2)
The vehicle position estimation unit 17 may correct the Kalman gain by taking into account the ratio between the size of the feature to be measured identified from the size information registered in the map DB 10 and the size identified from the measurement data of the LIDAR 2.

Figure 10 is a diagram showing the irradiated surface of feature 55 onto which the laser light from LIDAR 2 is irradiated. In the example of Figure 10, measurement points on the irradiated surface of feature 55 are clearly shown. Note that a portion of the left side of feature 55 is not irradiated with the laser light from LIDAR 2 due to occlusion by an obstacle, etc.

In this case, the vehicle position estimation unit 17 specifies the height width "H _M " and the lateral 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 estimation unit 17 specifies the height width "H _L " and the lateral width "W _L " of the range (measurement range) in which measurement points for the feature 55 are distributed based on the point cloud data output by the _LIDAR 2.

Then, the vehicle position estimation unit 17 generates reliability information "a(t)" indicating the accuracy of the distance measurement for the feature 55 using the following formula (10).

As shown in equation (10), the reliability information a(t) corresponds to the ratio of the size (area) of a feature identified based on the map DB 10 to the size of the feature identified based on the measurement data of the LIDAR 2, and has a value range from 0 to 1, with the smaller the occlusion, the higher the value.

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

(Variation 3)
The configuration of the driving assistance system shown in Fig. 1 is an example, and the configuration of a driving assistance system to which the present invention can be applied is not limited to the configuration shown in Fig. 1. For example, instead of having the on-board device 1, the driving assistance system may have an electronic control device of the vehicle execute the processing of the vehicle position estimation unit 17 of the on-board device 1. In this case, the map DB 10 may be stored in, for example, a storage unit in 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).

REFERENCE SIGNS LIST 1 Vehicle-mounted device 2 Lidar 3 Gyro sensor 4 Vehicle speed sensor 5 GPS receiver 10 Map DB

Claims (1)

A first acquisition unit that acquires predicted location information indicating a predicted self-location;
A second acquisition unit that acquires information about a traveling direction of the moving object;
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 correction unit that corrects the predicted self-position based on the information on the traveling direction, the normal information, and the size information;
A self-location estimation device comprising: