Information processing device, control method, program, and storage medium

Abstract

To provide an information processing device with which it is possible to suitably prevent a decrease in position estimation accuracy.SOLUTION: The controller 13 of an onboard device 1 acquires a DR position XDR that is the first candidate position of a vehicle. The controller 13 also acquires an NDT position XNDT that is the second candidate position of the vehicle determined on the basis of NDT scan matching for the verification of point cloud data outputted by a lidar 2, which is an external sensor provided to the vehicle, against voxel data VD which is map data. The controller 13 further calculates a reliability value NRV that represents the reliability of the NDT scan matching. Then the controller 13 determines an estimate position X^ of the vehicle on the basis of the DR position XDR, the NDT position XNDT, and the reliability NRV.SELECTED DRAWING: Figure 11

Description

[Technical field]

[0001] FIELD This disclosure relates to position estimation. [Background technology]

[0002] Conventionally, a technology is known for estimating the vehicle's own position by collating (matching) shape data of surrounding objects measured by a measuring device such as a laser scanner with map information in which the shapes of surrounding objects are stored in advance. For example, Patent Document 1 discloses an autonomous mobile system that determines whether a detected object in a voxel obtained by dividing a space according to a predetermined rule is a stationary object or a moving object, and matches map information with measurement data for voxels in which stationary objects exist. In addition, Patent Document 2 discloses a scan matching method that estimates the vehicle's own position by collating voxel data including the mean vector and covariance matrix of stationary objects for each voxel with point cloud data output by a lidar. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] International Publication WO2013 / 076829 [Patent Document 2] International Publication WO2018 / 221453 Summary of the Invention [Problem to be solved by the invention]

[0004] As in Patent Document 2, when estimating the vehicle position by matching voxel data with point cloud data output by a lidar, the search for a solution is limited in number of times in order to perform the search within a limited processing time, but in this case, the solution obtained may be a local solution and not an optimal solution. In particular, when performing measurements in a space with sparse surroundings, a space with little characteristic change, or a space where occlusion occurs due to obstacles such as other vehicles, the possibility of falling into a local solution increases. If a position based on a local solution is output as an estimation result, the accuracy of the position estimation will decrease.

[0005] The present disclosure has been made to solve the above-mentioned problems, and has as its main object to provide an information processing device that can preferably prevent a decrease in position estimation accuracy. [Means for solving the problem]

[0006] The claimed invention is a first candidate position acquisition means for acquiring a first candidate position of the moving object; a second candidate position acquisition means for acquiring a second candidate position of the moving body determined based on a comparison between data based on an output of an external sensor provided in the moving body and map data; A reliability value calculation means for calculating a reliability value representing the reliability of the matching; an estimated position determination means for determining an estimated position of the moving object based on the first candidate position, the second candidate position, and the reliability value; having the data is point cloud data, The map data includes voxel data representing a position of an object for each voxel, which is a unit area; The method further includes: associating means for associating measurement points constituting the point cloud data with each of the voxels; The reliability value calculation means is an information processing device that calculates the reliability value based at least on a correspondence ratio, which is the ratio of the number of measurement points for which the correspondence has been performed to the number of measurement points that constitute the point cloud data.

[0007] The claimed invention also includes: A computer-implemented control method, comprising: Obtain a first candidate position of the moving object; acquiring a second candidate position of the moving body determined based on a comparison between data based on an output of an external sensor provided in the moving body and map data; calculating a confidence value representing the confidence of the matching; determining an estimated location of the moving object based on the first candidate location, the second candidate location, and the reliability value; the data is point cloud data, The map data includes voxel data representing a position of an object for each voxel, which is a unit area; Corresponding measurement points constituting the point cloud data with each of the voxels; calculating the reliability value based at least on a correspondence ratio that is a ratio of the number of the measurement points for which the correspondence has been performed to the number of measurement points constituting the point cloud data; It is a control method.

[0008] The claimed invention also includes: Obtain a first candidate position of the moving object; acquiring a second candidate position of the moving body determined based on a comparison between data based on an output of an external sensor provided in the moving body and map data; calculating a confidence value representing the confidence of the matching; causing a computer to execute a process of determining an estimated position of the moving object based on the first candidate position, the second candidate position, and the reliability value; the data is point cloud data, The map data includes voxel data representing a position of an object for each voxel, which is a unit area; Correspondence between the measurement points constituting the point cloud data and each of the voxels is performed. stomach , before The program calculates the reliability value based at least on a correspondence ratio, which is a ratio of the number of measurement points for which the correspondence has been established to the number of measurement points constituting the point cloud data. [Brief description of the drawings]

[0009] [Figure 1] FIG. 1 is a schematic configuration diagram of a driving assistance system. [Diagram 2] FIG. 2 is a block diagram showing a functional configuration of an in-vehicle device. [Diagram 3] 4 shows an example of a functional block of a downsampling processing unit. [Figure 4]2 is a diagram showing a vehicle position to be estimated by a vehicle position estimation unit in two-dimensional orthogonal coordinates. [Diagram 5] 2 shows an example of a schematic data structure of voxel data. [Figure 6] 4 is an example of a functional block of a vehicle position estimation unit. [Figure 7] This shows the positional relationship between voxels in which voxel data exists and measurement points indicating positions near these voxels on a two-dimensional plane in the world coordinate system. [Figure 8] 1 is a graph showing the characteristics of each of the calculation formulas for the NDT reliability index. [Figure 9] An overhead view of the vehicle and its surroundings on which the onboard device is mounted is shown. [Figure 10] 1 is a graph showing the error distribution of dead reckoning and the error distribution of NDT scan matching. [Figure 11] 4 is an example of a flowchart showing a procedure of a process relating to vehicle position estimation. [Figure 12] 11 shows a result of self-location estimation by a first lidar in a comparative example. [Figure 13] 11 shows a result of self-location estimation by a first lidar in a comparative example. [Figure 14] 13 shows a result of self-localization estimation by a first lidar according to the embodiment. [Figure 15] 13 shows a result of self-localization estimation by a first lidar according to the embodiment. [Figure 16] 13 shows a self-position estimation result in a comparative example using a second lidar. [Figure 17] 13 shows a self-position estimation result in a comparative example using a second lidar. [Figure 18] 13 shows a result of self-localization estimation in the embodiment using a second lidar. [Figure 19] 13 shows a result of self-localization estimation in the embodiment using a second lidar. [Figure 20] 13 is a graph showing the relationship between the reliability value and the NDT reliability index in the modified example. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] According to a preferred embodiment of the present invention, an information processing device includes a first candidate position acquisition means for acquiring a first candidate position of a moving object, a second candidate position acquisition means for acquiring a second candidate position of the moving object determined based on matching data based on an output of an external sensor provided on the moving object with map data, a reliability value calculation means for calculating a reliability value representing the reliability of the matching, and an estimated position determination means for determining an estimated position of the moving object based on the first candidate position, the second candidate position, and the reliability value. According to this aspect, the information processing device can suitably determine an estimated position taking into account the first candidate position and the second candidate position according to the reliability of the matching when the second candidate position is calculated, and suitably prevent a decrease in the accuracy of position estimation.

[0011] In one aspect of the information processing device, the reliability value calculation means determines the reliability value based on at least a score value indicating a degree of suitability of the matching. With this aspect, the information processing device can accurately determine the reliability of the matching for determining the second candidate position.

[0012] In another aspect of the information processing device, the data is second point cloud data that is point cloud data that is downsampled from first point cloud data that is point cloud data output by the external sensor, and the reliability value calculation means calculates the reliability value based on at least the downsampling size or the number of measurement points of the first point cloud data. Even with this aspect, the information processing device can accurately determine the reliability of matching to determine the second candidate position.

[0013] In another aspect of the information processing device, the data is point cloud data, the map data is voxel data representing the position of an object for each voxel which is a unit area, and the second candidate position acquisition means associates measurement points constituting the point cloud data with each of the voxels, and determines the second candidate position based on a comparison between the voxel data of the voxels for which the association has been made and the measurement points associated with the voxels. With this aspect, the information processing device can suitably determine the second candidate position based on a comparison between the point cloud data and the voxel data.

[0014] In another aspect of the information processing device, the reliability value calculation means calculates the reliability value based on at least a correspondence ratio, which is a ratio of the number of the measurement points that have been associated with each other. This aspect also allows the information processing device to accurately determine the reliability of the matching for determining the second candidate position.

[0015] In another aspect of the information processing device, the reliability value calculation means calculates, as the reliability value, a value that is proportional to or positively correlated with the size of downsampling performed on the first point cloud data, which is the point cloud data output by the external sensor, or the number of measurement points of the first point cloud data, and a score value indicating the degree of suitability of the matching. With this aspect, the information processing device can accurately determine the reliability of the matching for determining the second candidate position.

[0016] In another aspect of the information processing device, the estimated position determination means determines the estimated position by a weighted average of the first candidate position and the second candidate position, the weighting being performed based on the reliability value. With this aspect, the information processing device can calculate an estimated position by appropriately fusing the first candidate position and the second candidate position.

[0017] In another aspect of the information processing device, the estimated position determination means calculates, as a weight for the second candidate position, a reliability index obtained by normalizing the reliability value to a value between 0 and 1. With this aspect, the information processing device can accurately set a weight to be used for a weighted average.

[0018] In another aspect of the information processing device, the first candidate position acquisition means acquires the position of the moving object determined by dead reckoning as the first candidate position. With this aspect, the information processing device can suitably acquire the first candidate position that does not depend on an external sensor.

[0019] According to another preferred embodiment of the present invention, there is provided a control method executed by a computer, which includes acquiring a first candidate position of a moving object, acquiring a second candidate position of the moving object determined based on a comparison between data based on an output of an external sensor provided on the moving object and map data, calculating a reliability value representing a reliability of the comparison, and determining an estimated position of the moving object based on the first candidate position, the second candidate position, and the reliability value. By executing this control method, the computer can preferably prevent a decrease in accuracy of position estimation.

[0020] According to yet another preferred embodiment of the present invention, a program causes a computer to execute a process of acquiring a first candidate position of a moving object, acquiring a second candidate position of the moving object determined based on matching data based on an output of an external sensor provided on the moving object with map data, calculating a reliability value representing the reliability of the matching, and determining an estimated position of the moving object based on the first candidate position, the second candidate position, and the reliability value. By executing the program, the computer can preferably prevent a decrease in accuracy of position estimation. Preferably, the program is stored in a storage medium. EXAMPLES

[0021] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. In this specification, for the sake of convenience, any symbol followed by "^" or "-" is referred to as "A^" or "A -” (where “A” is any letter).

[0022] (1) Overview of the driving assistance system 1 is a schematic configuration of a driving assistance system according to the present embodiment. The driving assistance system includes an on-board device 1 that moves together with a vehicle, which is a moving body, a Lidar (Light Detection and Ranging, or Laser Illuminated Detection and Ranging) 2, a gyro sensor 3, a vehicle speed sensor 4, and a GPS receiver 5.

[0023] The vehicle-mounted device 1 is electrically connected to the lidar 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and estimates the position of the vehicle in which the vehicle-mounted device 1 is installed (also referred to as "own vehicle position") based on the outputs of these sensors. Then, based on the estimation result of the own vehicle position, the vehicle-mounted device 1 performs automatic driving control of the vehicle so that the vehicle travels along a route to a set destination. The vehicle-mounted device 1 stores a map database (DB: DataBase) 10 including voxel data "VD". The voxel data VD is data that records position information of a stationary structure for each voxel that indicates a cube (regular lattice) that is the smallest unit of a three-dimensional space. The voxel data VD includes data that expresses measured point cloud data of a stationary structure in each voxel using a normal distribution, and is used for scan matching using NDT (Normal Distributions Transform) as described later. The vehicle-mounted device 1 estimates at least the vehicle's position on a plane and yaw angle by NDT scan matching. The vehicle-mounted device 1 may further estimate the vehicle height position, pitch angle, and roll angle. Unless otherwise specified, the vehicle position is assumed to include the vehicle's attitude angle, such as the yaw angle, to be estimated.

[0024] The lidar 2 measures the distance to an object in the outside world discretely by emitting a pulsed laser in a predetermined angular range in the horizontal and vertical directions, and generates three-dimensional point cloud data indicating the position of the object. In this case, the lidar 2 has an irradiation unit that irradiates a laser beam while changing the irradiation direction, a light receiving unit that receives the reflected light (scattered light) of the irradiated laser beam, and an output unit that outputs scan data (points that constitute the point cloud data, hereinafter referred to as "measurement points") based on the light receiving signal output by the light receiving unit. The measurement points are generated based on the irradiation direction corresponding to the laser beam received by the light receiving unit and the response delay time of the laser beam specified based on the above-mentioned light receiving signal. In general, the closer the distance to the object, the higher the accuracy of the distance measurement value of the lidar, and the farther the distance, the lower the accuracy. The lidar 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5 each supply output data to the vehicle-mounted device 1.

[0025] The vehicle-mounted device 1 is an example of an "information processing device" in the present invention, and the lidar 2 is an example of an "external sensor" or a "measurement device" in the present invention. Note that the driving assistance system may have an inertial measurement unit (IMU) instead of or in addition to the gyro sensor 3, which measures the acceleration and angular velocity of the measurement vehicle in three axial directions.

[0026] (2) Vehicle-mounted device configuration 2 is a block diagram showing an example of a hardware configuration of the vehicle-mounted device 1. The vehicle-mounted device 1 mainly includes an interface 11, a memory 12, and a controller 13. These elements are connected to each other via a bus line.

[0027] The interface 11 performs an interface operation related to the transmission and reception of data between the in-vehicle device 1 and an external device. In this embodiment, the interface 11 acquires output data from sensors such as the lidar 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and supplies the data to the controller 13. The interface 11 also supplies a signal related to the vehicle driving control generated by the controller 13 to an electronic control unit (ECU) of the vehicle. The interface 11 may be a wireless interface such as a network adapter for wireless communication, or may be a hardware interface for connecting to an external device via a cable or the like. The interface 11 may perform an interface operation with various peripheral devices such as an input device, a display device, and a sound output device.

[0028] The memory 12 is composed of various types of volatile and non-volatile memories such as a RAM (Random Access Memory), a ROM (Read Only Memory), a hard disk drive, a flash memory, etc. The memory 12 stores a program for the controller 13 to execute a predetermined process. The program executed by the controller 13 may be stored in a storage medium other than the memory 12.

[0029] The memory 12 also stores a map DB 10 including voxel data VD.

[0030] The map DB 10 may be stored in a storage device external to the vehicle-mounted device 1, such as a hard disk connected to the vehicle-mounted device 1 via the interface 11. The storage device may be a server device that communicates with the vehicle-mounted device 1. The storage device may be composed of a plurality of devices. The map DB 10 may be updated periodically. In this case, for example, the controller 13 receives partial map information related to the area to which the vehicle position belongs from a server device that manages map information via the interface 11, and reflects the partial map information in the map DB 10.

[0031] The memory 12 also stores information necessary for the processing executed by the vehicle-mounted device 1 in this embodiment, in addition to the map DB 10. For example, the memory 12 stores information used to set a downsampling size when downsampling is performed on point cloud data obtained when the LIDAR 2 performs one scanning cycle. The memory 12 also stores information on a calculation formula used for normalization when calculating the reliability of NDT scan matching.

[0032] The controller 13 includes one or more processors such as a CPU (Central Processing Unit), a GPU (Graphics Processing Unit), and a TPU (Tensor Processing Unit), and controls the entire vehicle-mounted device 1. In this case, the controller 13 executes a program stored in the memory 12 or the like to perform processing related to vehicle position estimation.

[0033] Moreover, functionally, the controller 13 has a downsampling processing unit 14 and a vehicle position estimation unit 15. The controller 13 functions as a "first candidate position acquisition means", a "second candidate position acquisition means", a "reliability value calculation means", an "estimated position determination means", a computer that executes a program, and the like.

[0034] The downsampling processing unit 14 performs downsampling on the point cloud data output from the LIDAR 2 to generate point cloud data (also referred to as "processed point cloud data") that has been corrected to reduce the number of measurement points. In this downsampling, the downsampling processing unit 14 averages the point cloud data within a space divided into a predetermined size to generate processed point cloud data with a reduced number of measurement points. Hereinafter, the downsampling size (corresponding to the above-mentioned predetermined size) will be referred to as the "downsampling size DSS." Furthermore, in NDT matching at each processing time, the downsampling processing unit 14 adaptively sets the downsampling size DSS so that the number of measurement points (also referred to as the "number of corresponding measurement points Nc") associated with the voxel data VD in the processed point cloud data after downsampling falls within a predetermined target range. Hereinafter, the target range of the number of corresponding measurement points Nc will be referred to as the "target range R Nc " is also called.

[0035] The vehicle position estimation unit 15 estimates the vehicle position by performing NDT-based scan matching (NDT scan matching) based on the processed point cloud data generated by the downsampling processing unit 14 and the voxel data VD corresponding to the voxel to which the point cloud data belongs.

[0036] (3) Setting the downsampling size Next, a detailed description will be given of the process of the downsampling processing unit 14. In summary, the downsampling processing unit 14 calculates the number of corresponding measurement points Nc and the target range R indicated by the target range information 9 at each processing time. Nc After downsampling, the downsampling processing unit 14 compares the number of corresponding measurement points Nc with the target range R Nc The downsampling size is adaptively determined so as to stay within the range.

[0037] Fig. 3 shows an example of the functional blocks of the downsampling processing unit 14. Functionally, the downsampling processing unit 14 has a downsampling block 16, a corresponding measurement point number acquisition block 17, a comparison block 18, and a downsampling size setting block 19. Note that in Fig. 3, the blocks through which data is exchanged are connected by solid lines, but the combination of blocks through which data is exchanged is not limited to that shown in Fig. 3. The same applies to the diagrams of other functional blocks described later.

[0038] The downsampling block 16 generates processed point cloud data for each processing time by downsampling the point cloud data of one cycle of scanning generated by the LIDAR 2 for each processing time. In this case, the downsampling block 16 divides the space into grids based on the set downsampling size, and calculates representative points of the measurement points of the point cloud data of the LIDAR 2 included in each grid. In this case, the above space is, for example, a space of a three-dimensional coordinate system with the travel direction, height direction, and lateral direction (i.e., directions perpendicular to the travel direction and height direction) of the vehicle-mounted device 1 as axes, and the grid is formed by dividing the space at equal intervals along each of these directions. The downsampling size is the initial size stored in the memory 12 or the like, or the size set immediately before by the downsampling size setting block 19. The downsampling block 16 supplies the generated processed point cloud data to the vehicle position estimation unit 15.

[0039] The downsampling size may be different for each of the travel direction, height direction, and lateral direction of the vehicle-mounted device 1. Furthermore, the processing of the downsampling block 16 may use any Voxel grid filter method.

[0040] The corresponding measurement point number acquisition block 17 acquires the number of corresponding measurement points Nc from the vehicle position estimation unit 15. In this case, the vehicle position estimation unit 15 performs NDT matching for each processing time on the processed point cloud data supplied from the downsampling block 16, and outputs the number of corresponding measurement points Nc for each processing time to the corresponding measurement point number acquisition block 17. The corresponding measurement point number acquisition block 17 supplies the number of corresponding measurement points Nc to the comparison block 18.

[0041] The comparison block 18 is a block that compares the target range R indicated by the target range information 9 at each processing time. Nc and the number of corresponding measurement points Nc, and supplies the comparison result to the downsampling size setting block 19. In this case, the comparison block 18 performs, for example, The number of corresponding measurement points Nc is within the target range R Nc or The number of corresponding measurement points Nc is within the target range R Nc is within, or The number of corresponding measurement points Nc is within the target range R Nc is less than the lower limit of The result of the determination is supplied to the downsampling size setting block 19. For example, if the target range R Nc If the target range R is between 600 and 800, the comparison block 18 Nc Upper limit of 800, target range R Nc The above judgment is made with the lower limit of 600. Nc is not limited to 600 to 800, and is determined according to the hardware used, the performance of the processor, and the processing time to be completed (that is, the time interval to the next processing time).

[0042] The downsampling size setting block 19 sets the size of downsampling to be applied at the next processing time based on the comparison result of the comparison block 18. In this case, the downsampling size setting block 19 sets the size of downsampling to be applied at the next processing time based on the comparison result of the comparison block 18. Nc If the number of corresponding measurement points Nc is larger than the upper limit of the target range R, the downsampling size is increased. NcOn the other hand, the downsampling size setting block 19 determines whether the number of corresponding measurement points Nc falls within the target range R Nc If it is within the range, the downsampling size is maintained (i.e., not changed).

[0043] FIG. 3 also shows a specific embodiment of the downsampling size setting block 19 having a gain setting sub-block 191 and a multiplication sub-block 192.

[0044] The gain setting sub-block 191 sets a gain to be multiplied by the current downsampling size in the multiplication sub-block 192 based on the comparison result in the comparison block 18. In this case, the gain setting sub-block 191 sets a gain to be multiplied by the current downsampling size in the multiplication sub-block 192 based on the comparison result in the comparison block 18. Nc If the number of corresponding measurement points Nc is greater than the upper limit of the target range R, the gain is increased to more than 1. Nc If the number of corresponding measurement points Nc is less than the lower limit of the target range R Nc If the gain is within the range, the gain is set to 1. In FIG. 3, the gain is set as follows as an example. The number of corresponding measurement points Nc is within the target range R Nc More than the upper limit ⇒ Gain "1.1", The number of corresponding measurement points Nc is within the target range R Nc Less than the lower limit ⇒ Gain "1 / 1.1", The number of corresponding measurement points Nc is within the target range R Nc Within the range ⇒ Gain "1.0"

[0045] The multiplication sub-block 192 outputs a value obtained by multiplying the current downsampling size by the gain set by the gain setting sub-block 191 as the downsampling size to be used at the next processing time.

[0046] According to this configuration, the downsampling size setting block 19 determines whether the number of corresponding measurement points Nc falls within the target range R NcIf the number of corresponding measurement points Nc is greater than the upper limit of the target range R, the downsampling size can be increased to change the downsampling size so that the number of measurement points of the processing point cloud data is easily reduced. Similarly, the downsampling size setting block 19 determines whether the number of corresponding measurement points Nc is greater than the upper limit of the target range R. Nc If it is less than the lower limit, the downsampling size can be reduced and the downsampling size can be changed so that the number of measurement points of the processing point cloud data is likely to increase.

[0047] The numerical values ​​of the gains set in the gain setting sub-block 191 are merely examples, and are set to appropriate numerical values ​​based on experimental results, etc. Also, the downsampling size setting block 19 may add or subtract a predetermined value instead of multiplying the gains. In this case, the downsampling size setting block 19 performs a step of adding or subtracting a predetermined value when the number of corresponding measurement points Nc is within the target range R Nc If the number of corresponding measurement points Nc is greater than the upper limit of the target range R, the downsampling size is increased by a predetermined positive value. Nc If the difference is less than the lower limit, the downsampling size is reduced by a predetermined positive value. Note that the gain and the predetermined value may be set to different values ​​for each direction (travel direction, horizontal direction, height direction).

[0048] (4) Position Estimation Based on NDT Scan Matching Next, the position estimation based on NDT scan matching executed by the vehicle position estimating unit 15 will be described.

[0049] FIG. 4 is a diagram showing the vehicle position to be estimated by the vehicle position estimation unit 15 in two-dimensional orthogonal coordinates. As shown in FIG. 4, the vehicle position on a plane defined on the two-dimensional orthogonal coordinates of xy is represented by coordinates "(x, y)" and the vehicle's orientation (yaw angle) "ψ". Here, the yaw angle ψ is defined as the angle between the vehicle's traveling direction and the x-axis. The coordinates (x, y) are, for example, absolute positions corresponding to a combination of latitude and longitude, or world coordinates indicating a position with a predetermined point as the origin. The vehicle position estimation unit 15 performs vehicle position estimation using these x, y, and ψ as estimation parameters. Note that the vehicle position estimation unit 15 may perform vehicle position estimation in which at least one of the vehicle's height position, pitch angle, and roll angle in a three-dimensional orthogonal coordinate system is further estimated as an estimation parameter in addition to x, y, and ψ.

[0050] Next, the voxel data VD used in the NDT scan matching will be described. The voxel data VD includes data in which measured point cloud data of a stationary structure in each voxel is expressed by a normal distribution.

[0051] Fig. 5 shows an example of a schematic data structure of the voxel data VD. The voxel data VD includes parameter information when expressing a point group in a voxel by a normal distribution, and in this embodiment, as shown in Fig. 5, includes a voxel ID, a voxel coordinate, a mean vector, and a covariance matrix.

[0052] "Voxel coordinates" indicate 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 since its shape and size are determined in advance, it is possible to identify the space of each voxel by its voxel coordinates. The voxel coordinates may be used as a voxel ID.

[0053] The "mean vector" and "covariance matrix" refer to the mean vector and covariance matrix that correspond to the parameters when expressing the point group in the target voxel as a normal distribution. Note that the coordinates of an arbitrary point "i" in an arbitrary voxel "n" are X n (i)=[x n(i), y n (i), z n (i)] T and the number of points in voxel n is defined as "N n ", then the mean vector at voxel n is "μ n " and the covariance matrix "V n " are expressed by the following formulas (1) and (2), respectively.

[0054]

number

[0055]

number

[0056] Next, an overview of NDT scan matching using the voxel data VD will be described.

[0057] Scan matching using NDT assuming a vehicle estimates parameters based on the amount of movement within the road plane (here, xy coordinates) and the vehicle direction. P=[t x , t y , t ψ ] T Here, t x " indicates the amount of movement in the x direction, and "t y " indicates the amount of movement in the y direction, and "t ψ " indicates the yaw angle.

[0058] In addition, the coordinates of the point cloud data output by LIDAR 2 are X L (j) = [x(j), y(j), z(j)] T Then, X L The average value of (j) "L´ n " is expressed by the following equation (3).

[0059]

number

[0060]

number

[0061]

number

[0062] Then, the vehicle position estimation unit 15 calculates a comprehensive evaluation function value (also called a "score value") "E(k)" for all voxels to be matched, as shown in the following formula (6). The score value E is an index showing the compatibility of the matching.

[0063]

number

[0064]

number

[0065] (5) Calculation of estimated vehicle position Next, a method for calculating the estimated vehicle position X^(k) will be described. In summary, the vehicle position estimating unit 15 calculates a reliability value of the NDT scan matching at each processing time, and estimates the DR position X^(k) at the target processing time by weighting based on the reliability value. DR and NDT position X NDT The estimated vehicle position X^ at the target processing time is calculated from the above. Hereinafter, the reliability value of NDT scan matching is also called the "NDT Reliability Value (NRV)."

[0066] In addition, DR position X DR is an example of the "first candidate position" in the present invention, and NDT position X NDT is an example of a "second candidate position" in the present invention. Moreover, the estimated vehicle position X^ is an example of an "estimated position" in the present invention.

[0067] (5-1) Block configuration Fig. 6 is an example of a functional block of the vehicle position estimation unit 15. As shown in Fig. 6, the vehicle position estimation unit 15 has a dead reckoning block 21, a DR position calculation block 22, a coordinate transformation block 23, a point cloud data association block 24, an NDT position calculation block 25, and a fusion block 26.

[0068] The dead reckoning block 21 uses the vehicle's moving speed and angular velocity based on the outputs of the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5 to calculate the moving distance and the change in direction from the previous time. The DR position calculation block 22 calculates the estimated vehicle position X at the immediately preceding processing time, i.e., the time k-1. ^ (k-1), the DR position X at time k is calculated by adding the movement distance and azimuth change obtained by dead reckoning. DR (k) is calculated. This DR position X DR (k) is the vehicle position obtained at time k based on the dead reckoning, and the predicted vehicle position X - Equivalent to (k).

[0069] The coordinate conversion block 23 converts the processed point cloud data after downsampling output from the downsampling processing unit 14 into a world coordinate system, which is the same coordinate system as the map DB 10. In this case, the coordinate conversion block 23 performs coordinate conversion of the processed point cloud data at time k, for example, based on the predicted vehicle position output by the DR position calculation block 22 at time k. Note that, instead of performing the above-mentioned coordinate conversion on the processed point cloud data after downsampling, the above-mentioned coordinate conversion may be performed on the point cloud data before downsampling. In this case, the downsampling processing unit 14 generates processed point cloud data in a world coordinate system by downsampling the point cloud data in the world coordinate system after the coordinate conversion. Note that the process of converting the point cloud data in a coordinate system based on a lidar installed on a vehicle into a vehicle coordinate system, and the process of converting from the vehicle coordinate system to the world coordinate system are disclosed, for example, in International Publication WO2019 / 188745.

[0070] The point cloud data matching block 24 matches the processing point cloud data in the world coordinate system output by the coordinate transformation block 23 with the voxel data VD expressed in the same world coordinate system, thereby matching the processing point cloud data with the voxels. The NDT position calculation block 25 calculates an individual evaluation function value based on equation (5) for each voxel that has been matched with the processing point cloud data, and calculates an estimated parameter P that maximizes the score value E(k) based on equation (6). The NDT position calculation block 25 then calculates the DR position X output by the DR position calculation block 22 based on equation (7). DR (k), the NDT position X at time k is determined by applying the estimated parameter P obtained at time k. NDT (k) is calculated. This NDT position X NDT (k) is the vehicle position at time k determined based on NDT scan matching.

[0071] The fusion block 26 determines the DR position X at time k. DR (k) and the NDT position X at time k NDT (k) and the estimated vehicle position X at time k. ^ In this case, the fusion block 26 calculates the reliability value NRV of the NDT scan matching at time k, and applies weighting based on the reliability value NRV to the DR position X DR (k) and NDT position X NDT (k) to estimate the vehicle position X ^ The process of the fusion block 26 will be described in detail later.

[0072] Here, a specific procedure for associating the measurement points with the voxel data VD will be further explained using a simple example.

[0073] 7 shows the positional relationship between voxels "Vo1" to "Vo6" in which voxel data VD exists on a two-dimensional xy plane in the world coordinate system, and measurement points 61 to 65 indicating positions near these voxels. For ease of explanation, it is assumed here that the z coordinates of the center positions of the voxels Vo1 to Vo6 in the world coordinate system are the same as the z coordinates of the measurement points 61 to 65 in the world coordinate system.

[0074] First, the coordinate conversion block 23 converts the point cloud data including the measurement points 61 to 65 into the world coordinate system. After that, the point cloud data association block 24 performs a process of rounding off the fractions of the measurement points 61 to 65 in the world coordinate system. In the example of Fig. 7, since the size of each voxel, which is a cube, is 1m, the point cloud data association block 24 rounds off the decimal points of the x, y, and z coordinates of each of the measurement points 61 to 65.

[0075] Next, the point cloud data matching block 24 checks the voxel data VD corresponding to the voxels Vo1 to Vo6 against the coordinates of each of the measurement points 61 to 65 to determine the voxels corresponding to each of the measurement points 61 to 65. In the example of FIG. 7, the (x, y) coordinates of the measurement point 61 become (2, 1) due to the above-mentioned rounding off, so the point cloud data matching block 24 matches the measurement point 61 with the voxel Vo1. Similarly, the (x, y) coordinates of the measurement points 62 and 63 become (3, 2) due to the above-mentioned rounding off, so the point cloud data matching block 24 matches the measurement points 62 and 63 with the voxel Vo5. In addition, the (x, y) coordinates of the measurement point 64 become (2, 3) due to the above-mentioned rounding off, so the point cloud data matching block 24 matches the measurement point 64 with the voxel Vo6. On the other hand, since the (x, y) coordinates of the measurement point 65 become (4, 1) due to the above-mentioned rounding, the point cloud data matching block 24 determines that there is no voxel data VD corresponding to the measurement point 65. Thereafter, the NDT position calculation block 25 estimates the estimation parameter P using the measurement point and the voxel data VD associated by the point cloud data matching block 24. Note that in this example, the number of measurement points after downsampling is five, but the number of corresponding measurement points Nc is four. The number of corresponding measurement points Nc is smaller when there are few structures around the road and the space is sparse, and also when a phenomenon occurs in which another vehicle is present near the vehicle and the lidar's light beam is blocked (called occlusion).

[0076] (5-2) Details of Fusion Block Processing First, a method for calculating the NRV, which represents the reliability of NDT scan matching, will be described. The fusion block 26 calculates a reliability value NRV based on the downsampling size DSS, the score value E, and the ratio of the number of measurement points associated with the voxel data VD in the processed point cloud data after downsampling (i.e., the number of corresponding measurement points Nc). Hereinafter, the ratio of the number of corresponding measurement points Nc is also called the "association ratio DAR (Data Association Ratio)." In other words, the association ratio DAR = (number of corresponding measurement points Nc / number of processed point cloud data after downsampling).

[0077] Specifically, the fusion block 26 calculates the reliability value NRV at each corresponding time by multiplying the downsampling size DSS, the score value E, and the association ratio DAR at each time, as shown in the following equation (8).

[0078] NRV = DSS × E × DAR (8) Empirically, the downsampling size DSS is in the range of 0.1 to 5.0, the association ratio DAR is in the range of 0 to 1.0, and the score value E is in the range of 0 to 3.0, so the reliability value is in the range of approximately 0 to 15.

[0079] Here, the validity of the calculation of the reliability value NRV based on the formula (8) will be explained. The performance of NDT scan matching has the following three features (first feature to third feature), and the formula (8) is established based on these three features.

[0080] In general, if the number of points in the point cloud data detected by the lidar 2 is large, calculations can be performed based on data with high density over a wide range, and the reliability of the NDT scan matching results will be higher. Therefore, when control is adopted that adaptively changes the downsampling size DSS to adjust the number of corresponding measurement points Nc, the more points in the point cloud data, the larger the downsampling size DSS. Therefore, as a first feature, the larger the downsampling size DSS, the higher the reliability of NDT scan matching.

[0081] In addition, it is estimated that the larger the matching ratio DAR, the less occlusion of the structures constituting the voxel data VD by obstacles such as other surrounding vehicles, and that the actual situation has not changed since the time the voxel data VD was created. Therefore, as a second characteristic, the larger the matching ratio DAR, the higher the reliability of the NDT scan matching.

[0082] Furthermore, the larger the score value E, the more likely it is that the calculated estimated parameters P are the optimal solution. Therefore, as a third feature, the larger the score value E, the higher the reliability of the NDT scan matching.

[0083] Taking the above into consideration, the fusion block 26 calculates the reliability value NRV using the downsampling size DSS, the matching ratio DAR, and the score value E at each time of the NDT scan matching process. In addition, in equation (8), since the reliability value NRV is determined by multiplication of these, when any of the downsampling size DSS, the matching ratio DAR, and the score value E approaches 0, the reliability value NRV also approaches 0.

[0084] Next, weighting by the reliability value NRV will be described. If the weight for the result of the NDT scan matching process is "w" (0≦w≦1), the fusion block 26 calculates the DR position X based on the following equation (9): DR and NDT position X NDT A weighted average is calculated to calculate the estimated vehicle position X^.

[0085]

number

[0086] In this embodiment, the fusion block 26 calculates an index (also called an "NDT reliability index NRI") by normalizing the reliability value NRV so that it falls within the range of 0 to 1, and uses the calculated NDT reliability index NRI as the weight w. That is, the fusion block 26 calculates the DR position X by using the NDT reliability index NRI based on the following formula (10): DR and NDT position X NDT A weighted average is calculated to calculate the estimated vehicle position X^.

[0087]

number

[0088] Next, a method for calculating the NDT reliability index NRI will be described. Fig. 8 is a graph showing the characteristics of each of the four calculation formulas for the NDT reliability index NRI. Here, the relationship between the reliability value NRV and the NDT reliability index NRI for each of the following formulas (11) to (14) is shown.

[0089]

number

[0090] Here, the higher the accuracy and stability of NDT scan matching, the larger the downsampling size DSS, the matching ratio NDR, and the score value E will be, so when all of these have large values, the NDT reliability index NRI approaches 1. On the other hand, when any one of the downsampling size DSS, the matching ratio NDR, and the score value E is a small value, the NDT reliability index NRI approaches 0 even if the other values ​​are large.

[0091] Next, a supplementary explanation will be given for formulas (11) to (14). As shown in FIG. 8, in formula (11), the NDT reliability index NRI rises quickly with respect to the reliability value NRV, in formula (12), the rise is gentle in the first half, and in formula (13), the change in the NDT reliability index NRI with respect to the reliability value NRV is generally gentle. In addition, formula (14) rises slightly more gently than formula (11), but shows the characteristic that the NDT reliability index NRI approaches 1 the earliest. Thus, it can be said that there is a difference in the sensitivity of the NDT reliability index NRI with respect to the reliability value NRV in each formula. Which formula is used to calculate the NDT reliability index NRI from the reliability value NRV from formulas (11) to (14) is determined in advance according to the reliability of the dead reckoning. Note that, as the reliability of the dead reckoning, for example, the unevenness in the sensitivity of the gyro sensor 3 and the performance values ​​listed in the catalog of each sensor used for the dead reckoning may be taken into consideration.

[0092] (5-3) Technical Effects Next, the DR position X is calculated based on the reliability value NRV. DR and NDT position X NDT The following provides a supplementary explanation of the technical effect of calculating the estimated vehicle position X^ by weighted averaging with

[0093] NDT scan matching may have low accuracy when there are few surrounding structures or when occlusion occurs, as will be described with reference to Figures 9(A) to 9(C).

[0094] 9(A) to 9(C) show an overhead view of the periphery of a vehicle equipped with an on-board device 1 when the downsampling size is set to one time the voxel size. In FIG. 9(A) to FIG. 9(C), voxels in which voxel data VD exists are indicated by rectangular frames, and the positions of the measurement points of the lidar 2 obtained by one period of scanning are indicated by dots. Here, FIG. 9(A) shows an example in which there are many structures around the lidar 2, FIG. 9(B) shows an example in which there are few structures around the lidar 2, and FIG. 9(C) shows an example in which occlusion occurs. Note that the voxels corresponding to the surface positions of the structures have corresponding voxel data VD.

[0095] As shown in Fig. 9(A) to Fig. 9(C), the number of measurement points of the point cloud data acquired by the LIDAR 2 depends not only on the measurement area and viewing angle of the LIDAR 2 but also on the environment around the vehicle. For example, when the surrounding space is dense (see Fig. 9(A)), the number of measurement points of the point cloud data is large, but when the surrounding space is sparse (see Fig. 9(B)), the number of measurement points of the point cloud data is small. In addition, when occlusion by other vehicles or the like occurs (see Fig. 9(C)), the number of corresponding measurement points Nc available for NDT scan matching is reduced. In the example of Fig. 9(A), the number of corresponding measurement points Nc available for NDT scan matching is sufficient, so there is a high possibility that an optimal solution can be obtained by NDT scan matching, but in the examples of Fig. 9(B) and Fig. 9(C), the number of corresponding measurement points Nc is small, so there is a possibility that NDT scan matching will fall into a local solution.

[0096] Next, a comparison of accuracy between NDT scan matching and dead reckoning will be described with reference to FIGS. 10(A) and 10(B).

[0097] 10(A) and (B) are graphs showing the error distribution of dead reckoning and the error distribution of NDT scan matching, respectively. Specifically, FIG. 10(A) shows the error distribution of dead reckoning and the error distribution of NDT scan matching, respectively, in the case where the number of points in the point cloud data output by the LIDAR 2 is relatively abundant and there is relatively little occlusion. Also, FIG. 10(B) shows the error distribution of dead reckoning and the error distribution of NDT scan matching, respectively, in the case where the number of points in the point cloud data output by the LIDAR 2 is too few or there is relatively much occlusion. Also, for the sake of simplification, FIGS. 10(A) and (B) show the error distribution for one element constituting the estimated position (including the attitude), and the DR position X DR , NDT position NDT , and the estimated vehicle position X^ are indicated by arrows.

[0098] When the point cloud data output by the LIDAR 2 has a large number of points and little occlusion, NDT scan matching is calculated stably, and the error distribution of NDT scan matching exhibits normal distribution characteristics. Generally, the measurement accuracy of the LIDAR 2 is high, so in this case, as shown in FIG. 10(A), the error distribution of NDT scan matching has narrow bases and steeper characteristics compared to the error distribution of dead reckoning. Therefore, in this case, the vehicle position calculation result by NDT scan matching is sufficiently reliable. Therefore, since the reliability value NRV is large and the NDT reliability index NRI is close to 1, the estimated vehicle position X^ by the weighted average of equation (10) is X NDT It will be close to:

[0099] On the other hand, if the number of points in the point cloud data output by the lidar 2 is small or occlusion occurs, the NDT scan matching calculation will contain errors. If the errors are large, the characteristics may be gentle and have multiple peaks, as shown in Figure 10(B). Here, the NDT scan matching calculation searches for the condition that maximizes the evaluation function (score value E), and the point with the highest peak is calculated as the result. In the case of Figure 10(B), the right end of the distribution has the highest peak, so this is selected as the NDT position X. NDT In this case, the NDT position is calculated as X NDT is not very trustworthy.

[0100] Taking the above into consideration, the fusion block 26 according to the present embodiment calculates the DR position X based on the reliability value NRV. DR and NDT position X NDT The estimated vehicle position X^ is calculated by a weighted average calculation of the NDT scan matching and the NDT reliability index NRI, as shown in Fig. 10(A). NDT The result closest to the estimated vehicle position X^ (fusion position) is calculated as the final estimated vehicle position X^ (fusion position). On the other hand, when the reliability of the NDT scan matching is low and the NDT reliability index NRI is close to 0, the NDT position X^ is calculated as shown in Figure 10(B). NDT Away from DR position X DRThis makes it possible to prevent the accuracy of the final estimated vehicle position from deteriorating even when the reliability of NDT scan matching is low.

[0101] (6) Processing Flow Fig. 11 is an example of a flowchart showing a procedure of a process related to vehicle position estimation executed by the controller 13 of the vehicle-mounted device 1. The controller 13 starts the process of the flowchart in Fig. 11 when it becomes necessary to perform vehicle position estimation, for example, when the power is turned on.

[0102] First, the controller 13 determines a predicted vehicle position based on the positioning result of the GPS receiver 5 (step S11). Then, the controller 13 judges whether or not the point cloud data of the LIDAR 2 has been acquired (step S12). Then, if the controller 13 cannot acquire the point cloud data of the LIDAR 2 (step S12; No), the controller 13 continues to perform the judgment of step S12. Furthermore, during a period in which the controller 13 is unable to acquire the point cloud data, the controller 13 continues to determine the vehicle position based on the positioning result of the GPS receiver 5. Note that the controller 13 may determine the vehicle position based on the output of any sensor other than the point cloud data, not limited to the positioning result of the GPS receiver 5 alone.

[0103] Then, when the controller 13 has acquired the point cloud data of the LIDAR 2 (step S12; Yes), the controller 13 executes dead reckoning based on the vehicle moving speed and angular velocity detected based on the gyro sensor 3 and the vehicle speed sensor 4, etc., and the previous estimated vehicle position, and calculates a DR position X DR (Step S13). Then, the controller 13 performs downsampling on the point cloud data for one period scanned by the LIDAR 2 at the current processing time using the current setting value of the downsampling size DSS (Step S14).

[0104] Next, the controller 13 sets the DR position X DR By performing the NDT matching process with the initial value, the NDT position X NDT(Step S15). In this case, the controller 13 performs a process of converting the processing point cloud data into data in the world coordinate system, and associates the processing point cloud data converted into the world coordinate system with the voxels in which the voxel data VD exists.

[0105] Next, the controller 13 calculates the correspondence ratio DAR based on the correspondence result between the processing point cloud data and the voxel data VD in the NDT matching process (step S16). Nc The downsampling size DSS for the next downsampling is determined by comparing with the estimated parameter P. Furthermore, the controller 13 obtains a score value E for the estimated parameter P obtained by the NDT matching process (step S17).

[0106] Then, the controller 13 calculates the reliability value NRV (step S18) based on the downsampling size DSS, the association ratio DAR, and the score value E. Then, the controller 13 generates the NDT reliability index NRI from the reliability value NRV (step S19).

[0107] Then, the controller 13 calculates the DR position X using the NDT reliability index NRI. DR and NDT position X NDT The controller 13 calculates the estimated vehicle position X^ by calculating a weighted average of the above (step S20). Then, the controller 13 determines whether or not the vehicle position estimation process should be ended (step S21). If the controller 13 determines that the vehicle position estimation process should be ended (step S21; Yes), the controller 13 ends the process of the flowchart. On the other hand, if the controller 13 determines that the vehicle position estimation process should be continued (step S21; No), the controller 13 returns the process to step S12 and estimates the vehicle position at the next processing time.

[0108] (7) Considerations based on the experimental results Next, experimental results regarding the above-described embodiment will be considered.

[0109] The applicant targeted medium distances, and drove a vehicle equipped with two front and two rear lidars with a horizontal viewing angle of 60 degrees and an operating frequency of 12 Hz (period 83.3 ms) along a certain road, and estimated the vehicle's position by matching voxel data (ND map) created in advance for the road with the lidar point cloud data obtained during the drive. In addition, for accuracy evaluation, the RTK-GPS positioning results were used as the correct position data.

[0110] 12(A) to 12(F) and 13(A) to 13(D) show the NDT position X NDT 12 shows the result of vehicle position estimation according to a comparative example in which the estimated vehicle position X^ is defined as the estimated vehicle position X^. Here, for the RTK-GPS positioning result, FIG. 12(A) shows the error in the traveling direction, FIG. 12(B) shows the error in the lateral direction, FIG. 12(C) shows the error in the height direction, and FIG. 12(D) shows the error in the yaw angle, FIG. 12(E) shows the number of measurement points of the point cloud data before downsampling, and FIG. 12(F) shows the size of the downsampling. Also, FIG. 13(A) shows the number of measurement points of the processed point cloud data after downsampling, FIG. 13(B) shows the number of corresponding measurement points Nc, FIG. 13(C) shows the correspondence ratio DAR, and FIG. 13(D) shows the score value E(k). Also, the RMSE (Root Mean Squared Error) value in the traveling direction was 0.235 m, the RMSE value in the lateral direction was 0.044 m, the RMSE value in the height direction was 0.039 m, and the RMSE value in the azimuth (yaw angle) was 0.232 degrees.

[0111] As shown in FIG. 12(A), in the period around 130 s, the error in the direction of travel is large and the lines are generally thick, so it is inferred that NDT matching is not performed stably in that period. In addition, since the number of measurement points before downsampling shown in FIG. 12(E) is not very large, the downsampling size DSS shown in FIG. 12(F) is relatively small, being less than 1 m. In addition, the matching ratio DAR in FIG. 13(C) is also low, and the score value E in FIG. 13(D) is also slightly small. In other words, the calculation of NDT scan matching in the comparative example can be judged to be in a state where the reliability is not very high, since the downsampling size DSS, the matching ratio DAR, and the score value E are all relatively small. Therefore, it is inferred that the NDT matching result contains many errors, which causes the error in the direction of travel shown in FIG. 12(A) to be large.

[0112] 14(A) to 14(F) and 15(A) to 15(F) are diagrams showing the NDT position X NDT and DR position X DR 14 shows the vehicle position estimation result according to the embodiment in which the estimated vehicle position X^ is determined by fusion with the RTK-GPS. Here, for the RTK-GPS positioning result, Fig. 14(A) shows the error in the traveling direction, Fig. 14(B) shows the error in the lateral direction, Fig. 14(C) shows the error in the height direction, Fig. 14(D) shows the error in the yaw angle, Fig. 14(E) shows the number of measurement points of the point cloud data before downsampling, and Fig. 14(F) shows the size of the downsampling. Also, Fig. 15(A) shows the number of measurement points of the processed point cloud data after downsampling, Fig. 15(B) shows the number of corresponding measurement points Nc, Fig. 15(C) shows the correspondence ratio DAR, Fig. 15(D) shows the score value E(k), Fig. 15(E) shows the reliability value NRV, and Fig. 15(F) shows the NDT reliability index NRI. In addition, the RMSE value in the direction of travel was 0.150 m, the RMSE value in the lateral direction was 0.034 m, the RMSE value in the height direction was 0.022 m, and the RMSE value in the azimuth (yaw angle) was 0.171 degrees, which were good values ​​compared to the comparative example.

[0113] Referring to the reliability value NRV shown in FIG. 15(E), it fluctuates around 1.0, and it cannot be said that the reliability of the NDT scan matching is very high. Also, FIG. 15(F) corresponds to the NDT reliability index NRI calculated by equation (11) using the reliability value NRV shown in FIG. 15(E), and is not very close to 1. The estimated vehicle position X^ is calculated based on equation (10) using this NDT reliability index NRI. In this case, the error in the traveling direction shown in FIG. 14(A) is greatly improved compared to the comparative example, and the other errors shown in FIG. 14(B) to FIG. 14(D) are also improved. In this way, the NDT position X NDT and DR position X DR It is assumed that the fusion of this technology has improved the accuracy of estimating the vehicle's position.

[0114] The applicant also conducted additional experiments using a different type of lidar. In the additional experiments, the applicant ran a vehicle equipped with a lidar capable of measuring long distances, with a horizontal field of view of 360 degrees and an operating frequency of 10 Hz (period of 100 ms) along a certain road, and estimated the vehicle's position by matching voxel data (ND map) created in advance for the road with the lidar point cloud data obtained during the road. In addition, the positioning results of RTK-GPS were used as the correct position data.

[0115] 16(A) to 16(F) and 17(A) to 17(D) are diagrams showing the NDT position X NDT16 shows the result of vehicle position estimation according to a comparative example in which the estimated vehicle position X^ is defined as the estimated vehicle position X^. Here, for the RTK-GPS positioning result, FIG. 16(A) shows the error in the traveling direction, FIG. 16(B) shows the error in the lateral direction, FIG. 16(C) shows the error in the height direction, and FIG. 16(D) shows the error in the yaw angle, FIG. 16(E) shows the number of measurement points of the point cloud data before downsampling, and FIG. 16(F) shows the size of the downsampling. Also, FIG. 17(A) shows the number of measurement points of the processed point cloud data after downsampling, FIG. 17(B) shows the number of corresponding measurement points Nc, FIG. 17(C) shows the correspondence ratio DAR, and FIG. 17(D) shows the score value E(k). Also, the RMSE value in the traveling direction was 0.031 m, the RMSE value in the lateral direction was 0.029 m, the RMSE value in the height direction was 0.027 m, and the RMSE value in the azimuth (yaw angle) was 0.051 degrees.

[0116] Since the lidar used in this experiment is a long-distance, all-around type, the number of measurement points before downsampling shown in FIG. 16(E) is tens of thousands, which is a sufficiently large amount of data. Therefore, the downsampling size DSS shown in FIG. 16(F) is a larger value compared to the experiments in FIG. 12 to FIG. 15. In addition, the matching ratio DAR drops around times 120 s and 220 s, but this is because the road is overpassing and the amount of map information is small, and the matching ratio DAR and score value are good values ​​outside of these places. As a result, although the error of the NDT position is good, there is a slight disturbance around 220 s in FIG. 16(A).

[0117] 18(A) to 18(F) and 19(A) to 19(F) show the NDT position X NDT and DR position X DR18 shows the result of vehicle position estimation according to an embodiment in which the estimated vehicle position X^ is determined by fusion with the RTK-GPS. Here, for the RTK-GPS positioning result, Fig. 18(A) shows the error in the traveling direction, Fig. 18(B) shows the error in the lateral direction, Fig. 18(C) shows the error in the height direction, Fig. 18(D) shows the error in the yaw angle, Fig. 18(E) shows the number of measurement points of the point cloud data before downsampling, and Fig. 18(F) shows the size of the downsampling. Also, Fig. 19(A) shows the number of measurement points of the processed point cloud data after downsampling, Fig. 19(B) shows the number of corresponding measurement points Nc, Fig. 19(C) shows the correspondence ratio DAR, Fig. 19(D) shows the score value E(k), Fig. 19(E) shows the reliability value NRV, and Fig. 19(F) shows the NDT reliability index NRI. In addition, the RMSE value in the direction of travel was 0.029 m, the RMSE value in the lateral direction was 0.027 m, the RMSE value in the height direction was 0.024 m, and the RMSE value in the azimuth (yaw angle) was 0.050 degrees, which were slightly better than those of the comparative example.

[0118] Here, the reliability value shown in FIG. 19(E) drops near 120 s and 220 s, and it can be said that the reliability of NDT scan matching drops during that period. Also, FIG. 19(F) corresponds to the NDT reliability index NRI calculated by equation (11) using the reliability value NRV shown in FIG. 19(E), and it can be seen that it shows values ​​close to 1 except near 120 s and 220 s. The estimated vehicle position X^ is calculated based on equation (10) using this NDT reliability index NRI. In this case, as shown in FIG. 18(A) to FIG. 18(D), the error near 220 s is improved. Thus, in this experimental result, the NDT position X NDT and DR position X DR It is assumed that the accuracy of self-location estimation has been improved by fusion with

[0119] (8) Variations Preferred modifications of the above-described embodiment will now be described. The following modifications may be combined and applied to these embodiments.

[0120] (Variation 1) The reliability value NRV in the formulas (11) to (14) may be multiplied by a coefficient "α" to calculate the NDT reliability index NRI.

[0121] In this case, the formulas (11) to (14) can be expressed as the following formulas (15) to (18) using the coefficient a.

[0122]

number

[0123] In this modification, the coefficient a is set to a value according to the measurement accuracy of the LIDAR 2 used. For example, if the accuracy of the distance measurement values ​​and angle measurement values ​​included in each measurement point of the LIDAR 2 is poor, each measurement point of the point cloud data will contain an error. In this case, the coefficient a is set to a small value because the NDT scan matching based on point cloud data with a large error will also result in an error. As a result, the NDT reliability index NRI can be set to a small value for the same reliability value NRV.

[0124] On the other hand, when the measurement accuracy of LIDAR 2 is high, the accuracy of the NDT scan matching is also good, so by setting the coefficient a to a larger value, the NDT reliability index NRI can be set to a larger value even for the same reliability value NRV.

[0125] The measurement accuracy of the lidar 2 used can be obtained from the values ​​in the data sheet, but it may also be obtained by actual measurement. For example, it is possible to actually measure the measurement accuracy of the target lidar 2 by continuously measuring a specific object with the target lidar 2 while the vehicle is stopped and obtaining the variance value of the values.

[0126] Moreover, the NDT reliability index NRI, which falls within the range of 0 to 1, may be determined based on any algorithm other than equations (11) to (18).

[0127] For example, the NDT reliability index NRI may be determined as follows: If NRV<2.5, NRI=NRV / 2.5 If NRV≧2.5, NRI=1.0

[0128] In another example, the NDT reliability index NRI may be determined by detailed case classification as follows. If NRV<0.5, NRI=0.5 When 0.5≦NRV<1.0, NRI=0.6 When 1.0≦NRV<1.5, NRI=0.7 When 1.5≦NRV<2.0, NRI=0.8 When 2.0≦NRV<2.5, NRI=0.9 When NRV is 2.5 or less, NRI=1.0

[0129] Even in these cases, the fusion block 26 can determine the NDT reliability index NRI by normalizing the reliability value NRV preferably within the range of 0-1.

[0130] (Variation 2) The vehicle-mounted device 1 calculates the reliability value NRV based on the downsampling size DSS on the assumption that downsampling will be performed. Alternatively, the vehicle-mounted device 1 may calculate the reliability value NRV and calculate the NDT position X NDT This may be used to determine the weight w of the

[0131] In this case, the vehicle-mounted device 1 performs correspondence between the voxel data VD and the point cloud data output by the LIDAR 2, and performs NDT scan matching, calculation of the score value E, and calculation of the correspondence ratio DAR. Then, the vehicle-mounted device 1 determines a reliability value NRV based on the calculated score value E and the correspondence ratio DAR. In this case, the reliability value NRV is, for example, the product of the score value E and the correspondence ratio DAR. Then, the vehicle-mounted device 1 calculates an NDT reliability index NRI by normalizing the reliability value NRV based on the embodiment or the first modified example, and determines the DR position X DR and NDT position X NDT The estimated vehicle position X^ is calculated by a weighted average calculation.

[0132] In this way, even if downsampling is not performed, the vehicle-mounted device 1 can suitably calculate the estimated vehicle position X^.

[0133] (Variation 3) The reliability value NRV is not limited to the multiplication value of the downsampling size DSS, the score value E, and the association ratio DAR, but may be a value defined by any formula that has a positive correlation with the downsampling size DSS, the score value E, and the association ratio DAR. The reliability value NRV may be calculated using at least one of the downsampling size DSS, the score value E, and the association ratio DAR. In other words, the reliability value NRV may be calculated based on any one or two of the downsampling size DSS, the association ratio DAR, and the score value E. Since the downsampling size DSS increases as the number of measurement points before downsampling (i.e., the number of measurement points of the point cloud data output by the LIDAR 2) increases, the reliability value NRV may be calculated using, for example, a value of (the number of measurement points before downsampling / 10,000) instead of the downsampling size DSS.

[0134] (Variation 4) The configuration of the driving assistance system shown in Fig. 1 is an example, and the configuration of the driving assistance system to which the present invention can be applied is not limited to the configuration shown in Fig. 1. For example, instead of having the on-board device 1, the driving assistance system may have an electronic control device of the vehicle execute the processing of the downsampling processing unit 14 and the vehicle position estimation unit 15 of the on-board device 1. In this case, the map DB 10 is stored in, for example, a storage unit in the vehicle or a server device that performs data communication with the vehicle, and the electronic control device of the vehicle executes the vehicle position estimation based on downsampling and NDT scan matching by referring to this map DB 10.

[0135] (Variation 5) The voxel data VD is not limited to a data structure including a mean vector and a covariance matrix as shown in Fig. 5. For example, the voxel data VD may include point cloud data measured by a measurement maintenance vehicle and used to calculate the mean vector and the covariance matrix.

[0136] As described above, the controller 13 of the vehicle-mounted device 1 according to this embodiment determines the first candidate position of the vehicle, DR position X DR The controller 13 also acquires an NDT position X, which is a second candidate position of the vehicle, determined based on NDT scan matching, which is a comparison between point cloud data output by the LIDAR 2, which is an external sensor provided on the vehicle, and voxel data VD, which is map data. NDT Further, the controller 13 calculates a reliability value NRV representing the reliability of the NDT scan matching. Then, the controller 13 calculates the DR position X DR and NDT position X NDT and the reliability value NRV, the controller 13 determines the estimated vehicle position X^. In this manner, even when there are few structures in the vicinity or when occlusion occurs, the controller 13 can suitably reduce the deterioration of the position estimation accuracy.

[0137] In the above-described embodiment, the program can be stored using various types of non-transitory computer readable media and supplied to a controller or the like that is a computer. The non-transitory computer readable medium includes various types of tangible storage media. Examples of the non-transitory computer readable medium include magnetic storage media (e.g., flexible disks, magnetic tapes, hard disk drives), magneto-optical storage media (e.g., magneto-optical disks), CD-ROM (Read Only Memory), CD-R, CD-R / W, and semiconductor memory (e.g., mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (Random Access Memory)).

[0138] Although the present invention has been described above with reference to the embodiments, the present invention is not limited to the above embodiments. Various modifications that can be understood by those skilled in the art can be made to the configuration and details of the present invention within the scope of the present invention. In other words, the present invention naturally includes various modifications and corrections that a person skilled in the art could make in accordance with the entire disclosure, including the claims, and the technical ideas. In addition, the disclosures of the above cited patent documents and the like are incorporated herein by reference. [Explanation of symbols]

[0139] 1 On-vehicle device 2. Rider 3 Gyro sensor 4 Vehicle speed sensor 5 GPS receiver 10 Map DB

Claims

1. a first candidate position acquisition means for acquiring a first candidate position of the moving object; a second candidate position acquisition means for acquiring a second candidate position of the moving body determined based on a comparison between data based on an output of an external sensor provided in the moving body and map data; A reliability value calculation means for calculating a reliability value representing the reliability of the matching; an estimated position determining means for determining an estimated position of the moving object based on the first candidate position, the second candidate position, and the reliability value; having the data is point cloud data, The map data includes voxel data representing a position of an object for each voxel, which is a unit area; The method further includes: associating means for associating measurement points constituting the point cloud data with each of the voxels; The reliability value calculation means calculates the reliability value based at least on a correspondence ratio which is a ratio of the number of the measurement points for which the correspondence has been performed to the number of measurement points constituting the point cloud data.

2. The information processing apparatus according to claim 1 , wherein the reliability value calculation means determines the reliability value based on at least the association ratio and a score value indicating a degree of matching of the matching.

3. the data is second point cloud data that is point cloud data obtained by downsampling first point cloud data that is point cloud data output by the external sensor, The information processing apparatus according to claim 1 , wherein the reliability value calculation means calculates the reliability value based on the association ratio and a size of the downsampling or a number of measurement points of the first point cloud data.

4. The information processing device according to any one of claims 1 to 3, wherein the reliability value calculation means calculates as the reliability value a value that has a proportional relationship or a positive correlation with the correspondence ratio, the size of downsampling performed on the first point cloud data, which is the point cloud data output by the external sensor, or the number of measurement points of the first point cloud data, and a score value indicating the degree of compatibility of the matching.

5. The information processing device according to any one of claims 1 to 4, wherein the estimated position determination means determines the estimated position by a weighted average of the first candidate position and the second candidate position, the weighting being based on the reliability value.

6. The information processing apparatus according to claim 5 , wherein the estimated position determining means calculates a reliability index obtained by normalizing the reliability value to a value between 0 and 1 as a weight for the second candidate position.

7. 7. The information processing apparatus according to claim 1, wherein the first candidate position acquisition means acquires, as the first candidate position, a position of the moving object determined by dead reckoning.

8. A computer-implemented control method, comprising: Obtaining a first candidate position of the moving object; acquiring a second candidate position of the moving body determined based on a comparison between data based on an output of an external sensor provided in the moving body and map data; calculating a confidence value representing the confidence of the matching; determining an estimated position of the moving object based on the first candidate position, the second candidate position, and the reliability value; the data is point cloud data, The map data includes voxel data representing a position of an object for each voxel, which is a unit area; Corresponding measurement points constituting the point cloud data with each of the voxels; calculating the reliability value based at least on a correspondence ratio that is a ratio of the number of the measurement points for which the correspondence has been performed to the number of measurement points constituting the point cloud data; Control methods.

9. Obtaining a first candidate position of the moving object; acquiring a second candidate position of the moving body determined based on a comparison between data based on an output of an external sensor provided in the moving body and map data; calculating a confidence value representing the confidence of the matching; causing a computer to execute a process of determining an estimated position of the moving object based on the first candidate position, the second candidate position, and the reliability value; the data is point cloud data, The map data includes voxel data representing a position of an object for each voxel, which is a unit area; Corresponding measurement points constituting the point cloud data with each of the voxels; calculating the reliability value based at least on a correspondence ratio that is a ratio of the number of the measurement points for which the correspondence has been performed to the number of measurement points constituting the point cloud data; program.

10. A storage medium storing the program according to claim 9.

Images