Measurement accuracy calculation apparatus, self-position estimation apparatus, control method, program, and storage medium

Abstract

To provide a measurement accuracy calculation apparatus and a self-position estimation apparatus capable of suitably calculating the accuracy of measuring a ground object measured by a measurement unit.SOLUTION: A self-position estimation unit 17 of an onboard apparatus acquires point cloud data showing a result of measuring a landmark obtained by a lidar 2 and also acquires landmark feature information included in a map DB 10. The self-position estimation unit 17 also calculates reliability information indicating the accuracy of measuring the landmark by the lidar 2 on the basis of the difference between the size of a landmark identified on the basis of the acquired point cloud data and the size of a landmark indicated by the feature information.SELECTED DRAWING: Figure 6

Description

[Technical Field]

[0001] The present invention relates to a technique for calculating the accuracy of measurements made by a measurement unit. [Background technology]

[0002] Conventionally, there have been known techniques for detecting features installed ahead of a vehicle using a 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. Furthermore, Patent Document 2 discloses a vehicle's position estimation technique using a Kalman filter. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Laid-Open No. 2013-257742 [Patent Document 2] Japanese Patent Application Publication No. 2017-72422 Summary of the Invention [Problem to be solved by the invention]

[0004] In the self-localization process, if a feature to be measured is adjacent to another feature, the other feature will be included in the measurement data along with the feature to be measured, which may reduce the precision (accuracy) of the measurement results. Similarly, if occlusion occurs in part of the feature to be measured, part of the feature that should have been measured cannot be measured, which may reduce the precision of the measurement results.

[0005] The present invention has been made to solve the above-mentioned problems, and its main object is to provide a measurement accuracy calculation device and a self-position estimation device that can suitably calculate the measurement accuracy of a feature by a measurement unit. [Means for solving the problem]

[0006] The claimed invention is a measurement accuracy calculation device having a first acquisition unit that acquires measurement results of a feature by a measurement unit, a second acquisition unit that acquires feature information of the feature contained in map data, and a calculation unit that calculates accuracy information indicating the measurement accuracy of the feature by the measurement unit based on a comparison result between the measurement results and the feature information. [Brief explanation of the drawings]

[0007] [Figure 1] FIG. 1 is a schematic configuration diagram of a driving assistance system. [Figure 2] FIG. 2 is a block diagram showing the functional configuration of the vehicle-mounted device. [Figure 3] 2 shows an example of a data structure of a map database. [Figure 4] This is a diagram showing a state variable vector in two-dimensional orthogonal coordinates. [Figure 5] FIG. 10 is a diagram illustrating a schematic relationship between a prediction step and a measurement update step. [Figure 6] 3 shows functional blocks of a vehicle position estimation unit. [Figure 7] 1 shows a graph of reliability information. [Figure 8] FIG. 10 is a diagram illustrating a schematic view of the irradiation range of the laser beam of the lidar when occlusion occurs. [Figure 9] FIG. 10 is a diagram showing a schematic view of the lidar laser beam irradiation range when another feature exists adjacent to a landmark feature. [Figure 10] FIG. 2 is a diagram showing the positional relationship between a vehicle and landmarks expressed in a two-dimensional world coordinate system and a vehicle coordinate system. [Figure 11] This is a two-dimensional graph of reliability information when the landmark is facing almost directly ahead as seen from the vehicle. [Figure 12] This is a two-dimensional graph of reliability information when the landmark is slightly sideways as seen from the vehicle. [Figure 13] 10 is a flowchart showing a vehicle position estimation process. [Figure 14] 10 shows a graph of reliability information according to Modification 1. [Figure 15] 10 shows a graph of reliability information according to Modification 2. DETAILED DESCRIPTION OF THE INVENTION

[0008] According to a preferred embodiment of the present invention, a measurement accuracy calculation device includes a first acquisition unit that acquires measurement results of a feature by a measurement unit, a second acquisition unit that acquires feature information of the feature included in map data, and a calculation unit that calculates accuracy information indicating the measurement accuracy of the feature by the measurement unit based on the difference between the measured size of the feature and the size of the feature indicated by the feature information. With this aspect, the measurement accuracy calculation device can accurately calculate accuracy information indicating the measurement accuracy of the feature by the measurement unit in both cases where occlusion occurs in the feature to be measured and some measurement data cannot be acquired, or where measurement data of another feature adjacent to the feature to be measured is acquired as part of the measurement data of the feature to be measured.

[0009] In one aspect of the measurement accuracy calculation device, the calculation unit generates accuracy information indicating lower measurement accuracy as the difference increases. This makes it possible to preferably generate accuracy information indicating low measurement accuracy in both cases where occlusion occurs in the feature to be measured and some measurement data cannot be acquired, or where measurement data of an adjacent feature is acquired as part of the measurement data of the feature to be measured.

[0010] In one aspect of the measurement accuracy calculation device, the calculation unit generates accuracy information indicating the measurement accuracy of the feature in each of the moving body's moving direction and lateral direction based on the difference, the moving body's moving direction, and the normal direction of the feature indicated by the feature information. Generally, depending on the orientation of the feature relative to the moving body's moving direction, there may be directions in which measurement errors are unlikely to occur and directions in which measurement errors are likely to occur. Therefore, according to this aspect, the measurement accuracy calculation device can preferably generate accuracy information indicating the measurement accuracy of the feature in each of the moving body's moving direction and lateral direction.

[0011] In another aspect of the measurement accuracy calculation device, the calculation unit reduces the influence of the difference on the measurement accuracy in the lateral direction as the angular difference between the traveling direction and the normal direction of the feature increases, and reduces the influence of the difference on the measurement accuracy in the traveling direction as the angular difference decreases. The normal direction of the feature here refers to the front or back direction of the feature, which is perpendicular to the plane formed by the feature and is within 90 degrees of the traveling direction of the moving object. In this case, the greater the angular difference between the traveling direction of the moving object and the normal direction of the feature, the more the feature will be oriented sideways relative to the moving object, making it less likely that a measurement error will occur in the lateral direction of the moving object. On the other hand, the smaller the angular difference between the traveling direction of the moving object and the normal direction of the feature, the more the feature will be oriented frontally relative to the moving object, making it less likely that a measurement error will occur in the traveling direction of the moving object. Thus, with this aspect, the measurement accuracy calculation device can generate accuracy information that accurately represents the measurement accuracy of the feature in each of the traveling direction and lateral direction of the moving object.

[0012] In another aspect of the measurement accuracy calculation device, the measurement accuracy calculation device further includes a third acquisition unit that acquires predicted position information indicating a predicted self-position, and a correction unit that corrects the predicted self-position based on the accuracy information. With this aspect, the measurement accuracy calculation device can preferably perform self-position estimation that reflects the generated accuracy information.

[0013] In another aspect of the measurement accuracy calculation device, the correction unit determines, based on the accuracy information, a gain for a difference value when correcting the predicted self-position using a difference value between a distance measured by the measurement unit from the moving body to the feature and a predicted distance from the moving body to the feature predicted based on position information of the feature included in the feature information. With this aspect, the measurement accuracy calculation device can prevent inaccurate correction and preferably improve the self-position estimation accuracy by adjusting the gain based on the accuracy information when correcting the predicted self-position using the difference value between the measured distance and the predicted distance.

[0014] According to another preferred embodiment of the present invention, a self-location estimation device includes a first acquisition unit that acquires measurement results of a feature by a measurement unit, a second acquisition unit that acquires feature information of the feature included in map data, a third acquisition unit that acquires predicted location information indicating a predicted self-location, and a correction unit that corrects the predicted self-location based on a difference between the measured size of the feature and the size of the feature indicated by the feature information. The difference occurs when occlusion occurs in the feature to be measured and some measurement data cannot be acquired, or when measurement data of another feature adjacent to the feature to be measured is acquired as part of the measurement data of the feature to be measured. Therefore, with this aspect, the self-location estimation device can appropriately correct the predicted self-location in consideration of the difference and determine a final self-location.

[0015] According to another preferred embodiment of the present invention, there is provided a control method executed by a measurement accuracy calculation device, the control method comprising: a first acquisition step of acquiring measurement results of a feature by a measurement unit; a second acquisition step of acquiring feature information of the feature included in map data; and a calculation step of calculating accuracy information indicating the measurement accuracy of the feature by the measurement unit based on the difference between the measured size of the feature and the size of the feature indicated by the feature information. By executing this control method, the measurement accuracy calculation device can accurately calculate accuracy information indicating the measurement accuracy of the feature by the measurement unit in both cases where occlusion occurs in the feature to be measured and some measurement data cannot be acquired, or where measurement data of another feature adjacent to the feature to be measured is acquired as part of the measurement data of the feature to be measured.

[0016] According to another preferred embodiment of the present invention, there is provided a computer-executable program that causes the computer to function as a first acquisition unit that acquires measurement results of a feature by a measurement unit, a second acquisition unit that acquires feature information of the feature contained in map data, and a calculation unit that calculates accuracy information indicating the accuracy of the measurement of the feature by the measurement unit based on the difference between the measured size of the feature and the size of the feature indicated by the feature information. By executing this program, the computer can accurately calculate the accuracy information indicating the accuracy of the measurement of the feature by the measurement unit in both cases where occlusion occurs in the feature to be measured and some measurement data cannot be acquired, or where measurement data of another feature adjacent to the feature to be measured is acquired as part of the measurement data of the feature to be measured. Preferably, the program is stored in a storage medium. [Example]

[0017] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. For the sake of convenience, in this specification, a character with "^" or "-" above any symbol will be referred to as "A ^ " or "A - " (where "A" is any letter).

[0018] [Overview of driving assistance system] Fig. 1 shows a schematic configuration of a driving assistance system according to this 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. Note that hereinafter, the term "map" includes data used in an ADAS (Advanced Driver Assistance System) and autonomous driving in addition to data referenced by conventional on-board devices for route guidance.

[0019] The vehicle-mounted device 1 is electrically connected to a lidar 2, a gyro sensor 3, a vehicle speed sensor 4, and a GPS receiver 5, and estimates the position of the vehicle (also referred to as "subject vehicle position") on which the vehicle-mounted device 1 is mounted based on the outputs of these sensors. Based on the result of estimating the subject 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) 10 that stores road data and feature information, which is information about landmark features installed near the road. Examples of the landmark features include kilometer posts, 100-meter posts, delineators, traffic infrastructure facilities (e.g., signs, direction signs, traffic lights), utility poles, streetlights, and other features that are periodically lined along the side of the road. Based on this feature information, the vehicle-mounted device 1 compares it with the output of the lidar 2 and the like to estimate the subject vehicle position. The vehicle-mounted device 1 is an example of a "measurement accuracy calculation device" and a "subject position estimation device" in the present invention. The map DB 10 may be stored in an external storage device such as an external server instead of being stored in the in-vehicle device 1. In this case, the in-vehicle device 1 acquires at least a part of the map DB 10 from the external storage device via wireless communication or the like.

[0020] The LIDAR 2 emits a pulsed laser beam within 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 includes an irradiation unit that irradiates a laser beam while changing the irradiation direction, a light receiving unit that receives light (scattered light) reflected from the object by the irradiated laser beam, and an output unit that outputs scan data based on the light receiving signal output by the light receiving unit. The scan data is point cloud data and is generated based on the irradiation direction corresponding to the laser beam received by the light receiving unit and the distance to the object in that irradiation direction, which is determined based on the above-mentioned light receiving signal. The LIDAR 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5 each supply output data to the vehicle-mounted device 1. The LIDAR 2 is an example of a "measurement unit."

[0021] 2 is a block diagram showing the functional configuration of the vehicle-mounted device 1. The vehicle-mounted 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 elements are interconnected via a bus line.

[0022] 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.

[0023] The storage 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 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.

[0024] The feature information is information in which information about each feature is associated with that feature, and here includes a feature ID, which corresponds to an index of the feature, location information, orientation information (normal information), and size information. The location information indicates the absolute position of the feature, expressed by latitude and longitude (and altitude), etc. The orientation information and size information are information provided for features that have a planar shape, such as a signboard. The orientation information is information that indicates the orientation of the feature, and indicates, for example, the normal vector to a surface formed on the feature. The size information is information that indicates the size of the feature, and in this embodiment, includes information on the width and height of the measurement surface formed on the feature.

[0025] The map DB 10 may be periodically updated. 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.

[0026] The input unit 14 is a button operated by the user, a touch panel, a remote controller, a voice input device, etc. 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.

[0027] The control unit 15 includes a CPU that executes a program and controls the entire in-vehicle device 1. In this embodiment, the control unit 15 has a vehicle position estimation unit 17 that estimates the vehicle position based on the output signals of each sensor supplied from the interface 11 and the map DB 10. The control unit 15 then performs control related to vehicle driving assistance, including automatic driving control, based on the result of the estimation of the vehicle position. The control unit 15 is an example of a "first acquisition unit," a "second acquisition unit," a "third acquisition unit," a "calculation unit," a "correction unit," and a "computer" that executes a program in the present invention.

[0028] [Outline of vehicle position estimation process] First, an overview of the process of estimating the vehicle position by the vehicle position estimating unit 17 will be described.

[0029] 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 measurements of the LIDAR 2 relative to the feature (also called a "landmark") to be measured and the position information of the landmark extracted from the map DB 10. In this embodiment, as an example, the vehicle position estimation unit 17 alternately executes a prediction step of predicting the vehicle position from the output data of the gyro sensor 3, the vehicle speed sensor 4, etc., based on a state estimation method based on Bayesian estimation, and a measurement update step of correcting the predicted value of the vehicle position calculated in the immediately preceding prediction step. 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. As such, various methods for position estimation based on Bayesian estimation have been proposed.

[0030] The following briefly describes vehicle position estimation using an extended Kalman filter.

[0031] FIG. 4 is a diagram showing state variable vectors in two-dimensional Cartesian coordinates. As shown in FIG. 4, the vehicle position on a plane defined on the two-dimensional Cartesian coordinates of x and y is represented by coordinates "(x, y)" and the vehicle's orientation (yaw angle) "ψ." Here, the yaw angle ψ is defined as the angle between the vehicle's traveling direction and the x-axis. The coordinates (x, y) are, for example, absolute positions corresponding to a combination of latitude and longitude, or world coordinates indicating a position with a predetermined point as the origin.

[0032] FIG. 5 is a diagram showing a schematic relationship between the prediction step and the measurement update step. FIG. 6 shows an example of the functional blocks 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 landmark search / extraction block 25 and a position correction block 26. In FIG. 5, the state variable vector at the reference time (i.e., the current time) "t" to be calculated is calculated as "X - (t)" or "X ^ (t)" ("State variable vector X(t) = (x(t), y(t), ψ(t)) T "). Here, the provisional estimates (predicted values) estimated in the prediction step are indicated by " - " is added to the character representing the value, and the more accurate estimated value updated in the measurement update step is added with " ^ " is added.

[0033] In the prediction step, the dead reckoning block 23 of the vehicle position estimation unit 17 calculates the vehicle's moving speed "v" and angular velocity "ω" (collectively referred to as the "control value u(t)=(v(t), ω(t)) T"). The position prediction block 24 of the vehicle position estimation unit 17 calculates the travel distance and change in direction from the previous time by using the state variable vector X ^ The calculated travel distance and direction change are added to (t-1) to obtain the predicted value of the vehicle position at time t (also called the "predicted vehicle position") X - (t) is calculated. At the same time, the predicted vehicle position X - The covariance matrix P corresponding to the error distribution of (t) - (t)” is the covariance matrix “P ^ (t-1)".

[0034] In the measurement update step, the landmark search and extraction block 25 of the vehicle position estimation unit 17 associates the position vector of the measurement target feature (landmark) registered in the map DB 10 with the scan data of the LIDAR 2. Then, when this association is established, the landmark search and extraction block 25 of the vehicle position estimation unit 17 calculates the measurement value "Z(t)" of the associated landmark by the LIDAR 2 and the predicted vehicle position X - (t) and the landmark position vector registered in the map DB 10, and the landmark measurement value (called the "measurement predicted value") obtained by modeling the measurement process by the lidar 2. - The measured value Z(t) is a vector value in the vehicle coordinate system, which is obtained by converting the distance and scan angle of the landmark measured by the lidar 2 at time t into components with axes in the traveling direction and lateral direction of the vehicle. The position correction block 26 of the vehicle position estimation unit 17 then calculates the measured value Z(t) and the predicted measured value Z - Calculate the difference value with (t).

[0035] In reality, the landmarks have a predetermined size, and data corresponding to a plurality of measurement points on the irradiated surface irradiated with the laser light of the lidar 2 is acquired as point cloud data. Therefore, for example, the landmark search / extraction block 25 calculates a measurement value Z(t) corresponding to the center position of the target landmark by averaging the measurement values ​​at each measurement point where the target landmark is measured.

[0036] Then, the position correction block 26 of the vehicle position estimation unit 17 calculates the measured value Z(t) and the predicted measured value Z(t) as shown in the following equation (1): - (t) is multiplied by the Kalman gain "K(t)" and used as the predicted vehicle position X - (t) to obtain the updated state variable vector (also called the "estimated vehicle position") X ^ Calculate (t).

[0037]

number

[0038] In this embodiment, the vehicle position estimation unit 17 corrects the Kalman gain K(t) in accordance with reliability information, which is an index indicating the reliability of the measurement value Z(t) (i.e., the measured distance to the center point of the landmark), as described below. As a result, when the reliability of the measurement value Z(t) is low due to the occurrence of occlusion or the presence of a feature adjacent to the landmark, the Kalman gain K(t) is lowered, and the predicted vehicle position X - In this case, the correction amount for the predicted vehicle position X - Inaccurate corrections to (t) are suitably suppressed, improving the accuracy of self-location estimation.

[0039] 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, it may perform the measurement update step based on any selected set of measurement predicted values ​​and measurement values, etc., or may perform the measurement update step multiple times based on all the associated measurement predicted values ​​and measurement values, etc. When using multiple measurement predicted values ​​and measurement values, etc., it is preferable that the vehicle position estimation unit 17 reduces the weighting for a feature the longer the distance between the LIDAR 2 and the feature, taking into account that the LIDAR measurement accuracy deteriorates as the feature is farther from the LIDAR 2.

[0040] In this way, the prediction step and the measurement update step are repeatedly performed, and the predicted vehicle position X - (t) and estimated vehicle position X ^ By successively calculating (t), the most probable vehicle position is calculated.

[0041] In the above description, the measured value Z(t) is an example of the "measured distance" of the present invention, and the predicted measured value Z - (t) is an example of the "predicted distance" of the present invention.

[0042] [Calculation of reliability information] Next, a method for calculating reliability information, which is an index representing the reliability of the measurement value Z(t), will be described. Briefly, the vehicle position estimation unit 17 calculates the difference between the size of the landmark on the map indicated by the size information included in the feature information and the size of the range (measurement range) in which the measurement points of the landmark measured by the LIDAR 2 are distributed, and normalizes the difference to calculate the reliability information. In this way, the vehicle position estimation unit 17 calculates reliability information that accurately represents the reliability of the measurement value Z(t).

[0043] (1) Calculation of reliability information independent of direction First, a case where reliability information that is not dependent on the direction is generated for the measurement value Z(t) will be described.

[0044] In this case, the vehicle position estimation unit 17 calculates the width on the map of the landmark included in the feature information, "W M " and vertical width (height direction) "H M ” and the width of the measurement range of the landmark identified based on the point cloud data of LIDAR 2 at time t, “W L (t)" and vertical width "H L Based on the "(t)" and the sensitivity coefficient "k", the reliability information a(t) is determined as shown in the following equation (2).

[0045]

number

[0046] Figure 7(A) shows the width W M 60cm, vertical width H M The vertical width H L Width W when (t) is fixed at 60cm L 7(B) is a two-dimensional graph showing the relationship between (t) and the reliability information a(t) based on Equation (2).L (t) is fixed at 60cm. L 7(A) and (B). L (t) and vertical width H L 7(A) is a three-dimensional graph showing the relationship between the reliability information a(t) and the time (t). Also, Fig. 7(D) is a graph showing the reciprocal "1 / a(t)" of the reliability information a(t) in Fig. 7(A).

[0047] As shown in Figures 7(A) and 7(C), the width W L (t) is the width W M As the vertical width H approaches 60 cm, the reliability information a(t) approaches the maximum value of 1. On the other hand, as shown in FIGS. 7(B) and 7(C), L (t) is the vertical width H M As the width approaches 60 cm, the reliability information a(t) approaches the maximum value of 1. As shown in FIG. 7(D), the reciprocal of the reliability information a(t) is L (t) is the width W M The closer it gets to the minimum value of 1, the closer it gets to the width W L (t) is the width W M Similarly, the inverse of the reliability information a(t) is L (t) is the vertical width H M The closer it gets to the minimum value of 1, the closer it gets to the vertical width H L (t) is the vertical width H M The inverse of the reliability information a(t) is used to set the Kalman gain according to the reliability information, as will be described later.

[0048] Here, the validity of formula (2) will be further explained using specific examples shown in FIGS.

[0049] Fig. 8(A) is a diagram showing a schematic view of the irradiation range of the laser light of the lidar 2 when occlusion occurs during measurement of a landmark feature 20 by the lidar 2, and Fig. 8(B) is a plan view of the feature 20 clearly showing measurement points on the irradiated surface irradiated with the laser light of the lidar 2. Note that here, a road sign with a high reflectivity of laser light is taken as an example of the feature 20.

[0050] The vehicle position estimation unit 17 recognizes that a landmark feature 20 exists within the measurement range of the LIDAR 2, and sets a window 21 indicating the range in which the landmark feature 20 is predicted to exist. For example, the vehicle position estimation unit 17 may estimate the predicted vehicle position X - The vehicle position estimation unit 17 identifies, as a landmark, a feature 20 whose location information indicates a position within a predetermined distance from (t) and whose location information is included in the feature information, and sets a rectangular area of ​​a predetermined size centered on the position indicated by the location information of the feature 20 as the window 21. In this case, the vehicle position estimation unit 17 may determine the size of the window 21 by further referring to the size information of the feature 20. Then, the vehicle position estimation unit 17 regards the point cloud data of measurement points that indicate positions within the window 21 and have reflection intensity equal to or greater than a predetermined threshold as point cloud data of the feature 20.

[0051] In this case, as shown in FIG. 8B, the width W of the feature 20 identified based on the point cloud data of the lidar 2 in the window 21 is L (t) is the width W of feature 20 on the map M Therefore, in this case, the vehicle position estimation unit 17 sets the reliability information a(t) to a value smaller than 1 based on the equation (2) (see FIGS. 7A and 7C).

[0052] On the other hand, the vehicle position estimation unit 17 averages the measurement values ​​of each measurement point within the window 21 to obtain the estimated vehicle position X ^ The measurement value Z(t) of the feature 20 used to calculate (t) (see equation (1)) is calculated. Here, because occlusion occurs in a part of the left side of the feature 20, the measurement value Z(t) calculated by the above-mentioned averaging is a value that is shifted to the right from the value that should have been calculated (i.e., the value indicating the center position of the feature 20).

[0053] In this way, according to equation (2), the vehicle position estimation unit 17 can set the reliability information a(t) to a low value when it is estimated that an occlusion will occur at a landmark and that a deviation will occur in the measurement value Z(t). As a result, as will be described later, the vehicle position estimation unit 17 can set the reliability information a(t) to a low value when the reliability of the measurement value Z(t) is low. - The amount of correction for (t) is reduced, and the accuracy of self-location estimation is suitably improved by preventing inaccurate correction.

[0054] Figure 9(A) is a diagram that shows a schematic view of the irradiation range of the laser light of the lidar 2 when another feature 25 is present adjacent to the landmark feature 20, and Figure 9(B) is a plan view of the feature 20 in Figure 9(A) that clearly shows the measurement points on the irradiated surface onto which the laser light of the lidar 2 is irradiated.

[0055] 9(A), after setting a window 21, the vehicle position estimation unit 17 regards the point cloud data of measurement points within the window 21 that have a reflection intensity equal to or greater than a predetermined threshold as point cloud data of the feature 20. Here, the window 21 includes not only the measurement points of the feature 20 but also some measurement points of a feature 25 adjacent to the feature 20.

[0056] In this case, as shown in FIG. 9B, the width W L (t) is the width W of feature 20 on the map M Therefore, in this case, the vehicle position estimation unit 17 calculates the width W L (t) and the width on the map W M Since the difference between the two becomes large, the reliability information a(t) is set to a value smaller than 1 based on equation (2) (see FIGS. 7A and 7C).

[0057] On the other hand, the vehicle position estimation unit 17 averages the measurement values ​​of each measurement point within the window 21 to obtain the estimated vehicle position X ^The measurement value Z(t) of the feature 20 used to calculate (t) (see equation (1)) is calculated. Here, the point cloud data in the window 21 includes not only point cloud data obtained by measuring the feature 20 but also point cloud data obtained by measuring a feature 25 adjacent to the feature 20, so the measurement value Z(t) calculated by the above-mentioned averaging is a value shifted to the left of the value that should have been calculated (i.e., the value indicating the center position of the feature 20).

[0058] In this way, according to equation (2), even if a deviation occurs in the measurement value Z(t) due to another feature being adjacent to the feature to be measured, the vehicle position estimation unit 17 can set the reliability information a(t) to a low value. As a result, as will be described later, the vehicle position estimation unit 17 can set the predicted vehicle position X - The amount of correction for (t) is reduced, and the accuracy of self-location estimation is suitably improved by preventing inaccurate correction.

[0059] In addition, many landmark features use retroreflective materials, and the intensity of the reflected light returning to the lidar 2 is high, so they can be said to be targets that are easy to extract point cloud data from by filtering using a threshold value for the reflection intensity. On the other hand, in the driving environment, there are many objects with high reflection intensity, such as delineators and reflectors on other vehicles, and there is a possibility that the point cloud data of these objects will be mistakenly detected as point cloud data of landmarks. In such cases, the width W L (t) is the width W M or / and the vertical width H L (t) is the vertical width H M In such a case, in this embodiment, the vehicle position estimation unit 17 can appropriately set the reliability information a(t) indicating that the reliability of the measurement value is low, based on equation (2).

[0060] (2) Calculation of reliability information for each direction Next, an example of calculating reliability information for each of the vehicle's traveling direction and lateral direction will be described. In this case, roughly speaking, the vehicle position estimation unit 17 refers to the orientation information included in the feature information, and calculates reliability information for each of the vehicle's traveling direction and lateral direction based on the relative orientation of the landmark with respect to the vehicle's traveling direction. Hereinafter, the reliability information for the vehicle's traveling direction at time t will be referred to as "a X (t)" and horizontal reliability information "a Y (t)".

[0061] When the landmark faces the same direction as the vehicle's traveling direction, the illuminated surface of the landmark is perpendicular to the vehicle's traveling direction. In this case, the measured width W L (t) and vertical width H L (t) and the width on the map W M and vertical width H M If the difference is large, the center point of the measurement value Z(t) will be shifted, but the measurement value L of the vehicle's traveling direction component will be X There is no significant error in (t). X It is not necessary to make the value of (t) too small. On the other hand, the measured value of the lateral component of the vehicle L Y Since there is an error in (t), the reliability information a Y The value of (t) needs to be reduced.

[0062] Taking the above into consideration, the vehicle position estimation unit 17 calculates the reliability information a X (t) and reliability information a Y (t) and the different sensitivity coefficient "k X "," k Y " are used to perform calculations by substituting the sensitivity coefficient k in equation (2). Specifically, the vehicle position estimation unit 17 performs calculations by substituting the sensitivity coefficient k X The reliability information a is calculated using the following formula (3) X (t) and the sensitivity coefficient k Y Using the following equation (4), a Y Calculate (t).

[0063]

number

[0064]

number

[0065] FIG. 10 is a diagram showing the positional relationship between a vehicle and landmarks expressed in a two-dimensional world coordinate system (absolute coordinate system) and a vehicle coordinate system. Here, the world coordinate system has a predetermined point as its origin and mutually perpendicular coordinate axes "x w " and coordinate axis "y w The vehicle coordinate system has a coordinate axis "x" along the direction of travel of the vehicle, with the center of the vehicle as the origin. b ” and the coordinate axis along the side of the vehicle, “y b In FIG. 10, the yaw angle of the vehicle in the world coordinate system is represented as "ψ - ", the vehicle's position is [x - , y - ] T The yaw angle of the landmark in the world coordinate system is defined as "M ψ ", position to [M x , M y ] T The yaw angle of the landmark in the vehicle coordinate system is defined as "L ψ ", position to [L x , L y] T It states that:

[0066] In this case, the orientation of the landmark as seen from the vehicle is determined by the yaw angle L of the landmark in the vehicle coordinate system. ψ Equal to the yaw angle L ψ As shown in Figure 10, ψ -ψ - Therefore, the vehicle position estimation unit 17 calculates the sensitivity coefficient k X , k Y "M ψ -ψ - " and a constant "c" are used to determine the values ​​using the following equations (5) and (6), respectively.

[0067]

number

[0068]

number

[0069] Figure 11(A) shows the "M ψ -ψ - When " is 10°, the width W M 60cm, vertical width H M The vertical width H L Width W when (t) is fixed at 60cm L (t) and the reliability information a based on Eq. (3) X 11(B) is a two-dimensional graph showing the relationship between the width W and the thickness W under the same conditions as in FIG. 11(A). L (t) and the reliability information a based on Eq. (4) Y11 and 12, which will be described later, the constant c is set to 20.

[0070] In the example of Figure 11(A), "M ψ -ψ - Since " is 10°, the sensitivity coefficient k X On the other hand, in the example of FIG. 11(B), the sensitivity coefficient k Y is "5.759c" based on equation (6). In this way, when the landmark is facing almost directly ahead as seen from the vehicle, the sensitivity coefficient k X is close to c, and the sensitivity coefficient k Y is the sensitivity coefficient k X In this case, the reliability information a Y (t) is the measured width W L (t) and vertical width H L (t) and the width on the map W M and vertical width H M The sensitivity to deviations from the

[0071] Figure 12(A) shows the "M ψ -ψ - When the angle is 60°, the width W M 60cm, vertical width H M The vertical width H L Width W when (t) is fixed at 60cm L (t) and the reliability information a based on Eq. (3) X 12(B) is a two-dimensional graph showing the relationship between the width W and the thickness W under the same conditions as in FIG. 12(A). L (t) and the reliability information a based on Eq. (4) Y This is a two-dimensional graph showing the relationship between (t) and

[0072] In the example of Figure 12(A), "M ψ -ψ - Since " is 60°, the sensitivity coefficient k X On the other hand, in the example of FIG. 12(B), the sensitivity coefficient k Yis "1.155c" based on equation (6). In this way, when the landmark is slightly sideways as seen from the vehicle, the sensitivity coefficient k X is the sensitivity coefficient k Y In this case, the reliability information a X (t) is the measured width W L (t) and vertical width H L (t) and the width on the map W M and vertical width H M The sensitivity to deviations from the reliability information a Y It is relatively higher than (t).

[0073] [Kalman gain according to reliability information] Next, a method for setting the Kalman gain according to the reliability information will be described.

[0074] The vehicle position estimation unit 17 calculates the Kalman gain K(t) by the following general formula (7), and assigns the reliability information a X (t), a Y Multiply each by the inverse of (t).

[0075]

number

[0076]

number

[0077] According to equation (7), when the reliability information is the highest value of "1," its inverse is also 1, so the modified observation noise matrix R(t)' remains the original observation noise matrix R(t). On the other hand, when the reliability information is a value close to "0," its inverse is a value greater than 1, and the diagonal elements of the modified observation noise matrix R(t)' are multiplied.

[0078] Then, the vehicle position estimation unit 17 uses the corrected observation noise matrix R(t)' shown in equation (7) to calculate an adaptive Kalman gain "K(t)'" from the following equation (9).

[0079]

number

[0080] The vehicle position estimation unit 17 adds reliability information a to the diagonal components of the observation noise matrix R(t). X (t), a Y Instead of multiplying the inverse of (t), we multiply the reliability information a X (t), a Y Alternatively, the vehicle position estimation unit 17 may multiply the Kalman gain K(t)′ by the inverse of (t). In this case, the vehicle position estimation unit 17 calculates the Kalman gain K(t)′ based on the following equation (10).

[0081]

number

[0082] [Processing flow] 13 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 FIG. 13. Here, as an example, the reliability information a X (t), a Y An example of calculating (t) will be described.

[0083] 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 acquires 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 direction of the vehicle based on the results acquired in step S102 (step S103).

[0084] Thereafter, the vehicle position estimation unit 17 calculates the estimated vehicle position X ^ (t-1) is added to the travel distance and direction change calculated in step S103 to obtain the predicted vehicle position X - (t) (step S104). Furthermore, the vehicle position estimation unit 17 calculates the predicted vehicle position X - Based on (t), the feature information in the map DB 10 is referenced to search for landmarks within the measurement range of the LIDAR 2 (step S105).

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

[0086] Then, the vehicle position estimation unit 17 calculates the predicted vehicle position X - (t) indicates the vehicle's heading (yaw angle) ψ - Based on the vehicle's traveling direction corresponding to the direction of travel of the vehicle and the landmark orientation information recorded in the map DB 10, a sensitivity coefficient k X and the vehicle's lateral sensitivity coefficient k Y In this case, for example, the vehicle position estimation unit 17 determines the sensitivity coefficient k X Calculate the sensitivity coefficient k based on equation (6). Y Calculate.

[0087] Next, the vehicle position estimation unit 17 calculates the width W of the landmark on the map. M and vertical width H M and the measured width W L (t) and vertical width H L (t) and the sensitivity coefficient k X , k Y Based on this, the reliability information a X (t), a Y Then, the vehicle position estimation unit 17 calculates the reliability information a X (t), a Y For example, the vehicle position estimation unit 17 generates a Kalman gain K(t)' according to the reliability information a X (t), a Y After calculating the corrected observation noise matrix R(t)' based on equation (8) using (t), the vehicle position estimation unit 17 calculates the Kalman gain K(t)' based on equation (9). Thereafter, the vehicle position estimation unit 17 uses the Kalman gain K(t)' instead of K(t) in equation (1) to calculate the predicted vehicle position X - (t) is corrected and the estimated vehicle position X ^ (t) is calculated (step S110).

[0088] As described above, the vehicle position estimation unit 17 of the in-vehicle device 1 according to this embodiment acquires point cloud data, which is the measurement result of the landmarks by the LIDAR 2, and also acquires feature information of the landmarks contained in the map DB 10. Then, the vehicle position estimation unit 17 calculates reliability information indicating the measurement accuracy of the landmarks by the LIDAR 2 based on the difference between the size of the landmark identified based on the acquired point cloud data and the size of the landmark indicated by the feature information. As a result, the vehicle position estimation unit 17 calculates the estimated vehicle position X ^ (t) is the predicted vehicle position X - The amount of correction for (t) can be determined appropriately depending on the accuracy of the measurement results of the lidar 2.

[0089] [Variations] Preferred modifications of the embodiments will be described below. The following modifications may be applied to these embodiments in combination.

[0090] (Variation 1) The vehicle position estimation unit 17 uses the absolute values ​​|W L (t)-W M |, |H L (t)-H M The reliability information a(t) may be calculated based on the following equation (11) using |.

[0091]

number

[0092] Figure 14(A) shows the width W M 60cm, vertical width H M The vertical width H measured at the landmark where L Width W when (t) is fixed at 60cm L 14(B) is a two-dimensional graph showing the relationship between the horizontal width W(t) and the reliability information a(t) based on Equation (11). L (t) is fixed at 60cm. L 14(A) and (B) are two-dimensional graphs showing the relationship between the horizontal width W L (t) and vertical width H L 14(D) is a three-dimensional graph showing the relationship between (t) and reliability information a(t) based on equation (11). Also, FIG. 14(D) is a graph showing the inverse of the reliability information a(t) in FIG. 14(A).

[0093] As shown in FIG. 14(A) and FIG. 14(C), the width W L (t) is the width W M As the vertical width H approaches 60 cm, the reliability information a(t) approaches 1. On the other hand, as shown in FIG. 14(B) and FIG. 14(C), L (t) is the vertical width H M The closer to 60 cm, the closer the reliability information a(t) is to 1. Also, as shown in FIG. 14(D), the reciprocal of the reliability information a(t) is L (t) is the width W M The closer it is to , the closer it is to 1, and the width W L (t) is the width W M Similarly, the inverse of the reliability information a(t) is L (t) is the vertical width H M The closer it gets to 1, the closer it gets to the vertical width H L (t) is the vertical width H M The further away from this point, the larger the value.

[0094] In this way, the reliability information a(t) based on equation (11) has a maximum value of 1, similar to the reliability information a(t) based on equation (2), and the larger the difference between the measured width and height and the width and height on the map, the smaller the value becomes. Therefore, the vehicle position estimation unit 17 calculates the corrected observation noise matrix R(t)' based on equation (8) using the inverse of the reliability information a(t) based on equation (11) to estimate the estimated vehicle position X ^ (t) is the predicted vehicle position X - The amount of correction for (t) can be suitably determined.

[0095] As in the embodiment, the vehicle position estimation unit 17 estimates the reliability information a X (t) and reliability information a Y In this case, the vehicle position estimation unit 17 may calculate the sensitivity coefficient k X , k Y By calculating based on equations (5) and (6), the reliability information a X (t) and reliability information a Y (t) are calculated respectively.

[0096] (Variation 2) Instead of using equation (2), the vehicle position estimation unit 17 may determine the reliability information a(t) so that the higher the reliability, the closer it is to the minimum value of 1, and the lower the reliability, the larger the value.

[0097] For example, the vehicle position estimation unit 17 calculates the reliability information a(t) based on the following equation (12).

[0098]

number

[0099] Figure 15(A) shows the width W M 60cm, vertical width H M The vertical width H measured at the landmark where L Width W when (t) is fixed at 60cm L 15(B) is a two-dimensional graph showing the relationship between the horizontal width W(t) and the reliability information a(t) based on Equation (12). L (t) is fixed at 60cm. L 15(A) and (B) are two-dimensional graphs showing the relationship between the width W L (t) and vertical width H L 10 is a three-dimensional graph showing the relationship between (t) and reliability information a(t) based on equation (12).

[0100] As shown in FIGS. 15A and 15C, the reliability information a(t) is M The width W is 60cm L As (t) approaches, it approaches the minimum value of 1, and the width W M From width W L Similarly, as shown in FIG. 15(B) and FIG. 15(C), the reliability information a(t) is M The vertical width H is 60cm L As (t) approaches, it approaches the minimum value of 1, and the vertical width H M From vertical width H L The further away (t) is, the larger the value becomes.

[0101] As in the embodiment, the vehicle position estimation unit 17 estimates the reliability information a X (t) and reliability information a Y In this case, the vehicle position estimation unit 17 may calculate the sensitivity coefficient k X , k YBy calculating based on equations (5) and (6), the reliability information a X (t) and reliability information a Y (t) are calculated respectively.

[0102] Furthermore, the reliability information a(t) based on the formula (12) has a minimum value of 1, similar to the reciprocal of the reliability information a(t) based on the formula (2) (see FIG. 7(D)), and the width W M From width W L (t) moves away or vertical width H M From vertical width H L Therefore, when calculating the corrected observation noise matrix R(t)', the vehicle position estimation unit 17 does not need to multiply the diagonal elements of the observation noise matrix R(t) by the inverse of the reliability information a(t). X (t) and reliability information a Y When each of (t) is calculated, the corrected observation noise matrix R(t)' can be calculated based on the following equation (13).

[0103]

number

[0104] (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 that executes the processing of the vehicle position estimation unit 17 of the on-board device 1. In this case, the map DB 10 is stored in, for example, a storage unit in the vehicle, and the electronic control device of the vehicle calculates the estimated vehicle position by executing the processing of the flowchart in Fig. 13.

[0105] (Variation 4) The reliability information is the estimated vehicle position X ^(t) is the predicted vehicle position X - The reliability information is not limited to being used as a parameter for determining the correction amount of (t). For example, the vehicle-mounted device 1 may use the calculated reliability information for other purposes, such as obstacle detection based on the point cloud data output by the LIDAR 2. [Explanation of symbols]

[0106] 1 Onboard device 2 Rider 3 Gyro sensor 4 Vehicle speed sensor 5 GPS receiver 10 Map DB

Claims

[Claim 1] a first acquisition unit that acquires the results of measurement of the feature by the measurement unit; a second acquisition unit that acquires feature information of the feature included in the map data; a calculation unit that calculates accuracy information indicating the accuracy of measurement of the feature by the measurement unit based on a comparison result between the measurement result and the feature information.

Images