Self-position estimation device

Abstract

This enables highly accurate self-position estimation even in areas with few surrounding structures or where similar structures are present in a continuous series. [Solution] The self-position estimation device is composed of 3D virtual regions of a predetermined size and acquires map data including first point cloud feature data that shows the characteristics of the first point cloud corresponding to the reflected light of light irradiated onto an object existing within the 3D virtual region. The first point cloud feature data is data that reflects the reflection intensity of the first point cloud. The self-position estimation device also generates measurement data for each 3D virtual region, including second point cloud feature data that shows the characteristics of the second point cloud obtained by measurement using a measurement unit that receives the reflected light of light irradiated onto an object existing within the 3D virtual region. The second point cloud feature data is data that reflects the reflection intensity of the second point cloud. The self-position estimation device then estimates the self-position of a moving object based on the first point cloud feature data and the second point cloud feature data for each 3D virtual region.

Description

[Technical Field]

[0001] This invention relates to a point cloud matching technique. [Background technology]

[0002] Conventionally, there is a known technique for estimating a vehicle's own position by matching shape data of surrounding objects measured using measuring devices such as laser scanners with map information that has been pre-stored with the shapes of surrounding objects. For example, Patent Document 1 discloses an autonomous mobile system that determines whether a detected object in a voxel, which is a space divided according to a predetermined rule, is stationary or moving, and performs matching between map information and measurement data for voxels in which stationary objects exist. [Prior art documents] [Patent Documents]

[0003] [Patent Document 1] International release WO2013 / 076829 [Overview of the Initiative] [Problems that the invention aims to solve]

[0004] In the above method, if there are many structures around the road and they are varied, the difference between when the map information and measurement data match and when they do not will be large, making matching easier and increasing the accuracy of location estimation. On the other hand, if there are few surrounding structures or if similar structures are continuous, the difference between when the map information and measurement data match and when they do not will be small, making matching more difficult and reducing the accuracy of location estimation. Specifically, difficulty in matching means that matching takes a long time or the location estimation result is inaccurate.

[0005] The above are just a few examples of problems that this invention aims to solve. The present invention aims to provide a self-position estimation device that can estimate its own position with high accuracy even in areas where there are few surrounding structures or where similar structures are continuous. [Means for solving the problem]

[0006] The invention described in the claims is a self-position estimation device for estimating the self-position of a moving object, comprising: map data acquisition means for acquiring map data which is composed of three-dimensional virtual region units of a predetermined size and includes first point cloud feature data which shows the characteristics of the first point cloud corresponding to the reflected light of light irradiated onto an object present in the three-dimensional virtual region; measurement data generation means for generating measurement data which shows the characteristics of the second point cloud obtained by measuring the reflected light of light irradiated onto an object present in the three-dimensional virtual region for each three-dimensional virtual region; and estimation means for estimating the self-position based on the first point cloud feature data and the second point cloud feature data for each three-dimensional virtual region, wherein the first point cloud feature data is data which reflects the reflection intensity of the first point cloud, and the second point cloud feature data is data which reflects the reflection intensity of the second point cloud.

[0007] The invention described in the claims is a self-position estimation method performed by a self-position estimation device for estimating the self-position of a moving object, comprising: a map data acquisition step of acquiring map data consisting of three-dimensional virtual region units of a predetermined size, and including first point cloud feature data that shows the characteristics of a first point cloud corresponding to the reflected light of light irradiated onto an object present in the three-dimensional virtual region; a measurement data generation step of generating measurement data for each three-dimensional virtual region, including second point cloud feature data that shows the characteristics of a second point cloud obtained by measurement by a measurement unit that receives the reflected light of light irradiated onto an object present in the three-dimensional virtual region; and an estimation step of estimating the self-position based on the first point cloud feature data and the second point cloud feature data for each three-dimensional virtual region, wherein the first point cloud feature data is data that reflects the reflection intensity of the first point cloud, and the second point cloud feature data is data that reflects the reflection intensity of the second point cloud.

[0008] The invention described in the claims is a program executed by a self-position estimation device equipped with a computer for estimating the self-position of a moving object, wherein the computer functions as follows: map data acquisition means for acquiring map data consisting of three-dimensional virtual region units of a predetermined size, and including first point cloud feature data that shows the characteristics of the first point cloud corresponding to the reflected light of light irradiated onto an object present in the three-dimensional virtual region; measurement data generation means for each three-dimensional virtual region, and including second point cloud feature data that shows the characteristics of the second point cloud obtained by measurement by a measurement unit that receives the reflected light of light irradiated onto an object present in the three-dimensional virtual region; and estimation means for estimating the self-position based on the first point cloud feature data and the second point cloud feature data for each three-dimensional virtual region, wherein the first point cloud feature data is data that reflects the reflection intensity of the first point cloud, and the second point cloud feature data is data that reflects the reflection intensity of the second point cloud. [Brief explanation of the drawing]

[0009] [Figure 1] This is a schematic configuration of the driver assistance system. [Figure 2] This shows the block configuration of the in-vehicle equipment and server device. [Figure 3] An example of a data structure for voxel data is shown. [Figure 4] Examples of basic voxel data and scan data are shown. [Figure 5] Examples of a 2D normal distribution for voxel data and the mean value for scan data are shown. [Figure 6] Examples of voxel data and scan data in the first embodiment are shown. [Figure 7] An example of matching using the average reflectance is shown. [Figure 8] This is a flowchart of the vehicle position estimation process. [Figure 9] An example of a two-dimensional normal distribution of voxel data according to the second embodiment is shown. [Modes for carrying out the invention]

[0010] In one preferred embodiment of the present invention, a self-position estimation device for estimating the self-position of a moving object comprises: a map data acquisition means for acquiring map data which includes a first point cloud feature data that shows the characteristics of a first point cloud corresponding to the reflected light of light irradiated onto an object present in the three-dimensional virtual region, and is composed of three-dimensional virtual region units of a predetermined size; a measurement data generation means for generating measurement data which includes a second point cloud feature data that shows the characteristics of a second point cloud obtained by measurement using a measurement unit that receives the reflected light of light irradiated onto an object present in the three-dimensional virtual region; and an estimation means for estimating the self-position based on the first point cloud feature data and the second point cloud feature data for each three-dimensional virtual region, wherein the first point cloud feature data is data that reflects the reflection intensity of the first point cloud, and the second point cloud feature data is data that reflects the reflection intensity of the second point cloud.

[0011] The self-position estimation device described above is composed of units of three-dimensional virtual regions of a predetermined size. It acquires map data that includes first-point cloud feature data, which represents the characteristics of the first point cloud corresponding to the reflected light of light irradiated onto an object present in the three-dimensional virtual region. The first-point cloud feature data reflects the reflection intensity of the first point cloud. The self-position estimation device also generates measurement data for each three-dimensional virtual region, which includes second-point cloud feature data, which represents the characteristics of the second point cloud obtained by a measurement unit that receives the reflected light of light irradiated onto an object present in the three-dimensional virtual region. The second-point cloud feature data reflects the reflection intensity of the second point cloud. The self-position estimation device then estimates the self-position of a moving object based on the first-point cloud feature data and second-point cloud feature data for each three-dimensional virtual region. Because this self-position estimation device estimates its self-position using the reflection intensity of the point cloud, it can estimate its self-position with high accuracy even in areas with few surrounding structures or areas where similar structures are continuous.

[0012] In one embodiment of the self-localization device described above, the first point cloud feature data includes the average position of the first point cloud, the variance of the first point cloud, and the average reflectance of the first point cloud, and the second point cloud feature data includes the average position of the second point cloud and the average reflectance of the second point cloud.

[0013] In another embodiment of the self-localization device described above, the estimation means obtains a matching result between the first point cloud feature data and the second point cloud feature data for each three-dimensional virtual region using an evaluation formula that uses the average reflectance of the first point cloud and the average reflectance of the second point cloud as weighted values.

[0014] In another embodiment of the self-localization device described above, the first point cloud feature data includes the average position and variance of the first point cloud calculated using the reflectance of each point in the first point cloud, and the second point cloud feature data includes the average position of the first point cloud calculated using the reflectance of each point in the second point cloud.

[0015] In another preferred embodiment of the present invention, a self-position estimation method performed by a self-position estimation device for estimating the self-position of a moving object comprises: a map data acquisition step of acquiring map data consisting of a predetermined size of three-dimensional virtual region units and including first point cloud feature data that shows the characteristics of a first point cloud corresponding to the reflected light of light irradiated onto an object present in the three-dimensional virtual region; a measurement data generation step of generating measurement data for each three-dimensional virtual region including second point cloud feature data that shows the characteristics of a second point cloud obtained by measurement by a measurement unit that receives the reflected light of light irradiated onto an object present in the three-dimensional virtual region; and an estimation step of estimating the self-position based on the first point cloud feature data and the second point cloud feature data for each three-dimensional virtual region, wherein the first point cloud feature data is data that reflects the reflection intensity of the first point cloud, and the second point cloud feature data is data that reflects the reflection intensity of the second point cloud. Since this self-position estimation method estimates the self-position using the reflection intensity of the point cloud, it can estimate the self-position with high accuracy even in areas where there are few surrounding structures or where similar structures are continuous.

[0016] In another preferred embodiment of the present invention, a program executed by a self-position estimation device equipped with a computer for estimating the self-position of a moving object is configured in units of three-dimensional virtual regions of a predetermined size. The program includes map data acquisition means for acquiring map data that includes first point cloud feature data indicating the characteristics of a first point cloud corresponding to the reflected light of light irradiated onto an object present in the three-dimensional virtual region; measurement data generation means for generating measurement data for each three-dimensional virtual region that includes second point cloud feature data indicating the characteristics of a second point cloud obtained by measurement using a measurement unit that receives the reflected light of light irradiated onto an object present in the three-dimensional virtual region; and estimation means for estimating the self-position based on the first point cloud feature data and the second point cloud feature data for each three-dimensional virtual region. The first point cloud feature data is data that reflects the reflection intensity of the first point cloud, and the second point cloud feature data is data that reflects the reflection intensity of the second point cloud. By executing this program on a computer, the above-described self-position estimation device can be realized. This program can be stored and handled on a storage medium. [Examples]

[0017] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings.

[0018] [Overview of the driver assistance system] Figure 1 shows a schematic configuration of a driver assistance system according to an embodiment. The driver assistance system comprises an in-vehicle unit 1 that moves with the vehicle and a server device 2 that distributes map information. In Figure 1, only one set of in-vehicle unit 1 and vehicle communicating with the server device 2 is shown, but in reality, there are multiple sets of in-vehicle unit 1 and vehicles at different locations.

[0019] The in-vehicle unit 1 is electrically connected to external sensors such as a Lidar (Light Detection and Ranging, or Laser Illuminated Detection and Ranging) and internal sensors such as a gyroscope and a vehicle speed sensor. Based on the outputs of these sensors, it estimates the position of the vehicle on which the in-vehicle unit 1 is installed (also called the "vehicle position"). Based on the estimated vehicle position, the in-vehicle unit 1 performs automatic driving control of the vehicle to drive along a set route to a destination.

[0020] The in-vehicle unit 1 stores a map database (DB:DataBase) 10 containing voxel data. Voxel data is data that records position information of stationary structures for each region (also called a "voxel") when a 3D space is divided into multiple regions. The voxel data includes data that represents the measured point cloud data of stationary structures within each voxel using a normal distribution, and is used for scan matching using NDT (Normal Distributions Transform), as described later. The in-vehicle unit 1 performs scan matching based on NDT based on the point cloud data measured by the lidar and the voxel data corresponding to the voxel to which the point cloud data belongs. Then, the in-vehicle unit 1 corrects the estimated position of the vehicle based on the matching result and performs automatic driving control, etc. In the following explanation, the map used for NDT scan matching will also be called the "ND map". Furthermore, the point cloud data used to create the voxel data for the ND map is called "map creation point cloud data," and the point cloud data measured by the LIDA 30 during scan matching is called "scan data" to distinguish between the two.

[0021] Server device 2 communicates data with the in-vehicle units 1 of multiple vehicles. Server device 2 stores a distribution map DB 20 that stores map information for distribution to the in-vehicle units 1 of multiple vehicles, and this map information includes voxel data corresponding to each voxel.

[0022] [In-vehicle system configuration] Figure 2(A) shows a block diagram representing the functional configuration of the in-vehicle unit 1. As shown in Figure 2(A), the in-vehicle unit 1 mainly consists of a communication unit 11, a storage unit 12, a sensor unit 13, an input unit 14, a control unit 15, and an output unit 16. The communication unit 11, storage unit 12, sensor unit 13, input unit 14, control unit 15, and output unit 16 are interconnected via a bus line.

[0023] The communication unit 11 receives map information distributed from the server device 2 based on the control of the control unit 15. The communication unit 11 also transmits measurement data from the sensor unit 13 to the server device 2 when requested by the server device 2. In addition, the communication unit 11 receives signals from the server device 2 for controlling the vehicle and transmits signals related to the vehicle's status to the server device 2.

[0024] The storage unit 12 stores programs executed by the control unit 15 and information necessary for the control unit 15 to perform predetermined processes. The storage unit 12 also has a map database 10 that stores voxel data of the ND map. The voxel data of the ND map stored in the storage unit 12 is downloaded from the distribution map database of the server device 2.

[0025] The sensor unit 13 includes a lidar 30, a camera 31, a GPS receiver 32, a gyro sensor 33, and a speed sensor 34. The lidar 30 discretely measures the distance to an object in the outside world by emitting a pulsed laser within a predetermined angular range in the horizontal and vertical directions, and generates scan data, which is three-dimensional point cloud data indicating the position of the object. In this case, the lidar 30 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 distance to the object in that irradiation direction of the laser light, which is determined based on the light receiving signal described above.

[0026] The input unit 14 includes buttons, a touch panel, a remote controller, a voice input device, etc., for user operation. It accepts inputs such as specifying a destination for route searching and specifying whether to turn autonomous driving on or off, and supplies the generated input signals to the control unit 15. The output unit 16 includes, for example, a display or speaker that outputs based on the control of the control unit 15.

[0027] The control unit 15 includes a CPU for executing programs and controls the entire in-vehicle unit 1. For example, the control unit 15 estimates the vehicle's position by performing scan matching based on NDT, using scan data output from the lidar 30 and voxel data of the ND map corresponding to the voxel to which the scan data belongs. The control unit 15 also downloads voxel data of the ND map from the server device 2 as needed. Furthermore, if requested by the server device 2, the control unit 15 transmits measurement data from the sensor unit 30, such as scan data from the lidar 30, to the server device 2.

[0028] In the above configuration, the in-vehicle unit 1 is an example of the self-position estimation device of the present invention, the control unit 15 is an example of the map data acquisition means and measurement data generation means of the present invention, and the lidar 30 is an example of the measurement unit of the present invention.

[0029] [Server configuration] Figure 2(B) shows the schematic configuration of server device 2. As shown in Figure 2(B), server device 2 has a communication unit 21, a storage unit 22, and a control unit 25. The communication unit 21, the storage unit 22, and the control unit 25 are interconnected via a bus line.

[0030] The communication unit 21 communicates various data with the in-vehicle device 1 based on the control of the control unit 25. The storage unit 22 stores programs for controlling the operation of the server device 2 and holds information necessary for the operation of the server device 2. The storage unit 22 also stores the distribution map DB 20, which includes voxel data of the ND map.

[0031] The control unit 25 includes a CPU, ROM, RAM, etc. (not shown), and performs various controls on each component within the server device 2. For example, the control unit 25 transmits voxel data contained in the distributed map 20 to the in-vehicle device 1 in response to a request from the in-vehicle device 1.

[0032] [Basic Scan Matching] Next, we will describe scan matching based on NDT in this embodiment. (1) Voxel data First, we will explain the voxel data used for scan matching based on NDT. Figure 3(A) shows an example of the data structure of voxel data for an ND map.

[0033] Voxel data is generated from map-creation point cloud data and includes parameter information for representing the point cloud within the voxel using a normal distribution. In this embodiment, as shown in Figure 3(A), the voxel data includes "voxel ID", "voxel coordinates", "mean vector", "covariance matrix", and "point cloud count information". Note that a voxel is an example of a three-dimensional virtual region of the present invention, and voxel data is an example of the first point cloud feature data of the present invention.

[0034] "Voxel coordinates" represent the absolute three-dimensional coordinates of a reference position, such as the center position of each voxel. Each voxel is a cube that divides space into a grid, and its shape and size are predetermined; therefore, the space of each voxel can be identified using voxel coordinates. Voxel coordinates may also be used as voxel IDs.

[0035] The "mean vector" and "covariance matrix" represent the mean vector and covariance matrix, which correspond to the parameters when representing the point cloud within the target voxel using a normal distribution. The coordinates of any point "i" within any voxel are X M (i) Let M be the number of points in a voxel. Then, the data X in the voxel. M The mean vector "μ" and covariance matrix "V" in (i) are expressed by the following equations (1) to (3).

Number

[0036] The "point group number information" is information indicating the number of point groups used for calculating the corresponding average vector and covariance matrix. The point group number information may be information indicating the specific number of point groups, or may be information indicating the level of the point group number (for example, large, medium, small, etc.).

[0037] FIG. 4(A) shows an example of voxel data. The voxel data includes voxel coordinates, an average vector, and a covariance matrix for each voxel defined by a voxel ID. The voxel coordinates include coordinates M x 、M y 、M z in the x, y, and z directions. The average vector includes average vectors μ x 、μ y 、μ z in the x, y, and z directions. The covariance matrix includes each element constituting the matrix shown by Equation (3).

[0038] (2) Scan data FIG. 4(B) shows an example of scan data by the lidar 30. FIG. 4(B) shows scan data corresponding to one voxel. The scan data is point group data measured by the lidar 30, and has an average vector L for each scan data number assigned to each point group data. Note that the scan data is an example of the second point group feature data of the present invention.

[0039] Now, assuming the number of point groups in the voxel is "N", the average value L of the scan data X L (j) in each voxel is given by the following equation.

Number

[0040] (3) Scan matching Next, we will explain scan matching using NDT with voxel data from an ND map. Here, the estimation parameter is the x-direction movement vector t. x , y-direction movement vector t y , z-direction movement vector t z , and the rotation angle (i.e., yaw angle) t in the azimuthal direction (xy-plane) Ψ The following parameters are used. Note that the pitch angle and roll angle are caused by road gradient and vibration and are considered to be negligibly small. Using the above estimated parameters, the coordinates of any point in the scan data measured by the lidar 30 are transformed, and the transformed coordinates are obtained by the following equation (5).

number

[0041] The in-vehicle device 1 uses the coordinates of the point cloud after coordinate transformation, the mean vector μ and covariance matrix V contained in the voxel data to calculate the evaluation function "E(k)" for the voxel with index number k shown by equation (6) below, and the overall evaluation function "E" (hereinafter also referred to as the "overall evaluation function") for all voxels subject to matching shown by equation (7).

number

[0042] Furthermore, the coordinates of the point cloud data obtained by LIDA30 and the coordinates of the mean vector of the voxel data must be unified into a coordinate system with a pre-set reference position as the origin. Therefore, coordinate transformations of both should be performed as necessary prior to matching.

[0043] Then, the in-vehicle device 1 calculates the estimated parameters when the overall evaluation function E is maximized, i.e., the movement vector t. x , t y , t z and rotation angle t Ψ The estimated vehicle position is corrected using this method.

[0044] Figure 5 conceptually illustrates the relationship between the voxel data of the ND map and the scan data obtained by the LIDA 30. For ease of explanation, Figure 5 is shown in two dimensions. Figure 5(A) shows the voxel data for four voxels B1 to B4. In each voxel, the circle Dm represents the point cloud data for map creation measured by the LIDA when the measurement and maintenance vehicle used for map creation was driven. Based on this point cloud data for map creation, a two-dimensional normal distribution created by equations (1) and (2) is shown as a gradient. The mean and variance of the normal distribution shown in Figure 5(A) correspond to the mean vector and covariance matrix in the voxel data, respectively.

[0045] Figure 5(B) shows the point cloud Ds of scan data measured by the lidar overlaid on Figure 5(A). Figure 5(C) shows the average value L of the scan data calculated from the point cloud Ds of the scan data displayed in Figure 5(B) overlaid on Figure 5(C).

[0046] [Scan matching using reflectivity] Next, we will explain scan matching using reflectance, which is a feature of this embodiment. The basic scan matching described above becomes difficult to perform when there are few surrounding structures or when similar structures are continuous, because the difference between when the map information and the measurement data match and when they do not match becomes small. Therefore, in this embodiment, scan matching is performed using the reflectance of point cloud data measured by a LiDAR.

[0047] (First embodiment) The first embodiment includes the average value of the reflectance intensity in the voxel data of the ND map, and performs scan matching using that average value and the average value of the reflectance intensity of the scan data measured by the LIDA 30. Figure 3(B) shows an example of the data structure of the voxel data of the ND map according to the first embodiment, and Figure 6(A) shows an example of the voxel data according to the first embodiment. Also, Figure 6(B) shows an example of the scan data according to the first embodiment. As can be seen by comparing with Figures 4(A) and 4(B), the voxel data includes the average value of the reflectance intensity of the point cloud contained within each voxel I MIt also includes the average value of the reflectance intensity of the scan data within the voxel, I, on the scan data side measured by the RIDA30. L This is calculated.

[0048] The in-vehicle unit 1 uses the average value of the reflectivity of the voxel data I as the evaluation function for scan matching. M and the average value of the reflectance intensity of the scan data I L These are used as weighting values. Specifically, the in-vehicle device 1 uses equation (8) to determine the evaluation function E(k) of a certain voxel k, and calculates the overall evaluation function E of all voxels using equation (9).

number

[0049] The evaluation function in equation (8) is the average value of the reflectance intensity of the voxel data I M and the average value of the reflectance intensity of the scan data I L The product of "I M ·I L This is included as a weighting value. By using this evaluation function, a difference is created between the evaluation function value of voxels that contain objects with high reflectivity (such as road signs and white lines) and the evaluation function value of voxels that do not contain objects with high reflectivity. This will be explained with a simple example.

[0050] Figure 7 shows the "I" used in equation (8). M ·I L This figure shows an example of calculating the value of ". Now, assume that the vehicle is traveling in the direction of the arrow, and there is an object such as a wall with a relatively uniform reflectivity to the left of the vehicle. This wall portion is voxel B. M1 ~B M6 Corresponding to, among them, voxel B M4 Average value of reflectance I M This is "1.0", and other voxel B M1 ~B M3 B M5 ~B M6 Average value of reflectance I M Assume that all values ​​are "0.1". That is, voxel B M4Only other voxel B M1 ~B M3 B M5 ~B M6 It has a higher reflectivity.

[0051] Now, the vehicle passes in front of this wall, and the lidar scans the wall from the side to generate scan data, and as shown in the figure, multiple voxel-based scan data B La ~B Le Assume that the following has been obtained. In the obtained scan data, voxel B Ld Average value of reflectance I L This is "1.0", and the other voxels B La ~B Lc B Le Reflectance I L Let's assume that this is "0.1". For the sake of simplicity, we will assume that voxel data and scan data can be matched only by the movement of the vehicle in the direction of travel.

[0052] In this case, as can be seen from Figure 7, voxel B of the scan data La ~B Le However, each of the voxel data voxel B M1 ~B M5 It is correct to conclude that this corresponds to "I M ·I L Let's consider what happens when we use the evaluation function of formula (8), which uses the value of ", to make a decision.

[0053] First, let's assume that voxel B of the scan data La ~B Le However, voxel B in voxel data M1 ~B M5 If we consider each to correspond to (hereinafter referred to as the "first correspondence"), then the "I M ·I L The value of " is I M ·I L = 0.1 × 0.1 + 0.1 × 0.1 + 0.1 × 0.1 +1.0 × 1.0 + 0.1 × 0.1 = 1.04 This is the result.

[0054] On the other hand, if we assume that voxel B of the scan data La ~B Le However, voxel B in voxel data M2 ~B M6 If we consider each to correspond to (hereinafter referred to as the "second correspondence"), then the "I M ·I L The value of " is I M ·I L = 0.1 × 0.1 + 0.1 × 0.1 + 1.0 × 0.1 +0.1 × 1.0 + 0.1 × 0.1 = 0.23 Therefore, the evaluation function value in equation (8) becomes large when "I M ·I L The one with the larger value of '', that is, the first correspondence, is the correct correspondence, as can be seen in Figure 7.

[0055] Incidentally, in Figure 7, voxel B of the wall M4 The reflectivity of that part is the same as the surrounding area, and voxel B of the voxel data M4 Average value of reflectance I M If it is "0.1" like the surrounding area, then voxel B of the scan data obtained by the lider Ld Average value of reflectance I L It also becomes "0.1". In this case, in both the first correspondence and the second correspondence, "I M ·I L The value of " is I M ·I L = 0.1 × 0.1 + 0.1 × 0.1 + 0.1 × 0.1 +0.1 × 0.1 + 0.1 × 0.1 = 0.05 As a result, they match, making it difficult to determine which of the first and second correspondences is correct.

[0056] Thus, in the first embodiment, even if there are few surrounding structures or if similar structures are continuous, by using the average reflectivity of voxel data and scan data in scan matching and including their product as a weighted value in the evaluation function, the matching accuracy of voxels containing objects with high reflectivity, such as road signs and white lines, can be improved. As a result, it becomes possible to improve the accuracy of vehicle self-position estimation.

[0057] Figure 8 is a flowchart of the vehicle position estimation process according to the first embodiment. This process is performed by the control unit 15 of the in-vehicle unit 1 executing a pre-prepared program. The in-vehicle unit 1 repeatedly performs this process.

[0058] First, the in-vehicle unit 1 determines whether or not there is an estimated vehicle position from one time point in the past (step S10). If there is an estimated vehicle position from one time point in the past (step S10: Yes), the in-vehicle unit 1 sets it as the initial value of the vehicle position (step S11). On the other hand, if there is no estimated vehicle position from one time point in the past (step S10: No), the in-vehicle unit 1 sets the initial value of the vehicle position based on the output of the GPS receiver 32, etc. (step S12). Next, the in-vehicle unit 1 obtains the vehicle speed from the speed sensor 34 and the angular velocity in the yaw direction from the gyro sensor 33 (step S13). Then, based on the results obtained in step S13, the in-vehicle unit 1 calculates the vehicle's travel distance and the change in the vehicle's orientation (step S14).

[0059] Next, the in-vehicle unit 1 adds the travel distance and azimuth change calculated in step S14 to the initial value of the vehicle's position to calculate the predicted vehicle position (step S15). Then, based on the predicted vehicle position calculated in step S15, the in-vehicle unit 1 refers to the map DB10 and obtains voxel data of voxels present around the vehicle's position (step S16). This voxel data includes the average reflectance I as shown in Figure 6(A). Mis included. Further, based on the predicted position of the host vehicle calculated in step S15, the in-vehicle device 1 divides the scan data obtained from the lidar 30 for each voxel (step S17). At this time, as shown in FIG. 6(B), the average reflection intensity value I of the scan data in the voxel L is calculated.

[0060] Next, the in-vehicle device 1 calculates NDT scan matching using an evaluation function. Specifically, based on equations (8) and (9), the in-vehicle device 1 calculates an evaluation function E k and a comprehensive evaluation function E, and calculates the estimated parameters that maximize the comprehensive evaluation function E, that is, the movement vectors t x , t y , tz and the rotation angle t Ψ (step S18). Then, the in-vehicle device 1 corrects the estimated host vehicle position based on the obtained movement vectors t x , t y , t z and the rotation angle t Ψ (step S19). Thus, the host vehicle position estimation process ends.

[0061] (Second Embodiment) In the second embodiment, the reflection intensity of the voxel data and the scan data is used in the same manner as in the first embodiment, but it is different from the first embodiment in that parameters such as an average vector and a covariance matrix are calculated using the reflection intensity of each point in the point cloud in the voxel.

[0062] Specifically, first, when generating voxel data of the ND map from the point cloud data for map creation, the average vector is calculated by the following equation (10) and the covariance matrix is calculated by equation (11) using the reflection intensity I M (i) of each point.

Equation

[0063] Furthermore, the scan data from the lidar also includes the reflectance I of each point within each voxel. L Using (j), the mean vector L is calculated by the following equation (12).

number

[0064] The voxel data and scan data of the ND map obtained by equations (10) to (12) represent the three-dimensional positional information with added characteristics of reflectivity. For example, in the case of a nearly flat road surface, the position of the point cloud is uniform on the xy plane, so the mean and variance in the x and y directions are constant. However, if there are areas with high reflectivity, such as white lines or pedestrian crossings, this creates characteristics in the mean vectors and covariance matrix in the x and y directions.

[0065] Figure 9 shows the two-dimensional normal distribution of voxel data for a certain ND map. Figure 9(A) shows the two-dimensional normal distribution 51 calculated without using reflectance. Since the reflectance of multiple points Dm is not considered, the normal distribution 51 is obtained based on the position of each point Dm. In contrast, Figure 9(B) shows the two-dimensional normal distribution 52 calculated using the reflectance of each point Dm according to the second embodiment. In this example, since multiple points Dm include a point Dmx with high reflectance, the normal distribution 52 is more biased towards point Dmx than the normal distribution 51, and reflects the characteristics of the part with high reflectance.

[0066] Thus, in the second embodiment, the average vector and the covariance matrix are calculated using the reflection intensity of each point in the voxel data of the ND map, and the average value vector is also calculated using the reflection intensity of each point in the scan data obtained by the lidar 30. Therefore, even in an environment where the shape of the road surroundings changes little, if there is a change in the reflection intensity, the parameters such as the average vector and the covariance matrix can be made characteristic by the reflection intensity, and the difference between match and mismatch during scan matching becomes large. Therefore, the accuracy of scan matching is improved, and as a result, the accuracy of estimating the vehicle position can be improved.

[0067] The vehicle position estimation process in the second embodiment is basically the same as that in the first embodiment shown in FIG. 8. However, the voxel data acquired from the map DB 10 in step S16 has the data structure and content shown in FIG. 4(A), and its average vector μ and covariance matrix V are calculated by the above-mentioned formulas (10) and (11), respectively. Also, in step S17, the in-vehicle device 1 calculates the average value vector L using the reflection intensity I L (j) of each point in each voxel. Then, in step S18, scan matching is performed using the evaluation functions of formulas (6) and (7).

[0068] (Modified Example) The above first embodiment and second embodiment may be combined and implemented. In that case, the voxel data of the ND map includes the average value of the reflection intensity, i.e., the reflection intensity average value I M as shown in FIG. 6(A). Also, as shown in FIG. 6(B), for the scan data obtained by the lidar, the reflection intensity average value I L is calculated based on the reflection intensity of each point. In addition, the average vector μ and covariance matrix V included in the voxel data are calculated using the reflection intensity I M (i) of each point as shown in the above-mentioned formulas (10) and (11), respectively. Also, the average vector L of the scan data obtained by the lidar is also calculated using the reflection intensity I LIt is calculated using (j). Then, using these voxel data and scan data, scan matching is performed using equations (8) and (9) to maximize the overall evaluation function E. [Explanation of Symbols]

[0069] 1 On-vehicle device 2 Server devices 10 Map Database 20 Distribution Map Database 11, 21 Communications Department 12, 22 Storage section 15, 25 Control Unit 13 Sensor section 14 Input section 16 Output section

Claims

[Claim 1] A self-position estimation device for estimating the self-position of a moving object, Map data acquisition means for acquiring map data consisting of three-dimensional virtual domain units of a predetermined size, and including first point cloud feature data that shows the characteristics of the first point cloud corresponding to the reflected light of light irradiated onto an object existing within the three-dimensional virtual domain, Measurement data generation means for each of the three-dimensional virtual regions, which generates measurement data including second point cloud feature data that shows the characteristics of the second point cloud obtained by measuring the reflected light of light irradiated onto an object present in the three-dimensional virtual region using a measurement unit, An estimation means for estimating the self-position based on the first point cloud feature data and the second point cloud feature data for each of the three-dimensional virtual regions, Equipped with, A self-localization device wherein the first point cloud feature data is data that reflects the reflectance intensity of the first point cloud, and the second point cloud feature data is data that reflects the reflectance intensity of the second point cloud.

Images