JP6923748B2 - Self-position estimation device, self-position estimation method, program and storage medium - Google Patents

Description

The present invention relates to a point cloud matching technique.

Conventionally, a technique for estimating the self-position of a vehicle by matching the shape data of a peripheral object measured using a measuring device such as a laser scanner with map information in which the shape of the peripheral object is stored in advance has been known. There is. For example, in Patent Document 1, it is determined whether a detected object in a voxel in which a space is divided according to a predetermined rule is a stationary object or a moving object, and matching of map information and measurement data is performed for the voxel in which the stationary object exists. The autonomous mobile system to be performed is disclosed.

International release WO2013 / 076829

In the above method, if there are many structures around the road and there is a lot of variation, it becomes easier to match because the difference between the case where the map information and the measurement data match and the case where they do not match becomes large. , The position estimation accuracy is high. On the other hand, if there are few peripheral structures or similar structures are continuous, the difference between the case where the map information and the measurement data match and the case where they do not match becomes small, so it becomes difficult to match, and the position estimation accuracy becomes difficult. Is likely to decrease. In addition, difficult to match specifically means that it takes a lot of time for matching and the position estimation result becomes inaccurate.

As an example of the problem to be solved by the present invention, the above is given as an example. The present invention provides a self-position estimation device capable of estimating a self-position with high accuracy even in an area where there are few peripheral structures or similar structures are continuous. The purpose.

The invention according to claim 1 is a self-position estimation device that estimates the self-position of a moving body, and is configured for a three-dimensional virtual area unit of a predetermined size with respect to an object existing in the three-dimensional virtual area. A map data acquisition means for acquiring map data including the first point cloud feature data indicating the features of the first point cloud corresponding to the reflected light of the irradiated light, and in the three-dimensional virtual area for each of the three-dimensional virtual areas. A measurement data generation means for generating measurement data including a second point cloud feature data showing the features of the second point cloud obtained by measuring with a measuring unit that receives the reflected light of the light radiated to an object existing in the object. The first point cloud feature data includes 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 area. is data reflecting the intensity of the first point group is reflected, the second point group feature data Ri Ah in data reflection intensity of the second point group is reflected, the first point group feature data, the The 2nd point cloud feature data includes the average position and dispersion of the 1st point cloud calculated using the reflection intensity of each point of the 1st point cloud, and the 2nd point cloud feature data uses the reflection intensity of each point of the 2nd point cloud. The average position of the first point cloud calculated in the above is included .

The invention according to claim 4 is a self-position estimation method executed by a self-position estimation device that estimates the self-position of a moving object, which is composed of a three-dimensional virtual area unit of a predetermined size and is composed of the three-dimensional virtual area. A map data acquisition step of acquiring map data including the first point cloud feature data indicating the features of the first point cloud corresponding to the reflected light of the light radiated to the object existing in the object, and each of the three-dimensional virtual areas. In addition, measurement data including a second point cloud feature data showing the features of the second point cloud obtained by measuring with a measuring unit that receives the reflected light of the light radiated to the object existing in the three-dimensional virtual area. A measurement data generation step for generating the above, and an estimation step 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 areas. the first point group characteristic data is data reflecting the intensity of the first point group is reflected, the second point group feature data Ri Ah in data reflection intensity of the second point group is reflected, the first The one-point cloud feature data includes the average position and dispersion of the first point cloud calculated using the reflection intensity of each point of the first point cloud, and the second point cloud feature data includes the second point cloud. Includes the average position of the first point cloud calculated using the reflection intensity of each point .

The invention according to claim 5 is a program that includes a computer and is executed by a self-position estimation device that estimates the self-position of a moving object, and is configured in units of three-dimensional virtual areas of a predetermined size. A map data acquisition means for acquiring map data including the first point group feature data indicating the features of the first point group corresponding to the reflected light of the light irradiated to the object existing in the area, for each of the three-dimensional virtual areas. In addition, measurement data including the second point group feature data showing the features of the second point group obtained by measuring by the measuring unit that receives the reflected light of the light radiated to the object existing in the three-dimensional virtual area. The computer is made to function as a measurement data generation means for generating the above, and an estimation means for estimating the self-position based on the first point group feature data and the second point group feature data for each of the three-dimensional virtual areas. the first point group characteristic data is data reflecting the intensity of the first point group is reflected, the second point group feature data Ri Ah in data reflection intensity of the second point group is reflected, The first point group feature data includes the average position and dispersion of the first point group calculated using the reflection intensity of each point of the first point group, and the second point group feature data includes the second point group feature data. The average position of the first point group calculated by using the reflection intensity of each point of the point group is included .

This is a schematic configuration of the driving support system. The block configuration of the in-vehicle device and the server device is shown. An example of the data structure of voxel data is shown. An example of basic voxel data and scan data is shown. An example of a two-dimensional normal distribution of voxel data and an average value of scan data is shown. An example of voxel data and scan data in the first embodiment is shown. An example of matching using the average reflection intensity is shown. It is a flowchart of the own vehicle position estimation process. An example of a two-dimensional normal distribution of voxel data according to the second embodiment is shown.

In one preferred embodiment of the present invention, the self-position estimation device for estimating the self-position of a moving body is configured in units of a three-dimensional virtual area of a predetermined size, and with respect to an object existing in the three-dimensional virtual area. A map data acquisition means for acquiring map data including the first point group feature data indicating the features of the first point group corresponding to the reflected light of the irradiated light, and in the three-dimensional virtual area for each of the three-dimensional virtual areas. A measurement data generation means for generating measurement data including a second point group feature data showing the features of the second point group obtained by measuring with a measuring unit that receives the reflected light of the light radiated to an object existing in. The first point group feature data is provided with an estimation means for estimating the self-position based on the first point group feature data and the second point group feature data for each of the three-dimensional virtual regions. The data reflects the reflection intensity of the first point group, and the second point group feature data is data that reflects the reflection intensity of the second point group.

The above-mentioned self-position estimation device is composed of a three-dimensional virtual area unit of a predetermined size, and shows the characteristics of a first point cloud corresponding to the reflected light of the light applied to an object existing in the three-dimensional virtual area. Acquire map data including 1-point cloud feature data. The first point cloud feature data is data that reflects the reflection intensity of the first point cloud. Further, the self-position estimation device is a second point cloud obtained by measuring each three-dimensional virtual area by a measuring unit that receives reflected light of light radiated to an object existing in the three-dimensional virtual area. The measurement data including the second point cloud feature data showing the feature is generated. The second point cloud feature data is data that reflects the reflection intensity of the second point cloud. Then, the self-position estimation device estimates the self-position of the moving body based on the first point cloud feature data and the second point cloud feature data for each three-dimensional virtual area. Since this self-position estimation device estimates the self-position using the reflection intensity of the point cloud, it is highly accurate even in an area where there are few peripheral structures or similar structures are continuous. The self-position can be estimated with.

In one aspect of the self-position estimation device, the first point cloud feature data includes the average position of the first point cloud, the dispersion of the one point cloud, and the average reflection intensity of the first point cloud. The second point cloud feature data includes the average position of the second point cloud and the average reflection intensity of the second point cloud.

In another aspect of the self-position estimation device, the estimation means is three-dimensionally based on an evaluation formula using the average reflection intensity of the first point cloud and the average reflection intensity of the second point cloud as weighted values. The matching result of the first point cloud feature data and the second point cloud feature data for each virtual area is obtained.

In another aspect of the self-position estimation device, the first point cloud feature data includes the average position and dispersion of the first point cloud calculated using the reflection intensity of each point in the first point cloud. , The second point cloud feature data includes the average position of the first point cloud calculated using the reflection intensity of each point of the second point cloud.

In another preferred embodiment of the present invention, the self-position estimation method executed by the self-position estimation device for estimating the self-position of the moving body is configured in units of three-dimensional virtual areas of a predetermined size, and the three-dimensional virtual area is described. A map data acquisition step of acquiring map data including the first point cloud feature data indicating the features of the first point cloud corresponding to the reflected light of the light radiated to the object existing in the object, and each of the three-dimensional virtual areas. In addition, measurement data including the second point cloud feature data showing the features of the second point cloud obtained by measuring by the measuring unit that receives the reflected light of the light radiated to the object existing in the three-dimensional virtual area. A measurement data generation step for generating the above, and an estimation step 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 areas. 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 is highly accurate even in an area where there are few peripheral structures or similar structures are continuous. The self-position can be estimated with.

In another preferred embodiment of the present invention, the program executed by the self-position estimation device including a computer and estimating the self-position of the moving body is configured in a three-dimensional virtual area unit of a predetermined size, and the three-dimensional virtual is described. A map data acquisition means for acquiring map data including the first point group feature data indicating the features of the first point group corresponding to the reflected light of the light irradiated to the object existing in the area, for each of the three-dimensional virtual areas. In addition, measurement data including the second point group feature data showing the features of the second point group obtained by measuring by the measuring unit that receives the reflected light of the light radiated to the object existing in the three-dimensional virtual area. The computer is made to function as a measurement data generation means for generating the above, and an estimation means for estimating the self-position based on the first point group feature data and the second point group feature data for each of the three-dimensional virtual areas. The first point group feature data is data that reflects the reflection intensity of the first point group, and the second point group feature data is data that reflects the reflection intensity of the second point group. By executing this program on a computer, the above-mentioned self-position estimation device can be realized. This program can be stored and handled in a storage medium.

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

[Overview of driving support system]
FIG. 1 shows a schematic configuration of a driving support system according to an embodiment. The driving support system includes an in-vehicle device 1 that moves with the vehicle and a server device 2 that distributes map information. In FIG. 1, only one set of the on-board unit 1 and the vehicle communicating with the server device 2 is displayed, but in reality, there are a plurality of sets of the on-board unit 1 and the vehicle at different positions.

The on-board unit 1 is electrically connected to an external sensor such as a lidar (Light Detection and Ringing or Laser Illuminated Detection And Ringing), and an internal sensor such as a gyro sensor or a vehicle speed sensor, and is based on these outputs. The position of the vehicle on which the on-board unit 1 is mounted (also referred to as “own vehicle position”) is estimated. Then, based on the estimation result of the position of the own vehicle, the vehicle-mounted device 1 performs automatic driving control of the vehicle so as to travel along the route to the set destination.

The vehicle-mounted device 1 stores a map database (DB: DataBase) 10 including voxel data. The voxel data is data in which position information and the like of a stationary structure are recorded for each region (also referred to as “voxel”) when the three-dimensional space is divided into a plurality of regions. The voxel data includes data representing the measured point cloud data of the stationary structures in each voxel by a normal distribution, and is used for scan matching using NDT (Normal Distributions Transfer) as described later. The in-vehicle device 1 performs scan matching based on NDT based on the point cloud data measured by the rider and the voxel data corresponding to the voxel to which the point cloud data belongs. Then, the vehicle-mounted device 1 performs automatic driving control and the like while correcting the estimation result of the position of the own vehicle based on the matching result. In the following description, the map used for NDT scan matching is also referred to as "ND map". Further, the point cloud data used to create the voxel data of the ND map is called "map creation point cloud data", and the point cloud data measured by the rider 30 at the time of scan matching is called "scan data". Distinguish between the two.

The server device 2 performs data communication with the on-board units 1 of a plurality of vehicles. The server device 2 stores a distribution map DB 20 that stores map information for distribution to the in-vehicle devices 1 of a plurality of vehicles, and the map information includes voxel data corresponding to each voxel.

[Configuration of in-vehicle device]
FIG. 2A shows a block diagram showing a functional configuration of the vehicle-mounted device 1. As shown in FIG. 2A, the vehicle-mounted device 1 mainly includes 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, the storage unit 12, the sensor unit 13, the input unit 14, the control unit 15, and the output unit 16 are connected to each other via a bus line.

The communication unit 11 receives the map information distributed from the server device 2 under the control of the control unit 15. Further, the communication unit 11 transmits the measurement data by the sensor unit 13 to the server device 2 when requested by the server device 2. Further, the communication unit 11 receives a signal for controlling the vehicle from the server device 2 and transmits a signal regarding the state of the vehicle to the server device 2.

The storage unit 12 stores a program executed by the control unit 15 and information necessary for the control unit 15 to execute a predetermined process. In addition, the storage unit 12 has a map DB 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 DB of the server device 2.

The sensor unit 13 includes a rider 30, a camera 31, a GPS receiver 32, a gyro sensor 33, and a speed sensor 34. The rider 30 discretely measures the distance to an object existing in the outside world by emitting a pulsed laser with respect to a predetermined angle range in the horizontal direction and the vertical direction, and a three-dimensional point indicating the position of the object. Generate scan data, which is group data. In this case, the rider 30 has an irradiation unit that irradiates the laser beam while changing the irradiation direction, a light receiving unit that receives the reflected light (scattered light) of the irradiated laser light, and scan data based on the light receiving signal output by the light receiving unit. It has an output unit that outputs. The scan data is generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the distance of the laser light specified based on the above-mentioned light receiving signal to the object in the irradiation direction.

The input unit 14 is a button, a touch panel, a remote controller, a voice input device, etc. for the user to operate, and accepts an input for designating a destination for route search, an input for designating on / off of automatic operation, and the like. , The generated input signal is supplied to the control unit 15. The output unit 16 is, for example, a display, a speaker, or the like that outputs under the control of the control unit 15.

The control unit 15 includes a CPU that executes a program and the like, and controls the entire vehicle-mounted device 1. For example, the control unit 15 estimates the position of the own vehicle by performing scan matching based on the NDT based on the scan data output from the rider 30 and the voxel data of the ND map corresponding to the voxel to which the scan data data belongs. I do. Further, the control unit 15 downloads voxel data of the ND map from the server device 2 as needed. Further, when the server device 2 requests, the control unit 15 transmits the measurement data by the sensor unit 30, for example, the scan data by the rider 30, to the server device 2.

In the above configuration, the vehicle-mounted device 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 the measurement data generation means of the present invention, and the rider 30 is the measurement unit of the present invention. This is an example.

[Server device configuration]
FIG. 2B shows a schematic configuration of the server device 2. As shown in FIG. 2B, the server device 2 includes 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 connected to each other via a bus line.

The communication unit 21 communicates various data with the vehicle-mounted device 1 based on the control of the control unit 25. The storage unit 22 stores a program for controlling the operation of the server device 2 and holds information necessary for the operation of the server device 2. In addition, the storage unit 22 stores the distribution map DB 20 including the voxel data of the ND map.

The control unit 25 includes a CPU, ROM, RAM, and the like (not shown), and performs various controls on each component in the server device 2. For example, the control unit 25 transmits the voxel data included in the distribution map 20 to the vehicle-mounted device 1 in response to a request from the vehicle-mounted device 1.

[Basic scan matching]
Next, scan matching based on NDT in this embodiment will be described.
(1) Voxel data First, voxel data used for scan matching based on NDT will be described. FIG. 3A shows an example of the data structure of voxel data of the ND map.

The voxel data is generated from the point cloud data for cartography, and includes parameter information when the point cloud in the voxel is represented by a normal distribution. In this embodiment, as shown in FIG. 3A, the voxel data includes "voxel ID", "voxel coordinates", "mean vector", "covariance matrix", and "point cloud number information". And include. The voxel is an example of the three-dimensional virtual area of the present invention, and the voxel data is an example of the first point cloud feature data of the present invention.

"Voxel coordinates" indicate the absolute three-dimensional coordinates of a reference position such as the center position of each voxel. Since each voxel is a cube in which the space is divided in a grid pattern and the shape and size are predetermined, it is possible to specify the space of each voxel by the voxel coordinates. The voxel coordinates may be used as the voxel ID.

The "mean vector" and "covariance matrix" indicate the mean vector and covariance matrix corresponding to the parameters when the point cloud in the target voxel is represented by a normal distribution. Any arbitrary coordinate of the point "i" in the voxel and X _{M (i),} when the point number set in the voxel and "M", the average vector in voxel data X _{M (i)} "μ" and The covariance matrix "V" is represented by the following equations (1) to (3).

The "point cloud number information" is information indicating the number of point clouds used in the calculation of the corresponding average vector and the covariance matrix. The point cloud number information may be information indicating a specific number of point clouds, or may be information indicating the level of the point cloud number (for example, large, medium, small, etc.).

FIG. 4A shows an example of voxel data. The voxel data includes voxel coordinates, an average vector, and a covariance matrix for each voxel defined by the voxel ID. Voxel coordinates include x-direction, y-direction, the coordinate _M x in the _z-direction, M y, and _{M z.} The average vector includes the average vectors μ _x , μ _y , and μ _z in the x, y, and z directions. The covariance matrix includes each element constituting the matrix represented by the equation (3).

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

Now, assuming that the number of point clouds in the voxels is "N", _{the average value L of the scan data XL} (j) in each voxel is given by the following equation.

(3) Scan Matching Next, scan matching by NDT using voxel data of an ND map will be described. Now, as the estimation parameters, the x-direction movement vector t _x , the y-direction movement vector _ty , the z-direction movement vector t _z , and the rotation angle (that is, the yaw angle) t _Ψ in the directional direction (xy plane) are used. The pitch angle and roll angle are caused by the road gradient and vibration, and shall be small enough to be ignored. When the coordinates of an arbitrary point in the scan data measured by the rider 30 are subjected to coordinate transformation using the above estimation parameters, the coordinates after the coordinate transformation are obtained by the following equation (5).

The in-vehicle device 1 uses the coordinates of the point group after the coordinate conversion, the average vector μ included in the voxel data, and the covariance matrix V, and uses the voxel evaluation function “ E (k) ”and a comprehensive evaluation function“ E ”(hereinafter, also referred to as“ comprehensive evaluation function ”) for all voxels to be matched represented by the equation (7) are calculated. ..

The coordinates of the point cloud data obtained by the rider 30 and the coordinates of the average vector of the voxel data need to be unified into a coordinate system having a preset reference position as the origin. Therefore, prior to matching, the coordinates of both are transformed as necessary.

Then, the in-vehicle device 1 corrects the estimated own vehicle position by using the estimation parameters when the comprehensive evaluation function E is maximized, that is, the movement vectors t _x , _ty , t _z, and the rotation angle t _Ψ.

FIG. 5 conceptually shows the relationship between the voxel data of the ND map and the scan data obtained by the rider 30. For convenience of explanation, FIG. 5 is a two-dimensional display. FIG. 5A shows voxel data of four voxels B1 to B4. In each voxel, the circle Dm is the map creation point cloud data measured by the rider when traveling in the measurement and maintenance vehicle for map creation. Based on these map-making point cloud data, the two-dimensional normal distribution created by the equations (1) and (2) is shown by gradation. The mean and variance of the normal distribution shown in FIG. 5 (A) correspond to the mean vector and covariance matrix in the voxel data, respectively.

FIG. 5B shows the point cloud Ds of the scan data measured by the rider superimposed on FIG. 5A. Further, FIG. 5C is an superimposed display of the average value L of the scan data calculated from the point cloud Ds of the scan data displayed in FIG. 5B.

[Scan matching using reflection intensity]
Next, scan matching using the reflection intensity, which is a feature of this embodiment, will be described. The basic scan matching described above matches when there are few peripheral structures or when similar structures are continuous, because the difference between the case where the map information and the measurement data match and the case where they do not match becomes small. It becomes difficult. Therefore, in this embodiment, scan matching is performed using the reflection intensity of the point cloud data measured by the rider.

(First Example)
In the first embodiment, the average value of the reflection intensity is included in the voxel data of the ND map, and the scan matching is performed using the average value and the average value of the reflection intensity of the scan data measured by the rider 30. FIG. 3B shows an example of the data structure of the voxel data of the ND map according to the first embodiment, and FIG. 6A shows an example of the voxel data according to the first embodiment. Further, FIG. 6B shows an example of scan data according to the first embodiment. Figure 4 (A), 4 (B ) as can be seen when compared to, the voxel data includes average value I _M of the reflection intensity of the point group included in each voxel. Also in the scan data side measured by the rider 30, the average value I _L of the reflection intensity of the scan data in the voxel is calculated.

Vehicle device 1, the evaluation function of the scan matching, using the reflection intensity average value I _L of the reflection intensity average value I _M and the scan data of the voxel data as a weighted value. Specifically, the vehicle-mounted device 1 uses the evaluation function E (k) of a certain voxel k as the equation (8), and obtains the comprehensive evaluation function E of all the voxels by the equation (9).

The evaluation function of equation (8), the product of the reflection intensity average value I _L of the average value I _M and the scan data reflection intensity of the voxel data "I _M · I _L" is included as a weighting value. By using this evaluation function, there is a difference between the evaluation function value of a voxel containing an object with high reflection intensity (for example, a road sign or a white line) and the evaluation function value of a voxel containing no object with high reflection intensity. This will be described with a simple example.

Figure 7 is a diagram showing an example of calculating the value of the "I _M · I _L" as used in equation (8). Now, it is assumed that the vehicle travels in the direction of the arrow and there is an object such as a wall on the left side of the vehicle with a relatively uniform reflection intensity. Portion of the wall corresponds to the voxel _B M1 _{.about.B M6,} a reflection intensity average value _{I M} of those voxels _{B M4} is "1.0", the other voxel _B M1 _{~B M3, B} M5 _~B reflection intensity average value _{I M} is all _M6 assumed to be "0.1". That is, _{only voxel B M4} has higher reflection intensity than other voxels B _{M1 to} B _M3 and B _{M5 to} B _M6.

Now, it is assumed that the vehicle passes in front of this wall, scans the wall from the side with a rider to generate scan data, and obtains _{scan data B La to} B _{Le for each voxel as shown in the figure.} .. In the obtained scan data, a reflection intensity average value _{I L} voxel _{B Ld} is "1.0", and the other voxels _B La _{.about.B Lc,} reflection intensity _{I L} of _{B Le} is "0.1" Suppose that For the sake of simplicity, it is assumed that the voxel data and the scan data can be matched only by moving in the traveling direction of the vehicle.

In this case, as can be seen from FIG. 7, it is correct to determine that the _{voxels B La to} B _{Le of the scan data correspond to the voxels B M1 to} B _M5 of the voxel data, respectively. This, to consider what will happen when it is determined by using the evaluation function of the equation (8) to use the value of "I _M · I _L".

First, assuming that the voxels B _{La to} B _Le of the scan data correspond to the voxels B _{M1 to} B _M5 of the voxel data, respectively (hereinafter, referred to as "first correspondence"), "I" at that time. _M · _{I L} value of "is,
_IM・_IL = 0.1 × 0.1 + 0.1 × 0.1 + 0.1 × 0.1
+1.0 x 1.0 + 0.1 x 0.1 = 1.04
Will be.

On the other hand, assuming that the voxels B _{La to} B _Le of the scan data correspond to the voxels B _{M2 to} B _M6 of the voxel data, respectively (hereinafter, referred to as "second correspondence"), "I" at that time. _M · _{I L} value of "is,
_IM・_IL = 0.1 × 0.1 + 0.1 × 0.1 + 1.0 × 0.1
+0.1 x 1.0 + 0.1 x 0.1 = 0.23
Will be. The value of the evaluation function in Eq. (8) becomes _{larger in the one with the larger value of "IM} / _IL ", that is, in the first correspondence, which is the correct correspondence as shown in FIG. It can be seen that it is.

Incidentally, in FIG. 7, the reflection intensity of the portion of voxels B _M4 wall is the same as the ambient, when the reflection intensity average value I _M voxel B _M4 voxel data were similar to the surrounding "0.1" , the reflection intensity average value _{I L} voxel _{B Ld} scan data obtained by the rider is also "0.1". In this case, in both the first correspondence and the second correspondence, the value of "_IM・_{IL" is
IM}・_IL = 0.1 × 0.1 + 0.1 × 0.1 + 0.1 × 0.1
+0.1 x 0.1 + 0.1 x 0.1 = 0.05
It is difficult to determine which of the first correspondence and the second correspondence is correct.

As described above, in the first embodiment, even if there are few peripheral structures or similar structures are continuous, the average value of the reflection intensity of the voxel data and the scan data is used in the scan matching, and those are used. By including the product as a weighted value in the evaluation function, it is possible to improve the matching accuracy of voxels containing objects with high reflection intensity such as road signs and white lines. As a result, it is possible to improve the self-position estimation accuracy of the vehicle.

FIG. 8 is a flowchart of the own vehicle position estimation process according to the first embodiment. This process is performed by the control unit 15 of the vehicle-mounted device 1 executing a program prepared in advance. The vehicle-mounted device 1 repeatedly executes this process.

First, the vehicle-mounted device 1 determines whether or not there is an estimated own vehicle position one time ago (step S10). If there is an estimated own vehicle position one time ago (step S10: Yes), the vehicle-mounted device 1 sets it as the initial value of the own vehicle position (step S11). On the other hand, when there is no estimated vehicle position one hour before (step S10: No), the vehicle-mounted device 1 sets an initial value of the vehicle position based on the output of the GPS receiver 32 or the like (step S12). Next, the vehicle-mounted device 1 acquires the vehicle body speed from the speed sensor 34 and the angular velocity in the yaw direction from the gyro sensor 33 (step S13). Then, the vehicle-mounted device 1 calculates the moving distance of the vehicle and the change in the orientation of the vehicle based on the acquisition result in step S13 (step S14).

Next, the in-vehicle device 1 adds the movement distance and the directional change calculated in step S14 to the initial value of the own vehicle position to calculate the predicted own vehicle position (step S15). Then, the in-vehicle device 1 refers to the map DB 10 based on the predicted vehicle position calculated in step S15, and acquires voxel data of voxels existing around the vehicle position (step S16). The voxel data includes reflection intensity average value I _M, as shown in FIG. 6 (A). Further, the vehicle-mounted device 1 divides the scan data obtained from the rider 30 for each voxel based on the predicted own vehicle position calculated in step S15 (step S17). At this time, as shown in FIG. 6 (B), the reflection intensity average value I _L of the scan data in the voxel is calculated.

Next, the vehicle-mounted device 1 calculates the NDT scan matching using the evaluation function. _{Specifically, the in-vehicle device 1 calculates the evaluation function Ek} and the comprehensive evaluation function E based on the equations (8) and (9), and the estimated parameter that maximizes the comprehensive evaluation function E, that is, the movement vector. _{t x,} t y, and calculates the tz and rotation angle t _[psi (step S18). Then, the vehicle-mounted device 1 corrects the estimated own vehicle position based on _{the obtained movement vectors t x} , _ty , t _z, and the rotation angle t _{Ψ (step S19).} In this way, the own vehicle position estimation process is completed.

(Second Example)
In the second embodiment, the reflection intensities of the voxel data and the scan data are used as in the first embodiment, but the reflection intensities of each point of the point cloud in the voxel are used to set parameters such as the mean vector and the covariance matrix. It differs from the first embodiment in that it is calculated.

Specifically, first, when generating voxel data of an ND map from the point cloud data for map creation, the average vector is calculated by the following equation (10) using _{the reflection intensity IM (i) of each point.} Then, the covariance matrix is calculated by the equation (11).

なお、ＮＤ地図のボクセルデータは、図３（Ａ）に示すデータ構造を有し、その内容は図４（Ａ）に示すものとなる。

The voxel data of the ND map has the data structure shown in FIG. 3 (A), and the contents thereof are as shown in FIG. 4 (A).

Also, for the scan data by the rider, the average value vector L is calculated by the following equation (12) using _{the reflection intensity IL (j) of each point in each voxel.}

そして、車載機１は、式（６）の評価関数Ｅ（ｋ）及び式（７）の総合評価関数Ｅを用いてスキャンマッチングを行う。

Then, the vehicle-mounted device 1 performs scan matching using the evaluation function E (k) of the equation (6) and the comprehensive evaluation function E of the equation (7).

The voxel data and scan data of the ND map obtained by the formulas (10) to (12) are expressions in which the characteristic of the reflection intensity is added to the three-dimensional position information. For example, in the case of a substantially flat road surface, the position of the point cloud is uniform on the xy plane, so that the average and variance in the x and y directions are constant. However, if there is a part with high reflection intensity such as a white line or a pedestrian crossing, the mean vector and covariance matrix in the x and y directions are characterized by this.

FIG. 9 shows a two-dimensional normal distribution of voxel data of a certain ND map. FIG. 9A shows a two-dimensional normal distribution 51 calculated without using the reflection intensity. Since the reflection intensity of the plurality of points Dm is not taken into consideration, the normal distribution 51 is obtained based on the position of each point Dm. On the other hand, FIG. 9B shows a two-dimensional normal distribution 52 calculated by using the reflection intensity of each point Dm according to the second embodiment. In this example, since the plurality of points Dm include the points Dmx having high reflection intensity, the normal distribution 52 becomes a distribution biased in the direction of the point Dmx more than the normal distribution 51, and the feature of the portion having high reflection intensity is characterized. It will be reflected.

As described above, 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 reflection intensity of each point is also used for the scan data by the rider 30. Calculate the average value vector. Therefore, even in an environment where there is little change in shape around the road, if there is a change in the reflection intensity, parameters such as the average vector and covariance matrix can be characterized by the reflection intensity, and at the time of scan matching. The difference between a match and a mismatch is large. Therefore, the accuracy of scan matching is improved, and as a result, the accuracy of own vehicle position estimation can be improved.

The own vehicle position estimation process in the second embodiment is basically the same as that in the first embodiment shown in FIG. However, the voxel data acquired from the map DB 10 in step S16 has the data structure and contents shown in FIG. 4 (A), and the average vector μ and the covariance matrix V are the above-mentioned equations (10) and (11), respectively. It is calculated by. Further, in step S17, the vehicle-mounted device 1 calculates the average value vector L using _{the reflection intensity IL (j) of each point in each voxel.} Then, in step S18, scan matching is performed using the evaluation functions of the equations (6) and (7).

(Modification example)
The above-mentioned first embodiment and the second embodiment may be combined and carried out. In that case, the voxel data of the ND map, including the average value is reflected intensity average value I _M of the reflection intensity of each point in the voxel, as shown in FIG. 6 (A). _{Further, as shown in FIG. 6B, the average reflection intensity IL} is calculated based on the reflection intensity of each point for the scan data by the rider. In addition to this, the average vector μ and the covariance matrix V included in the voxel data are _{calculated using the reflection intensity IM} (i) at each point as shown by the above equations (10) and (11), respectively. It has become a thing. Further, the average vector L of the scan data by the rider is also calculated by using _{the reflection intensity IL (j) of each point in the voxel as shown in the above equation (12).} Then, using these voxel data and scan data, scan matching is performed so that the comprehensive evaluation function E is maximized by the equations (8) and (9).

1 On-board unit 2 Server device 10 Map DB
20 Distribution map DB
11, 21 Communication unit 12, 22 Storage unit 15, 25 Control unit 13 Sensor unit 14 Input unit 16 Output unit

Claims (6)

It is a self-position estimation device that estimates the self-position of a moving body.
It is composed of three-dimensional virtual area units of a predetermined size, and includes first point cloud feature data showing the features of the first point cloud corresponding to the reflected light of the light applied to the object existing in the three-dimensional virtual area. Map data acquisition means to acquire map data and
The second point cloud showing the characteristics of the second point cloud obtained by measuring with the measuring unit that receives the reflected light of the light radiated to the object existing in the three-dimensional virtual area for each of the three-dimensional virtual areas. Measurement data generation means for generating measurement data including feature data,
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 areas, and
With
The first point group, wherein the data is data reflecting the intensity of the first point group is reflected, the second point group feature data Ri Ah in data reflection intensity of the second point group is reflected,
The first point cloud feature data includes the average position and variance of the first point cloud calculated using the reflection intensity of each point of the first point cloud.
The second point cloud feature data is a self-position estimation device including the average position of the first point cloud calculated by using the reflection intensity of each point of the second point cloud. The first point cloud feature data for each of the three-dimensional virtual regions has a distribution that reflects the reflection intensity of each point in the first point cloud.
The self-position estimation device according to claim 1, wherein the second point cloud feature data for each three-dimensional virtual region has a distribution that reflects the reflection intensity of each point in the second point cloud. The distribution in the first point cloud feature data for each of the three-dimensional virtual regions is shaped according to the reflection intensity when the reflection intensity of at least one point of the first point group is high or there is a difference in the reflection intensity of each point. To change,
The distribution in the second point cloud feature data for each of the three-dimensional virtual regions is shaped according to the reflection intensity when the reflection intensity of at least one point of the second point group is high or the reflection intensity of each point is different. The self-position estimation device according to claim 2. A self-position estimation method performed by a self-position estimation device that estimates the self-position of a moving object.
It is composed of three-dimensional virtual area units of a predetermined size, and includes first point cloud feature data showing the features of the first point cloud corresponding to the reflected light of the light applied to the object existing in the three-dimensional virtual area. Map data acquisition process to acquire map data and
The second point cloud showing the characteristics of the second point cloud obtained by measuring with the measuring unit that receives the reflected light of the light radiated to the object existing in the three-dimensional virtual area for each of the three-dimensional virtual areas. A measurement data generation process that generates measurement data including feature data,
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 of the three-dimensional virtual areas, and
With
The first point group, wherein the data is data reflecting the intensity of the first point group is reflected, the second point group feature data Ri Ah in data reflection intensity of the second point group is reflected,
The first point cloud feature data includes the average position and variance of the first point cloud calculated using the reflection intensity of each point of the first point cloud.
The second point cloud feature data is a self-position estimation method including the average position of the first point cloud calculated by using the reflection intensity of each point of the second point cloud. A program that is equipped with a computer and is executed by a self-position estimation device that estimates the self-position of a moving object.
It is composed of three-dimensional virtual area units of a predetermined size, and includes first point cloud feature data showing the features of the first point cloud corresponding to the reflected light of the light applied to the object existing in the three-dimensional virtual area. Map data acquisition means to acquire map data,
The second point cloud showing the characteristics of the second point cloud obtained by measuring with the measuring unit that receives the reflected light of the light radiated to the object existing in the three-dimensional virtual area for each of the three-dimensional virtual areas. Measurement data generation means for generating measurement data including feature data,
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.
To make the computer function as
The first point group, wherein the data is data reflecting the intensity of the first point group is reflected, the second point group feature data Ri Ah in data reflection intensity of the second point group is reflected,
The first point cloud feature data includes the average position and variance of the first point cloud calculated using the reflection intensity of each point of the first point cloud.
The second point cloud feature data is a program including the average position of the first point cloud calculated by using the reflection intensity of each point of the second point cloud. A storage medium that stores the program according to claim 5.

Images