Information processing apparatus, method, program, and storage medium

Abstract

To provide an information processing apparatus that can suitably prevent a reduction in position estimation accuracy.SOLUTION: A controller 13 of an on-vehicle machine 1 acquires a DR position XDR that is a first candidate position of a vehicle. The controller 13 acquires an NDT position XNDT that is a second candidate position of the vehicle determined on the basis of NDT scan matching that is collation of point group data output by a lidar 2 that is an external sensor provided in the vehicle and voxel data VD that is map data. The controller 13 calculates a reliability value NRV that represents the reliability of the NDT scan matching. The controller 13 determines an estimated vehicle position X^ on the basis of the DR position XDR, the NDT position XNDT, and the reliability value NRV.SELECTED DRAWING: Figure 11

Description

Technical Field

[0001] The present disclosure relates to position estimation.

Background Art

[0002] Conventionally, a technique for estimating the self-position of a vehicle is known in which shape data of surrounding objects measured using a measuring device such as a laser scanner is collated (matched) with map information in which the shapes of the surrounding objects are stored in advance. For example, Patent Document 1 discloses an autonomous driving 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 performs matching between map information and measurement data for voxels in which stationary objects exist. Further, Patent Document 2 discloses a scan matching method for estimating the position of the host vehicle by collating voxel data including the average 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

Patent Document 2

Summary of the Invention

Problems to be Solved by the Invention

[0004] When estimating the position of the host vehicle by collating voxel data with point cloud data output by a lidar as in Patent Document 2, the search for a solution is limited to a finite number of times in order to perform the process within a limited processing time. In this case, the obtained solution may be a local solution and not an optimal solution. In particular, when measurement is performed in a sparse space around, a space with few feature changes, or a space where occlusion occurs due to obstacles such as other vehicles, the possibility of falling into a local solution increases. And when the position based on the local solution is output as an estimation result, the position estimation accuracy is reduced.

[0005] The present disclosure has been made to solve the above problems, and a main object thereof is to provide an information processing apparatus capable of suitably preventing a decrease in position estimation accuracy.

Means for Solving the Problems

[0006] The invention according to the claim is a first candidate position acquisition means for acquiring a first candidate position of a moving body; a second candidate position acquisition means for acquiring a second candidate position of the moving body determined based on collation 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 collation; an estimated position determination means for determining an estimated position of the moving body based on the first candidate position, the second candidate position, and the reliability value; and further includes an acquisition means for acquiring the measurement accuracy of the measurement device, wherein the reliability value calculation means calculates the reliability value using a coefficient corresponding to the measurement accuracy, and is an information processing apparatus.

[0007] Also, the invention according to the claim is a method executed by a computer, comprising: acquiring a first candidate position of a moving body; acquiring a second candidate position of the moving body determined based on collation between data based on an output of an external sensor provided in the moving body and map data; calculating a reliability value representing the reliability of the collation; determining an estimated position of the moving body based on the first candidate position, the second candidate position, and the reliability value; acquiring the measurement accuracy of the measurement device; calculating the reliability value using a coefficient corresponding to the measurement accuracy; and is a method.

[0008] Also, the invention according to the claim is acquiring a first candidate position of a moving body; Obtain the second candidate position of the moving body determined based on the comparison between the data based on the output of the external sensor provided on the moving body and the map data, Calculate a reliability value representing the reliability of the comparison, Based on the first candidate position, the second candidate position, and the reliability value, determine the estimated position of the moving body, Obtain the measurement accuracy of the measuring device, A program that causes a computer to execute a process of calculating the reliability value using a coefficient corresponding to the measurement accuracy.

Brief Description of Drawings

[0009]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Figure 8

Figure 9

Figure 10

Figure 11

Figure 12

Figure 13

Figure 14

Figure 15

Figure 16

Figure 17

Figure 18

Figure 19

Figure 20

Mode for Carrying Out the Invention

[0010] According to a preferred embodiment of the present invention, an information processing apparatus includes a first candidate position acquisition means for acquiring a first candidate position of a moving body, a second candidate position acquisition means for acquiring a second candidate position of the moving body determined based on collation 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 collation, and an estimated position determination means for determining an estimated position of the moving body based on the first candidate position, the second candidate position, and the reliability value. According to this aspect, the information processing apparatus can suitably determine an estimated position considering the first candidate position and the second candidate position according to the reliability of the collation at the time of calculating the second candidate position, and can suitably prevent a decrease in position estimation accuracy.

[0011] In one aspect of the above information processing apparatus, the reliability value calculation means determines the reliability value based at least on a score value indicating the degree of fitness of the collation. According to this aspect, the information processing apparatus can accurately determine the reliability of the collation for determining the second candidate position.

[0012] In another aspect of the information processing apparatus, the data is second point cloud data obtained by downsampling 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 size of the downsampling or the number of measurement points of the first point cloud data. Also according to this aspect, the information processing apparatus can accurately determine the reliability of the collation for determining the second candidate position.

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

[0014] In another aspect of the information processing apparatus, the reliability value calculation means calculates the reliability value based on at least the association ratio that is the ratio of the number of the measurement points with which the association has been made. Also according to this aspect, the information processing apparatus can accurately determine the reliability of the collation for determining the second candidate position.

[0015] In another aspect of the information processing apparatus, the reliability value calculation means calculates, as the reliability value, a value having a proportional relationship or a positive correlation with the association ratio, the size of the downsampling performed on the first point cloud data that is 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 fitness of the collation. According to this aspect, the information processing apparatus can accurately determine the reliability of the collation for determining the second candidate position.

[0016] In another aspect of the information processing apparatus, the estimated position determining means determines the estimated position by a weighted average of the first candidate position and the second candidate position, which are weighted based on the reliability value. According to this aspect, the information processing apparatus can calculate an estimated position in which the first candidate position and the second candidate position are suitably fused.

[0017] In another aspect of the information processing apparatus, the estimated position determining means calculates a reliability index obtained by normalizing the reliability value to a value from 0 to 1 as the weight of the second candidate position. According to this aspect, the information processing apparatus can accurately set the weight used for the weighted average.

[0018] In another aspect of the information processing apparatus, the first candidate position obtaining means obtains the position of the moving body determined by dead reckoning as the first candidate position. According to this aspect, the information processing apparatus can suitably obtain a 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, the method including obtaining a first candidate position of a moving body, obtaining a second candidate position of the moving body determined based on a comparison between data based on an output of an external sensor provided on the moving body and map data, calculating a reliability value representing the reliability of the comparison, and determining an estimated position of the moving body based on the first candidate position, the second candidate position, and the reliability value. By executing this control method, the computer can suitably prevent a decrease in position estimation accuracy.

[0020] According to still another preferred embodiment of the present invention, the program acquires a first candidate position of the moving body, acquires a second candidate position of the moving body determined based on the collation of data based on the output of an external sensor provided in the moving body and map data, calculates a reliability value representing the reliability of the collation, and causes a computer to execute a process of determining an estimated position of the moving body based on the first candidate position, the second candidate position, and the reliability value. By executing the program, the computer can suitably prevent a decrease in the position estimation accuracy. Preferably, the above program is stored in a storage medium.

Example

[0021] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. In this specification, for convenience, a character with "^" or "-" attached to an arbitrary symbol is represented as "A^" or "A" (where "A" is an arbitrary character). - ” (where “A” is an arbitrary character).

[0022] (1) Overview of the driving support system FIG. 1 shows a schematic configuration of a driving support system according to this embodiment. The driving support system includes an in-vehicle device 1 that moves together with a vehicle as 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 in-vehicle device 1 is electrically connected to the lidar 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5, and based on their outputs, estimates the position of the vehicle (also referred to as the "own vehicle position") in which the in-vehicle device 1 is installed. Then, based on the estimated result of the own vehicle position, the in-vehicle device 1 performs automatic driving control of the vehicle, such as traveling along a set route to a destination. The in-vehicle device 1 stores a map database (DB: DataBase) 10 including voxel data "VD". The voxel data VD is data that records position information of stationary structures for each voxel indicating a cube (regular lattice) that is the minimum unit in three-dimensional space. The voxel data VD includes data representing the measured point cloud data of stationary structures in each voxel by a normal distribution, and is used for scan matching using NDT (Normal Distributions Transform) as described later. The in-vehicle device 1 estimates at least the position on the plane of the vehicle and the yaw angle by NDT scan matching. Note that the in-vehicle device 1 may further estimate the height position, pitch angle, and roll angle of the vehicle. Unless otherwise specified, the own vehicle position shall include attitude angles such as the yaw angle of the vehicle to be estimated.

[0024] The lidar 2 generates three-dimensional point cloud data indicating the position of an object existing in the external world by emitting a pulsed laser with respect to a predetermined angular range in the horizontal and vertical directions and discretely measuring the distance to the object. In this case, the lidar 2 includes an irradiation unit that irradiates laser light while changing the irradiation direction, a light receiving unit that receives the reflected light (scattered light) of the irradiated laser light, and an output unit that outputs scan data (points constituting the point cloud data, hereinafter referred to as "measurement points") based on the light reception signal output by the light receiving unit. The measurement points are generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the response delay time of the laser light specified based on the above-described light reception signal. Generally, 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, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5 each supply output data to the in-vehicle device 1.

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

[0026] (2) Configuration of in-vehicle unit FIG. 2 is a block diagram showing an example of the hardware configuration of the in-vehicle unit 1. The in-vehicle unit 1 mainly includes an interface 11, a memory 12, and a controller 13. These elements are interconnected via a bus line.

[0027] The interface 11 performs an interface operation related to the exchange of data between the in-vehicle unit 1 and an external device. In this embodiment, the interface 11 acquires output data from sensors such as the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and supplies it to the controller 13. Further, the interface 11 supplies a signal related to the driving control of the vehicle generated by the controller 13 to the electronic control unit (ECU) of the vehicle. The interface 11 may be a wireless interface such as a network adapter for performing wireless communication, or 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 volatile memories and non-volatile memories such as RAM (Random Access Memory), ROM (Read Only Memory), a hard disk drive, and a flash memory. The memory 12 stores a program for the controller 13 to execute a predetermined process. Note that the program executed by the controller 13 may be stored in a storage medium other than the memory 12.

[0029] Further, the memory 12 stores a map DB 10 including voxel data VD.

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

[0031] In addition to the map DB 10, the memory 12 stores information necessary for the processing executed by the in-vehicle unit 1 in this embodiment. For example, the memory 12 stores information used for setting the size of downsampling when performing downsampling on the point cloud data obtained when the lidar 2 performs one-cycle scanning. Also, the memory 12 stores information such as calculation formula information 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 in-vehicle unit 1. In this case, the controller 13 performs processing related to estimating the vehicle's own position by executing a program stored in the memory 12 and the like.

[0033] Further, the controller 13 functionally has a downsampling processing unit 14 and a vehicle's own position estimation unit 15. And 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", and a computer that executes a program.

[0034] The downsampling processing unit 14 generates point cloud data (also referred to as "processed point cloud data") that is corrected to reduce the number of measurement points by performing downsampling on the point cloud data output from the lidar 2. In this downsampling, the downsampling processing unit 14 generates processed point cloud data with a reduced number of measurement points by averaging within a space obtained by dividing the point cloud data into a predetermined size. Hereinafter, the size of the downsampling (corresponding to the above-described predetermined size) is also referred to as the "downsampling size DSS". Further, in the 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 "corresponding measurement point number Nc") associated with the voxel data VD among the processed point cloud data after downsampling falls within a predetermined target range. Hereinafter, the target range of the corresponding measurement point number Nc is also referred to as the "target range R" Nc is also referred to as

[0035] The host vehicle position estimation unit 15 estimates the host vehicle position by performing scan matching based on NDT (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 of downsampling size Next, the details of the processing of the downsampling processing unit 14 will be described. Generally, the downsampling processing unit 14 compares the corresponding measurement point number Nc with the target range R indicated by the target range information 9 after downsampling at each processing time, and sets the downsampling size at the next processing time according to the comparison result of their magnitudes. Thereby, the downsampling processing unit 14 adaptively determines the downsampling size so that the corresponding measurement point number Nc is maintained within the target range R Nc and sets the downsampling size at the next processing time according to the comparison result of their magnitudes. Thereby, the downsampling processing unit 14 adaptively determines the downsampling size so that the corresponding measurement point number Nc is maintained within the target range R Nc inside.

[0037] FIG. 3 shows an example of the functional blocks of the downsampling processing unit 14. Functionally, the downsampling processing unit 14 includes a downsampling block 16, a corresponding measurement point number acquisition block 17, a comparison block 18, and a downsampling size setting block 19. In FIG. 3, the blocks that exchange data are connected by solid lines, but the combinations of blocks that exchange data are not limited to those 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 performing downsampling on 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 the representative points of the measurement points of the point cloud data of the lidar 2 contained in each grid. In this case, the above space is, for example, a three-dimensional coordinate system space with the traveling direction, height direction, and lateral direction (i.e., the direction perpendicular to the traveling direction and the height direction) of the vehicle-mounted device 1 as axes, and grids are formed by dividing at equal intervals along these respective directions. The downsampling size is the initial size stored in the memory 12 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 host vehicle position estimation unit 15.

[0039] Note that the downsampling size may be different in the traveling direction, height direction, and lateral direction of the vehicle-mounted device 1, respectively. Also, any Voxel grid filter method may be used for the processing of the downsampling block 16.

[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 The upper limit is 800, and the target range is 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. NcIf it is less than the lower limit, the size of the downsampling is reduced. On the other hand, when the downsampling size setting block 19 has the corresponding number of measurement points Nc within the target range R Nc within the range, the size of the downsampling is maintained (i.e., not changed).

[0043] Also, FIG. 3 shows a specific aspect 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, when the corresponding number of measurement points Nc is more than the upper limit of the target range R Nc the gain is set to be greater than 1-fold, when the corresponding number of measurement points Nc is less than the lower limit of the target range R Nc the gain is set to be less than 1-fold, and when the corresponding number of measurement points Nc is within the target range R Nc the gain is set to 1-fold. In FIG. 3, the gain is set as follows as an example. When the corresponding number of measurement points Nc is more than the upper limit of the target range R Nc ⇒ gain "1.1", When the corresponding number of measurement points Nc is less than the lower limit of the target range R Nc ⇒ gain "1 / 1.1", When the corresponding number of measurement points Nc is within the target range R Nc ⇒ gain "1.0"

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

[0046] According to such a configuration, the downsampling size setting block 19 has the corresponding number of measurement points Nc within the target range R NcIf it is more than the upper limit, the size of the downsampling can be changed so that the size of the downsampling is increased and the number of measurement points of the processed point cloud data is likely to decrease. Similarly, when the corresponding number of measurement points Nc is less than the lower limit of the target range R Nc If it is less than the lower limit, the size of the downsampling can be changed so that the size of the downsampling is decreased and the number of measurement points of the processed point cloud data is likely to increase.

[0047] Note that each numerical value of the gain set in the gain setting sub-block 191 is an example, and appropriate numerical values are set based on experimental results and the like. Further, the downsampling size setting block 19 may perform addition or subtraction of a predetermined value instead of multiplying the gain. In this case, when the corresponding number of measurement points Nc is more than the upper limit of the target range R Nc If it is more than the upper limit, the size of the downsampling is increased by a positive predetermined value, and when the corresponding number of measurement points Nc is less than the lower limit of the target range R Nc If it is less than the lower limit, the size of the downsampling is decreased by a positive predetermined value. Note that the above gain and predetermined value may be set to different values for each direction (travel direction, lateral direction, height direction).

[0048] (4) Position estimation based on NDT scan matching Next, an explanation will be given regarding the position estimation based on the NDT scan matching executed by the host vehicle position estimation unit 15.

[0049] FIG. 4 is a diagram showing the vehicle position to be estimated by the host vehicle position estimation unit 15 in two-dimensional orthogonal coordinates. As shown in FIG. 4, the host vehicle position on the plane defined in the two-dimensional orthogonal coordinates of xy is represented by the coordinates "(x, y)" and the azimuth (yaw angle) "ψ" of the host vehicle. Here, the yaw angle ψ is defined as the angle formed by the traveling direction of the vehicle 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. Then, the host vehicle position estimation unit 15 performs host vehicle position estimation using these x, y, and ψ as estimation parameters. Note that, in addition to x, y, and ψ, the host vehicle position estimation unit 15 may perform host vehicle position estimation by further using at least any one of the height position, pitch angle, and roll angle of the vehicle in a three-dimensional orthogonal coordinate system as an estimation parameter.

[0050] Next, the voxel data VD used for NDT scan matching will be described. The voxel data VD includes data representing the measured point cloud data of the stationary structures in each voxel 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 information on parameters when expressing the point cloud in the voxel by a normal distribution. In the present embodiment, as shown in FIG. 5, it includes a voxel ID, voxel coordinates, a mean vector, and a covariance matrix.

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

[0053] The "mean vector" and the "covariance matrix" indicate the mean vector and the covariance matrix corresponding to the parameters when expressing the point cloud in the target voxel by a normal distribution. 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 Define it as such, and let the number of point clouds in voxel n be "N n ". Then, the average vector "μ n " and the covariance matrix "V n " of voxel n are represented by the following equations (1) and (2), respectively.

[0054] [Number]

[0055] [Number]

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

[0057] Scan matching by NDT assuming a vehicle estimates the estimation parameters P = [t x , t y , t ψ T Here, "t x " indicates the movement amount in the x direction, "t y " indicates the movement amount in the y direction, and "t ψ " indicates the yaw angle.

[0058] Also, if the coordinates of the point cloud data output by lidar 2 are X L (j) = [x(j), y(j), z(j)] T Then, the average value "L´ L " of X n (j) is represented by the following equation (3).

[0059] [Number] ​ Then, using the above-mentioned estimated parameter P, when the average value L´ is subjected to coordinate transformation, the transformed coordinate "L n " is represented by the following formula (4).

[0060] [Number] Then, the host vehicle position estimation unit 15 searches for voxel data VD associated with the processed point cloud data transformed into an absolute coordinate system (also referred to as the "world coordinate system") that is the same coordinate system as the map DB10, and the average vector μ n and the covariance matrix V n contained in the voxel data VD are used to calculate the evaluation function value (also referred to as the "individual evaluation function value") "E n " of the voxel n. In this case, the host vehicle position estimation unit 15 calculates the individual evaluation function value E n of the voxel n based on the following formula (5).

[0061] [Number]

[0062] Then, the host vehicle position estimation unit 15 calculates a comprehensive evaluation function value (also referred to as the "score value") "E(k)" for all voxels to be the object of matching, which is represented by the following formula (6). The score value E is an index indicating the degree of fitness of the matching.

[0063] [Number] After that, the host vehicle position estimation unit 15 calculates the estimated parameter P at which the score value E(k) becomes maximum by an arbitrary root-finding algorithm such as the Newton method. Then, the host vehicle position estimation unit 15 applies the estimated parameter P to the position (also referred to as the "DR position") "X DR (k)" calculated by dead reckoning at time k, and thereby obtains the host vehicle position (also referred to as the "NDT position") "X NDT(k) is calculated. Here, the DR position X DR (k) corresponds to the provisional vehicle position before calculating the estimated own vehicle position X^(k), and is also denoted as the predicted own vehicle position "X - (k)". In this case, the NDT position X NDT (k) is represented by the following formula (7).

[0064] [Number] After that, the own vehicle position estimation unit 15 calculates the final own vehicle position (also referred to as the "estimated own vehicle position") "X^(k)" at the current processing time k by fusing the DR position X DR (k) and the NDT position X NDT (k).

[0065] (5) Calculation of estimated own vehicle position Next, a method for calculating the estimated own vehicle position X^(k) will be described. Generally, the own vehicle position estimation unit 15 calculates the reliability value of the NDT scan matching at each processing time, and based on the weighting according to the reliability value, calculates the estimated own vehicle position X^ at the target processing time from the DR position X DR and the NDT position X NDT at the target processing time. Hereinafter, the reliability value of the NDT scan matching is also referred to as the "reliability value NRV (NDT Reliability Value)".

[0066] Note that the DR position X DR is an example of the "first candidate position" in the present invention, and the NDT position X NDT is an example of the "second candidate position" in the present invention. Also, the estimated own vehicle position X^ is an example of the "estimated position" in the present invention.

[0067] (5-1) Block configuration FIG. 6 is an example of the functional blocks of the host vehicle position estimation unit 15. As shown in FIG. 6, the host vehicle position estimation unit 15 includes a dead reckoning block 21, a DR position calculation block 22, a coordinate conversion 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 moving speed and angular velocity of the vehicle based on the outputs of the gyro sensor 3, the vehicle speed sensor 4, the GPS receiver 5, etc., and obtains the moving distance and azimuth change from the previous time. The DR position calculation block 22 calculates the DR position X ^ (k) at time k by adding the moving distance and azimuth change obtained by dead reckoning to the estimated host vehicle position X DR (k-1) at the previous processing time k-1. This DR position X DR (k) is the host vehicle position obtained at time k based on dead reckoning and corresponds to the predicted host vehicle position X - (k).

[0069] The coordinate conversion block 23 converts the downsampled processed point cloud data output from the downsampling processing unit 14 into the 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 based on, for example, the predicted host vehicle position output by the DR position calculation block 22 at time k. Instead of performing the above-described coordinate conversion on the downsampled processed point cloud data, the above-described coordinate conversion may be performed on the point cloud data before downsampling. In this case, the downsampling processing unit 14 generates the processed point cloud data in the world coordinate system obtained by downsampling the point cloud data in the world coordinate system after coordinate conversion. Note that the processing for converting the point cloud data in the coordinate system based on the lidar installed in the vehicle into the vehicle coordinate system and the processing for converting from the vehicle coordinate system to the world coordinate system are disclosed in, for example, International Publication WO2019 / 188745.

[0070] The point cloud data association block 24 performs the association between the machined point cloud data in the world coordinate system output by the coordinate transformation block 23 and the voxel data VD represented in the same world coordinate system. The NDT position calculation block 25 calculates the individual evaluation function values based on Equation (5) for each voxel associated with the machined point cloud data, and calculates the estimated parameter P that maximizes the score value E(k) based on Equation (6). Then, the NDT position calculation block 25 applies the estimated parameter P obtained at time k to the DR position X DR (k) output by the DR position calculation block 22 at time k to obtain the NDT position X NDT (k) at time k determined thereby. This NDT position X NDT (k) is the position of the host vehicle at time k obtained based on NDT scan matching.

[0071] The fusion block 26 calculates the estimated host vehicle position X DR at time k by fusing (integrating) the DR position X NDT (k) at time k and the NDT position X ^ (k) at time k. In this case, the fusion block 26 calculates the reliability value NRV of the NDT scan matching at time k, and calculates the estimated host vehicle position X DR by performing weighting based on the reliability value NRV on the DR position X NDT (k) and the NDT position X ^ (k). Details of the processing of the fusion block 26 will be described later.

[0072] Here, a simple example is used to supplement and explain the specific procedure for associating the measurement points with the voxel data VD.

[0073] FIG. 7 shows the positional relationship between voxels "Vo1" to "Vo6" where voxel data VD exists on the two-dimensional x-y plane in the world coordinate system and measurement points 61 to 65 at positions near these voxels. Here, for convenience of explanation, it is assumed that the z coordinate in the world coordinate system of the center positions of voxels Vo1 to Vo6 is the same as the z coordinate in the world coordinate system of measurement points 61 to 65.

[0074] First, the coordinate conversion block 23 converts the point cloud data including measurement points 61 to 65 into the world coordinate system. Then, the point cloud data association block 24 performs rounding processing such as the fractional part 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 1 m, the point cloud data association block 24 rounds off the decimal part of each of the x, y, and z coordinates of each measurement point 61 to 65.

[0075] Next, the point cloud data association block 24 determines the voxels corresponding to each measurement point 61 to 65 by collating the voxel data VD corresponding to the voxels Vo1 to Vo6 with the coordinates of each measurement point 61 to 65. In the example of FIG. 7, since the (x, y) coordinates of the measurement point 61 become (2, 1) by the above-mentioned rounding, the point cloud data association block 24 associates the measurement point 61 with the voxel Vo1. Similarly, since the (x, y) coordinates of the measurement point 62 and the measurement point 63 become (3, 2) by the above-mentioned rounding, the point cloud data association block 24 associates the measurement point 62 and the measurement point 63 with the voxel Vo5. Further, since the (x, y) coordinates of the measurement point 64 become (2, 3) by the above-mentioned rounding, the point cloud data association block 24 associates 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) by the above-mentioned rounding, the point cloud data association 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 points associated by the point cloud data association block 24 and the voxel data VD. In this example, although the number of measurement points after downsampling is 5, the number of corresponding measurement points Nc is 4. The number of corresponding measurement points Nc decreases when the space around the road has few and sparse structures, and also decreases when a phenomenon called occlusion occurs where other vehicles are present near the host vehicle and the light beam of the lidar is blocked.

[0076] (5-2) Details of the Processing of the Fusion Block First, a method for calculating the NRV representing the reliability of the 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 (i.e., the corresponding number of measurement points Nc) associated with the voxel data VD in the processed point cloud data after downsampling. Hereinafter, the ratio of the above-mentioned corresponding number of measurement points Nc is also referred to as the "data association ratio DAR (Data Association Ratio)". That is, the data association ratio DAR = (the corresponding number of measurement points Nc / the number of processed point cloud data after downsampling).

[0077] Specifically, as shown in the following formula (8), 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 data association ratio DAR at each time.

[0078] NRV = DSS × E × DAR (8) In addition, empirically, since the downsampling size DSS ranges from 0.1 to 5.0, the data association ratio DAR ranges from 0 to 1.0, and the score value E ranges from 0 to 3.0, the reliability value ranges from approximately 0 to 15.

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

[0080] Generally, when the number of point clouds in the point cloud data detected by the lidar 2 is large, calculations can be based on data with high density over a wide range, so the reliability of the NDT scan matching result is high. Therefore, when adopting control to adaptively change the downsampling size DSS to adjust the corresponding number of measurement points Nc, the larger the number of point clouds in the point cloud data, the larger the downsampling size DSS. Thus, as the first characteristic, the larger the downsampling size DSS, the higher the reliability of the NDT scan matching.

[0081] Also, the larger the association ratio DAR, the less occlusion by obstacles such as other vehicles around the structure that constitutes the voxel data VD, and it is presumed that the actual situation has not changed from the time when the voxel data VD was created. Therefore, as a second feature, the larger the association ratio DAR, the higher the reliability of the NDT scan matching.

[0082] Furthermore, the larger the score value E, the higher the probability that the calculated estimated parameter P is 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 by utilizing the downsampling size DSS, the association ratio DAR, and the score value E at each time of the NDT scan matching process. Also, in Equation (8), since the reliability value NRV is determined by these multiplications, when any of the downsampling size DSS, the association ratio DAR, and the score value E approaches 0, the reliability value NRV will also approach 0.

[0084] Next, the weighting by the reliability value NRV will be described. If the fusion block 26 sets the weight for the result of the NDT scan matching process as "w" (0 ≤ w ≤ 1), based on the following Equation (9), it performs a weighted average calculation of the DR position X DR and the NDT position X NDT to calculate the estimated ego vehicle position X^.

[0085]

Equation

[0086] In this embodiment, the fusion block 26 calculates an index (also referred to as the "NDT reliability index NRI (NDT Reliability Index)") that normalizes the reliability value NRV to a value range of 0 to 1, and uses the calculated NDT reliability index NRI as the weight w. That is, based on the following equation (10), the fusion block 26 uses the NDT reliability index NRI to perform a weighted average calculation between the DR position X DR and the NDT position X NDT to calculate the estimated own vehicle position X^.

[0087]

Equation

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

[0089]

Equation

[0090] Here, as the accuracy and stability of the NDT scan matching increase, the downsampling size DSS, the association ratio NDR, and the score value E take larger values. Therefore, when all of these are large values, the NDT reliability index NRI approaches 1. On the other hand, if even one of the downsampling size DSS, the association ratio NDR, and the score value E is a small value, the NDT reliability index NRI approaches 0 even if the other numerical values are large.

[0091] Next, a supplementary explanation will be given for Expressions (11) to (14). As shown in FIG. 8, the rise of the NDT reliability index NRI with respect to the reliability value NRV is rapid in Expression (11), the rise in the first half is gentle in Expression (12), and the change in the NDT reliability index NRI with respect to the reliability value NRV is generally gentle in Expression (13). Also, Expression (14) has a slightly gentler rise than Expression (11), but shows the characteristic of approaching 1 for the NDT reliability index NRI most quickly. Thus, it can be said that there are differences in the sensitivity of the NDT reliability index NRI with respect to the reliability value NRV among the respective expressions. Which expression to use for calculating the NDT reliability index NRI from the reliability value NRV among Expressions (11) to (14) is determined in advance according to the reliability of dead reckoning. Note that, as the reliability of dead reckoning, for example, uneven sensitivity of the gyro sensor 3 and the performance values described in the catalog of each sensor used for dead reckoning may be taken into consideration.

[0092] (5-3) Technical effects Next, a supplementary explanation will be given for the technical effect of calculating the estimated own vehicle position X^ by weighted average calculation between the DR position X DR and the NDT position X NDT based on the reliability value NRV.

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

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

[0095] As shown in FIGS. 9(A) to 9(C), the number of measurement points of the point cloud data acquired by the lidar 2 depends not only on the measurement principle and the viewing angle of the lidar 2 but also on the environment around the host vehicle. For example, in the case of a dense surrounding space (see FIG. 9(A)), the number of measurement points of the point cloud data is large, but in the case of a sparse surrounding space (see FIG. 9(B)), the number of measurement points of the point cloud data is small. Further, when occlusion occurs due to other vehicles or the like (see FIG. 9(C)), the number Nc of corresponding measurement points available for NDT scan matching decreases. And in the example of FIG. 9(A), since the number Nc of corresponding measurement points available for NDT scan matching is sufficient, there is a high possibility of obtaining an optimal solution by NDT scan matching. However, in the examples of FIGS. 9(B) and 9(C), since the number of corresponding measurement points Nc is small, there is a possibility of falling into a local solution in NDT scan matching.

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

[0097] FIGS. 10(A) and 10(B) are graphs showing the error distributions of dead reckoning and NDT scan matching, respectively. Specifically, FIG. 10(A) shows the error distributions of dead reckoning and NDT scan matching when the number of point clouds of the point cloud data output by the lidar 2 is relatively large and the occlusion is relatively small. Further, FIG. 10(B) shows the error distributions of dead reckoning and NDT scan matching when the number of point clouds of the point cloud data output by the lidar 2 is too small or the occlusion is relatively large. Also, in FIGS. 10(A) and 10(B), for simplicity, the error distributions with respect to one element constituting the estimated position (including the attitude) are shown, and the DR position X DR , the NDT position X NDT , and the estimated host vehicle position X^ are indicated by arrows, respectively.

[0098] When the number of points in the point cloud data output by the lidar 2 is large and occlusion is small, the NDT scan matching is calculated stably. Therefore, the error distribution of the NDT scan matching shows characteristics of a normal distribution. Generally, since the measurement accuracy of the lidar 2 is high, in this case, as shown in Fig. 10(A), compared with the error distribution of dead reckoning, the error distribution of the NDT scan matching has a narrow and steep tail characteristic. Therefore, in this case, the calculated result of the ego vehicle position by the NDT scan matching is a sufficiently reliable result. Thus, since the reliability value NRV is large and the NDT reliability index NRI is close to 1, the estimated ego vehicle position X^ by the weighted average of Equation (10) is close to X NDT close to it.

[0099] On the other hand, when the number of points in the point cloud data output by the lidar 2 is small or occlusion occurs, errors will be included in the calculation of the NDT scan matching. If there are many such errors, as shown in Fig. 10(B), it may have a smooth characteristic with multiple peaks. Here, since the calculation of the NDT scan matching searches for the condition where the evaluation function (score value E) is maximized, the highest point of the mountain is calculated as the result. In the case of Fig. 10(B), since the right end of the distribution has the highest peak, it is calculated as the NDT position X NDT , and in such a case, the NDT position X NDT is not very reliable.

[0100] Taking the above into consideration, the fusion block 26 according to this embodiment calculates the estimated ego vehicle position X^ by the weighted average calculation between the DR position X DR and the NDT position X NDT . Thereby, when the reliability of the NDT scan matching is high and the NDT reliability index NRI is close to 1, a result close to the NDT position X NDT is calculated as the final estimated ego vehicle position X^ (fusion position) as shown in Fig. 10(A). On the other hand, when the reliability of the NDT scan matching is low and the NDT reliability index NRI is close to 0, as shown in Fig. 10(B), it is away from the NDT position X NDT and close to the DR position X DRIt will approach this. As a result, even when the reliability of NDT scan matching is low, it is possible to prevent deterioration in the accuracy of the final estimated own-vehicle position.

[0101] (6) Processing flow FIG. 11 is an example of a flowchart showing the procedure of processing related to own-vehicle position estimation executed by the controller 13 of the in-vehicle device 1. The controller 13 starts the processing of the flowchart in FIG. 11 when it is necessary to perform own-vehicle position estimation, such as when the power is turned on.

[0102] First, the controller 13 determines a predicted own-vehicle position based on the positioning result of the GPS receiver 5 (step S11). Then, the controller 13 determines whether or not it has acquired the point cloud data of the lidar 2 (step S12). And when the controller 13 cannot acquire the point cloud data of the lidar 2 (step S12; No), it continues to make the determination in step S12. Also, during the period when the point cloud data cannot be acquired, the controller 13 continues to determine the own-vehicle position based on the positioning result of the GPS receiver 5. Note that the controller 13 may determine the own-vehicle position based not only on the positioning result of the GPS receiver 5 but also on the output of any sensor other than the point cloud data.

[0103] Then, when the controller 13 has acquired the point cloud data of the lidar 2 (step S12; Yes), it performs dead reckoning from the moving speed and angular velocity of the vehicle detected based on the gyro sensor 3, the vehicle speed sensor 4, etc., and the previous estimated own-vehicle position, and calculates the DR position X DR as the predicted own-vehicle position (step S13). Then, the controller 13 performs downsampling on the point cloud data for one cycle scanned by the lidar 2 at the current processing time according to the current setting value of the downsampling size DSS (step S14).

[0104] Next, the controller 13 uses the DR position X DR as an initial value and performs NDT matching processing to obtain the NDT position X NDTCalculate it (step S15). In this case, the controller 13 performs processing for converting the processed point cloud data into data in the world coordinate system, and association between the processed point cloud data converted into the world coordinate system and the voxel in which the voxel data VD exists, and the like.

[0105] Next, the controller 13 calculates an association ratio DAR based on the association result between the processed point cloud data and the voxel data VD in the NDT matching process (step S16). Further, the controller 13 compares the corresponding measurement point number Nc with the target range R Nc to determine the downsampling size DSS in the next downsampling. Furthermore, the controller 13 obtains a score value E for the estimated parameter P obtained in the NDT matching process (step S17).

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

[0107] Then, the controller 13 calculates an estimated ego vehicle position X^ by weighted average calculation of the DR position X DR and the NDT position X NDT (step S20). Then, the controller 13 determines whether to end the ego vehicle position estimation process (step S21). Then, when the controller 13 determines to end the ego vehicle position estimation process (step S21; Yes), the process of the flowchart ends. On the other hand, when the controller 13 continues the ego vehicle position estimation process (step S21; No), the process returns to step S12 to estimate the ego vehicle position at the next processing time.

[0108] (7) Consideration based on experimental results Next, consider the experimental results regarding the above-described embodiments.

[0109] The applicant drove a vehicle equipped with two lidars with a horizontal viewing angle of 60 degrees and an operating frequency of 12 Hz (period 83.3 ms) in the front and two in the rear on a certain road section for medium distances, and matched the voxel data (ND map) created in advance for the road section with the point cloud data of the lidars obtained during driving to estimate the position of the host vehicle. Also, for accuracy evaluation, the positioning results of RTK-GPS were used as the correct position data.

[0110] Figures 12(A) to 12(F) and Figures 13(A) to 13(D) show the estimated host vehicle position results according to a comparative example in which the NDT position X NDT is defined as the estimated host vehicle position X^. Here, with respect to the positioning results of RTK-GPS, Figure 12(A) shows the error in the traveling direction, Figure 12(B) shows the error in the lateral direction, Figure 12(C) shows the error in the height direction, Figure 12(D) shows the error in the yaw angle, Figure 12(E) shows the number of measurement points of the point cloud data before downsampling, and Figure 12(F) shows the downsampling size respectively. Also, Figure 13(A) shows the number of measurement points of the processed point cloud data after downsampling, Figure 13(B) shows the corresponding number of measurement points Nc, Figure 13(C) shows the association ratio DAR, and Figure 13(D) shows the score value E(k) respectively. 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 of 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 traveling direction is large and the lines are generally thick, so it is presumed that NDT matching is not stably performed during this period. Also, since the number of measurement points before downsampling shown in Fig. 12(E) is not so large, the downsampling size DSS shown in Fig. 12(F) is relatively small and is 1 m or less. Also, the association ratio DAR in Fig. 13(C) is also low, and the score value E in Fig. 13(D) is also slightly small. That is, in the calculation of NDT scan matching in the comparative example, all of the downsampling size DSS, the association ratio DAR, and the score value E are relatively small, and it can be judged that the reliability is not so high. Therefore, it is presumed that many errors are included in the NDT matching result, and this is the cause of the large error in the traveling direction shown in Fig. 12(A).

[0112] Figs. 14(A) to 14(F) and Figs. 15(A) to 15(F) show the self-vehicle position estimation results according to an embodiment in which the estimated self-vehicle position X^ is determined by the fusion of the NDT position X NDT and the DR position X DR Here, with respect to the positioning result of RTK-GPS, 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 downsampling, respectively. Also, Fig. 15(A) shows the number of measurement points of the processed point cloud data after downsampling, Fig. 15(B) shows the corresponding number of measurement points Nc, Fig. 15(C) shows the association 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, respectively. Also, the RMSE value in the traveling direction is 0.150 m, the RMSE value in the lateral direction is 0.034 m, the RMSE value in the height direction is 0.022 m, and the RMSE value of the azimuth (yaw angle) is 0.171 degrees, which are good values compared with the comparative example.

[0113] Here, referring to the reliability value NRV shown in Fig. 15(E), the value is changing around approximately 1.0, and it cannot be said that the reliability of 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 it is not a value very close to 1. Based on this NDT reliability index NRI, the estimated ego-vehicle position X^ is calculated according to Equation (10). In this case, the error in the traveling direction shown in Fig. 14(A) is greatly improved compared to the comparative example, and other errors shown in Figs. 14(B) to 14(D) are also improved. Thus, it is inferred that the accuracy of ego-vehicle position estimation has been improved by the fusion of the NDT position X NDT and the DR position X DR .

[0114] In addition, the applicant conducted additional experiments using another type of lidar. In the additional experiments, the applicant drove a vehicle equipped with a lidar capable of measuring long distances, having a horizontal field of view of 360 degrees and an operating frequency of 10 Hz (period 100 ms), on a certain driving route, and matched the voxel data (ND map) created in advance for the driving route with the point cloud data of the lidar obtained during driving to estimate the ego-vehicle position. Also, the positioning result of RTK-GPS was used as the correct position data.

[0115] Figs. 16(A) to 16(F) and Figs. 17(A) to 17(D) show the NDT position X NDTShows the vehicle position estimation result according to the comparative example defined as the estimated host vehicle position \(\hat{X}\). Here, for the positioning result of RTK - GPS, 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, 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 downsampling size respectively. Also, Fig. 17(A) shows the number of measurement points of the processed point cloud data after downsampling, Fig. 17(B) shows the corresponding number of measurement points \(N_c\), Fig. 17(C) shows the association ratio DAR, and Fig. 17(D) shows the score value \(E(k)\) respectively. Also, the RMSE value in the traveling direction is 0.031 m, the RMSE value in the lateral direction is 0.029 m, the RMSE value in the height direction is 0.027 m, and the RMSE value of the azimuth (yaw angle) is 0.051 degrees.

[0116] Since the lidar used in this experiment is of the long - distance and full - surrounding type, the number of measurement points before downsampling shown in Fig. 16(E) is in the tens of thousands, which is a sufficiently large amount of data. Therefore, the downsampling size DSS shown in Fig. 16(F) is a relatively large value compared to the experiments in Figs. 12 - 15. Also, the association ratio DAR decreases around time 120 s and 220 s. This is because this is the location where the road passes over the overpass and the amount of map information is small. Outside that location, the association ratio DAR and the score value are good values. 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] Figs. 18(A) - 18(F) and Figs. 19(A) - 19(F) show the NDT position \(X\) NDT and the DR position \(X\) DRThe estimated ego-vehicle position results according to an embodiment for determining the estimated ego-vehicle position X^ by fusion with [something not specified in the original] are shown. Here, with respect to the positioning results of RTK-GPS, Figure 18(A) shows the error in the traveling direction, Figure 18(B) shows the error in the lateral direction, Figure 18(C) shows the error in the height direction, Figure 18(D) shows the error in the yaw angle, Figure 18(E) shows the number of measurement points of the point cloud data before downsampling, and Figure 18(F) shows the size of downsampling respectively. Also, Figure 19(A) shows the number of measurement points of the processed point cloud data after downsampling, Figure 19(B) shows the corresponding number of measurement points Nc, Figure 19(C) shows the association ratio DAR, Figure 19(D) shows the score value E(k), Figure 19(E) shows the reliability value NRV, and Figure 19(F) shows the NDT reliability index NRI respectively. Also, the RMSE value in the traveling direction is 0.029 m, the RMSE value in the lateral direction is 0.027 m, the RMSE value in the height direction is 0.024 m, and the RMSE value of the azimuth (yaw angle) is 0.050 degrees, which is a slightly better value than the comparative example.

[0118] Here, the reliability value shown in Figure 19(E) decreases near 120 s and 220 s, and it can be said that the reliability of NDT scan matching decreases during that period. Also, Figure 19(F) corresponds to the NDT reliability index NRI calculated by Equation (11) using the reliability value NRV shown in Figure 19(E), and it can be seen that it shows a value close to 1 except near 120 s and 220 s. The estimated ego-vehicle position X^ is calculated based on Equation (10) using this NDT reliability index NRI. In this case, as shown in Figures 18(A) to 18(D), the error near 220 s is improved. Thus, also in this experimental result, it is speculated that the self-position estimation accuracy is improved by the fusion of the NDT position X NDT and the DR position X DR and.

[0119] (8) Modification example Hereinafter, modified examples suitable for the above-described embodiments will be described. The following modified examples may be applied to these embodiments in combination.

[0120] (Modified Example 1) The calculation of the NDT reliability index NRI may be performed by multiplying the reliability value NRV in formulas (11) to (14) by the coefficient "α".

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

[0122]

Number

[0123] And in this modified example, the coefficient a is set to a value corresponding to the measurement accuracy of the lidar 2 to be used. For example, when the accuracy of the distance measurement values and angle measurement values included in each measurement point of the lidar 2 is poor, an error is added to each of the measurement points of the point cloud data. In this case, since the NDT scan matching based on the point cloud data with a large error also results in an error, the coefficient a is set to a smaller value. As a result, the NDT reliability index NRI can be set to a smaller value for the same reliability value NRV.

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

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

[0126] Alternatively, an NDT reliability index NRI with a value range of 0 to 1 may be determined based on any algorithm other than formulas (11) to (18).

[0127] For example, the NDT reliability index NRI may be determined as follows. When NRV < 2.5, NRI = NRV / 2.5 When NRV ≥ 2.5, NRI = 1.0

[0128] In other examples, the NDT reliability index NRI may be determined by detailed case-by-case analysis as follows. When 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 2.5 ≤ NRV, NRI = 1.0

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

[0130] (Modification Example 2) The in-vehicle device 1 calculated the reliability value NRV based on the downsampling size DSS on the premise of performing downsampling. Instead of this, even when the in-vehicle device 1 does not perform downsampling, the reliability value NRV may be calculated and used for determining the weight w of the NDT position X NDT .

[0131] In this case, the in-vehicle device 1 associates the point cloud data output by the rider 2 with the voxel data VD, performs NDT scan matching, calculates the score value E, and calculates the association ratio DAR. Then, the in-vehicle device 1 determines the reliability value NRV based on the calculated score value E and the association ratio DAR. The reliability value NRV in this case is, for example, the product of the score value E and the association ratio DAR. Then, the in-vehicle device 1 calculates the NDT reliability index NRI obtained by normalizing the reliability value NRV based on the embodiment or the modified example 1, and the DR position X DR and the NDT position X NDT to calculate the estimated ego vehicle position X^ by weighted average calculation of

[0132] Thus, even when downsampling is not performed, the in-vehicle device 1 can suitably calculate the estimated ego vehicle position X^.

[0133] (Modified Example 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, and may be a value defined by any formula having a positive correlation with the downsampling size DSS, the score value E, and the association ratio DAR, respectively. Further, 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. Also, 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 rider 2) increases, the reliability value NRV may be calculated using, for example, the value of (the number of measurement points before downsampling / 10,000) instead of the downsampling size DSS.

[0134] (Modified Example 4) The configuration of the driving support system shown in FIG. 1 is an example, and the configuration of the driving support system to which the present invention is applicable is not limited to the configuration shown in FIG. 1. For example, instead of having the in-vehicle device 1, the electronic control unit of the vehicle may execute the processing of the downsampling unit 14 and the own vehicle position estimation unit 15 of the in-vehicle device 1. In this case, the map DB 10 is stored, for example, in a storage unit in the vehicle or a server device that performs data communication with the vehicle, and the electronic control unit of the vehicle refers to this map DB 10 to execute own vehicle position estimation based on downsampling and NDT scan matching, etc.

[0135] (Modification Example 5) As shown in FIG. 5, the voxel data VD is not limited to a data structure including an average vector and a covariance matrix. For example, the voxel data VD may directly include the point cloud data measured by the measurement maintenance vehicle used when calculating the average vector and the covariance matrix.

[0136] As described above, the controller 13 of the in-vehicle device 1 according to the present embodiment acquires the DR position X DR which is the first candidate position of the vehicle. Further, the controller 13 acquires the NDT position X NDT which is the second candidate position of the vehicle determined based on NDT scan matching, which is the collation of the point cloud data output by the lidar 2, which is an external sensor provided in the vehicle, and the voxel data VD which is map data. Furthermore, the controller 13 calculates a reliability value NRV representing the reliability of the NDT scan matching. Then, the controller 13 determines the estimated own vehicle position X^ based on the DR position X DR , the NDT position X NDT , and the reliability value NRV. By doing so, the controller 13 can suitably reduce the decrease in position estimation accuracy even when there are few structures around or occlusion occurs.

[0137] In the above-described embodiments, the program can be stored using various types of non-transitory computer readable media and supplied to a controller or the like which is a computer. The non-transitory computer readable media include various types of tangible storage media. Examples of the non-transitory computer readable media 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 memories (e.g., mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (Random Access Memory)).

[0138] As described above, the present invention has been described with reference to the embodiments, but the present invention is not limited to the above embodiments. Various changes 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. That is, the present invention naturally includes various modifications and corrections that those skilled in the art could make in accordance with the entire disclosure including the claims and the technical idea. Also, each disclosure of the above-cited patent documents and the like is incorporated herein by reference.

Description of Reference Numerals

[0139] 1 In-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 measuring device further includes an acquisition unit for acquiring a measurement accuracy of the measuring device, The reliability value calculation means calculates the reliability value using a coefficient corresponding to the measurement accuracy.

2. The information processing apparatus according to claim 1 , wherein the estimated position determining 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.

3. The information processing apparatus according to claim 2 , 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.

4. 4. 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.

5. 1. A computer-implemented 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; Obtaining the measurement accuracy of the measuring device; Calculating the reliability value using a coefficient corresponding to the measurement accuracy. method.

6. 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; Obtaining the measurement accuracy of the measuring device; A program that causes a computer to execute a process of calculating the reliability value using a coefficient according to the measurement accuracy.

7. A storage medium storing the program according to claim 6.

Images