JP2019040445A - Estimation device and program - Google Patents

Abstract

To provide an estimation device capable of performing voxel-based matching between a reference point group (map point group) and a query point group (sensor point group), which is required in self-position estimation processing, etc., even in situations where there may be differences in voxel division positions.SOLUTION: The estimation device is provided with a first division unit 13 configured to apply a plurality of division methods in which voxel division positions are different from each other, including differences on a sub voxel basis, with respect to a query point group, a first modeling unit 14 configured to obtain query point group modeling data obtained by modeling query point groups in voxels for each of the plurality of division methods applied, and a comparison unit 17 configured to obtain query point group modeling data in division methods determined to be similar, by comparing reference point group modeling data obtained by applying a predetermined voxel division method to a reference point group and modeling query point groups in voxels with query point group modeling data for each of the division methods.SELECTED DRAWING: Figure 2

Description

  In the present invention, matching based on voxels between a reference point group (map point group) and a query point group (sensor point group), which are necessary in self-position estimation processing, is performed even in a situation where there is a difference in voxel division positions. The present invention relates to an estimation device and a program that can be used.

  In recent years, with the commercialization of various types of distance sensors (for example, LiDAR, time-of-flight camera, structured light), 3D point clouds have attracted attention. 3D point clouds are used in various technical areas of computer vision, such as object recognition, area segmentation, self-position estimation, and change detection.

  The self-position estimation technology using the 3D point cloud is based on a point cloud (hereinafter referred to as “map point cloud”) obtained by measuring a specific range in the real world (field) with a sensor in advance, and the user This is realized by alignment with a point cloud (hereinafter referred to as “sensor point cloud”) acquired using a sensor in the field. For example, as a typical self-position estimation method, there is Iterative Closest Point (ICP) in Non-Patent Document 1.

  Here, since distance sensors in recent years measure hundreds of thousands of 3D points in one second, the amount of map point data generated through a wide range of measurements in the real world is enormous. For this reason, in order to reduce the required storage amount and / or improve the efficiency of processing using the point group, the data amount of the point group may be reduced.

  One method is downsampling of point clouds. This method reduces the amount of data by, for example, acquiring a specified number of points randomly from a given point cloud. However, since this method simply makes the data sparse, as the amount of data is reduced, there is a higher possibility that the result of the subsequent processing will be adversely affected (for example, the alignment accuracy is reduced).

Another method for reducing the amount of data is the modeling of point clouds. In this method, for example, a point cloud is modeled using a probability density model such as a multivariate normal distribution, and the original data is expressed as a set of model parameters, thereby reducing the data amount.
Many 3D point cloud data modeling techniques rely on voxel grids. In Normal Distribution Transform (NDT) in Non-Patent Document 2, after a space is divided by voxels, a point group located in each voxel is expressed by parameters of a three-dimensional multivariate normal distribution, that is, an average and a variance covariance matrix. Then, self-position estimation is realized using the modeled map point group and sensor point group.

Non-Patent Document 1 P. Besl and N. McKay, "A Method for Registration of 3-D Shapes," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, pp. 239-256, 1992. Magnusson, Martin, Achim Lilienthal, and Tom Duckett. "Scan registration for autonomous mining vehicles using 3D‐NDT.” Journal of Field Robotics 24.10 (2007): 803-827.

  However, reduction of the point cloud data amount of the conventional method as described above has the following problems. First, the modeling method as in Non-Patent Document 2 has a problem of quantization error.

  In other words, dividing a 3D point group whose continuous geometric shape has been measured at the time of modeling with voxels brings about a problem due to data voxel quantization errors. In order to explain the voxel quantization error of data, a schematic diagram is shown in FIG. In FIG. 1, an ellipsoid schematically represents data distribution of a certain point group PG1, and cubes V11, V12, V21, and V22 formed by straight lines represent voxels. Here, the coordinates of the point cloud PG1 do not change between [1] and [2] in FIG. 1 (that is, the point cloud PG1 data is the same in [1] and [2]. )) Only the coordinates of the reference point which becomes the reference for forming the voxel are changed as the reference point R1 and the reference point R2. That is, FIG. 1 shows an example in which the same point group PG1 data is quantized with different voxels in [1] and [2], and the same point group PG1 data is given by a common coordinate system CS1. In [1], a reference point R1 is provided at a position that coincides with the origin O and voxel quantization is performed. In [2], a reference point R2 is provided at a position shifted by a certain distance in the + x direction from the origin O to provide a voxel quantum. It has become.

  The point group PG1 data voxel quantized in [1] in FIG. 1 is allocated in a manner of being roughly divided into two parts across the boundary between the two voxels V11 and V12, whereas FIG. In [2] voxel quantization, all of the same point group PG1 data is assigned only to the left voxel V21 of the two voxels V21 and V22. As a result, even though the same point group PG1 data is modeled between [1] and [2] in Fig. 1, the modeling results (for example, mean and variance-covariance matrix) vary greatly. Therefore, the parameters of the obtained model are not similar.

  In FIG. 1, as a schematic example, a case where the grid direction of voxels is common between [1] and [2] but only the reference point position is different is shown. More generally, in the voxel quantization in which the reference point position and / or the voxel grid direction are different from each other, a quantization error occurs as in the schematic example of FIG. .

  As a simple technique for reducing the voxel quantization error as described above, it is conceivable to use voxels with fine resolution. That is, if a sufficiently small voxel is used compared to the scale of the object measured by the sensor, voxel quantization errors can be avoided for most points. However, when the resolution of the voxel is fine, the amount of data becomes larger than when the resolution is coarse. In addition, the amount of calculation and memory usage of the subsequent processing (self-position estimation processing) often increase accordingly. In particular, even with downsampled point cloud and modeled point cloud data, considering that the amount of data for point clouds obtained from a wide range of measurements is enormous, the voxel resolution is simply It is not preferable to make the method fine.

  Furthermore, there is generally a trade-off relationship between the data downsampling rate and the accuracy of the subsequent processing (self-position estimation processing), and the data can be downsampled by allowing a decrease in the accuracy of the subsequent processing due to the tradeoff relationship. Even when applied, the amount of data of a map point cloud composed of many 3D points is still large. Here, in addition to the large number of 3D points themselves, the number of parameters for each voxel based on the modeling of the method of Non-Patent Document 2 is also large, so the data amount is still large.

  As described above, in the conventional method, it is difficult to achieve a reduction in the amount of data and an improvement in the accuracy of the estimation process at the same time in the self-position estimation process because it is not possible to take measures against the quantization error mainly due to the difference in the voxel division position.

  In view of the problem, the present invention has a difference in the voxel division position for matching based on the voxel between the reference point group (map point group) and the query point group (sensor point group) required in the self-position estimation process or the like. It is an object of the present invention to provide an estimation device that can be used even in a situation.

  In order to achieve the above object, the present invention is an estimation device, and a first dividing unit that applies a plurality of division methods including different sub-voxel units and different voxel division positions to a query point group, A first modeling unit that obtains query point cloud modeling data obtained by modeling a query point cloud in a voxel for each of a plurality of division methods to be applied, and a predetermined voxel partitioning method applied to the reference point cloud and within the voxel The reference point cloud modeling data obtained by modeling the query point cloud of the above and the query point cloud modeling data for each of the series of division methods are compared, and the query points in the division method determined to be similar And a comparison unit for obtaining group modeling data. Further, the present invention is a program that causes a computer to function as the estimation device.

  According to the present invention, matching based on voxels between a reference point group (map point group) and a query point group (sensor point group) required in self-position estimation processing or the like may cause a difference in voxel division position. Is also possible.

It is a figure which shows the model example for demonstrating the voxel quantization error of data. It is a functional block diagram of the estimation apparatus and modeling apparatus which concern on one Embodiment. It is a figure for demonstrating typically the reduction | restoration of a voxel quantization error and the exclusion of the influence of the error of an initial position attitude | position being implement | achieved. It is a flowchart of operation | movement of the modeling apparatus and estimation apparatus which concern on one Embodiment. It is a figure which shows the schematic example of the relationship between a voxel and a block. It is a figure which shows the model example of the condition which acquired the surrounding block in addition to the nearest neighbor block. It is a figure which shows the example of a model for demonstrating the vector which a neighborhood acquisition part calculates. It is a figure which shows the model example of map side space, the search space of an initially set, and the updated search space. It is a figure which shows the schematic example of the search process which will be implement | achieved as a result by the comparison part of a back | latter stage by the said procedure as a schematic example for demonstrating the meaning of (procedure 3-2) of a 1st division part. .

  Hereinafter, embodiments of the present invention will be described with reference to the drawings. Note that each item described as a “special note” in the following description is for describing a preferable item from a certain point of view in a certain embodiment and / or for alerting the explanation, and is not necessarily essential to the present invention. It is not a statement.

  FIG. 2 is a functional block diagram of the estimation device and the modeling device according to an embodiment. As illustrated, the modeling device 20 includes a second acquisition unit 21, a second alignment unit 22, a second division unit 23, a second modeling unit 24, a second quantization unit 25, an output unit 32, and a storage unit 33. And each piece of data is processed in the order shown in the figure with respect to the map point cloud data. The estimation apparatus 10 includes a first acquisition unit 11, a first alignment unit 12, a first division unit 13, a first modeling unit 14, a first quantization unit 15, and a first generation unit 16, and includes sensor point cloud data. Each data is processed in the order shown. Furthermore, the estimation device 10 includes a second generation unit 26, a comparison unit 17, an estimation unit 18, an initial position and orientation acquisition unit 31, and a neighborhood acquisition unit 34.

  In the following, first, as a general description, the flow of data exchange between the functional blocks in FIG. 2 will be described. Detailed processing contents when each functional block exchanges the data will be described later.

  In FIG. 2, for example, the first acquisition unit 11 and the second acquisition unit 21 are different in function block names from each other only in the first part “first” and “second”. Corresponding processing is performed on different data.

  For example, the first acquisition unit 11 acquires the sensor point group and outputs it to the first alignment unit 12, and the second acquisition unit 21 acquires the map point group and outputs it to the second alignment unit 22.

  In the following, for simplification of description, the same or corresponding processes as described above will be described in the notation of enclosing them in parentheses “” and writing them in slash symbols “/”. For example, the above content is expressed by the following notation.

  The “first acquisition unit 11 / second acquisition unit 12” acquires “sensor point group / map point group” and outputs them to the “first alignment unit 12 / second alignment unit 22”.

  According to the notation, the processes of the corresponding parts 12 to 16 and the parts 22 to 26 and the processes of the other parts in FIG. 2 are as follows as depicted in FIG.

  The “first alignment unit 12 / second alignment unit 22” is an “azimuth-aligned sensor point group / azimuth-aligned map point group” in which the “sensor point group / map point group” is aligned in a predetermined direction. Is output to “first dividing unit 13 / second dividing unit 23”.

  The “first segmentation unit 13 / second segmentation unit 23” obtains “a series of voxels and blocks obtained by performing voxel segmentation and block definition on the“ azimuth-aligned sensor point group / azimuth-aligned map point group ”. Orientation-aligned sensor point cloud under block / series of voxels and orientation-aligned map point cloud under block ”to“ first modeling part 14 / second modeling part 24 ” Output. Here, the voxel division and block definition in the second division unit 23 is only one type of “predetermined”, whereas the voxel division and block definition in the first division unit 13 is a plurality of “series”. It is noted that. Further, it is noted that the “series of voxel division and block definition” in the first division unit 13 is intended to eliminate the influence of the voxel quantization error as described in FIG. The specific effect exclusion is realized by functional blocks after the first dividing unit 13.

  “First Modeling Unit 14 / Second Modeling Unit 24” is “azimuthally aligned sensor point cloud under a series of voxels and blocks / a series of voxels and blocks. The “series of modeled sensor data / modeled map data” obtained by applying the modeling to the “map point cloud” is output to the “first quantizing unit 15 / second quantizing unit 25”.

  The “first quantizing unit 15 / second quantizing unit 25” is obtained by quantizing “a series of modeled sensor data / modeled map data”, “a series of quantized modeled sensor data / quantization”. The modeled map data ”is output to the“ first generation unit 16 / output unit 32 ”.

  The output unit 32 outputs data obtained by collecting the quantized modeled map data for each block defined by the second dividing unit 23 to the storage unit 33, and the storage unit 33 displays the data collected for each block. By storing it, it is provided for reference from the neighborhood acquisition unit 34. Here, the output unit 32 collects the data for each block in this way, in the self-position estimation process by the estimation device 10 to be described in detail later, a so-called “stage” in the previous stage of obtaining a precise self-position estimation result as a final result. It is noted that the result corresponding to “rough alignment” can be obtained in units of the block.

  The initial position and orientation acquisition unit 31 acquires the initial position and orientation of the estimation device 10 and outputs the initial position and orientation to the first alignment unit 12, the neighborhood acquisition unit 34, and the estimation unit 18. It is noted that the initial position and orientation to be acquired is for enabling the so-called “coarse alignment”. The neighborhood obtaining unit 34 obtains data (quantized modeled map data) of one or more blocks corresponding to the initial position and orientation with reference to the storage unit 33, and then obtains the obtained data from neighboring map data. To the second generation unit 26. The neighborhood obtaining unit 34 also calculates a vector corresponding to the difference between the obtained position of the neighboring map data and the position of the initial position and orientation, and outputs the vector to the first dividing unit 13.

  The “first generation unit 16 / second generation unit 26” stores “a series of quantized modeled sensor data obtained from the first quantization unit 15 / neighboring map data obtained from the neighborhood acquisition unit 34” as data. A “series of sensor data strings / map data strings” is generated as a column representation and output to the comparison unit 17. It is noted that the data string generation is a process for enabling the comparison process in the comparison unit 17.

  The comparison unit 17 compares the output data sequence with the most similar to the map data sequence obtained from the second generation unit 26 among the series of sensor data sequences obtained from the first generation unit 16. The result of which sensor data string is is output to the estimation unit 18 as the most similar result. The estimation unit 18 outputs the position and orientation corresponding to the sensor data string as the most similar result as the final position and orientation estimation result of the estimation device 10.

  The processing outline of each unit in FIG. 2 has been described above regarding data exchange between the units. Here, in the position / orientation estimation result obtained as the most similar result in the comparison unit 17 and output from the estimation unit 18, the influence of the voxel quantization error as described in FIG. 1 is reduced, and the initial position is determined. It is noted that the position / orientation estimation result is obtained in consideration of the error of the initial position / orientation acquired by the attitude acquisition unit 34. Here, the process of estimating the initial position / posture error is realized by the proximity acquisition unit 34. In addition, in order to reduce the voxel quantization error, sensor data corresponding to a series (that is, generally a large number) of voxel segmentation results obtained by the first segmentation unit 13 is obtained as a neighborhood map obtained by the neighborhood acquisition unit 34. Although it is necessary to compare the data, the modeled data is further quantized and then compared in the form of a data string that has the same size as each other. Note that processing is possible.

  FIG. 3 is a diagram for schematically explaining that the above-described reduction of the voxel quantization error (and the exclusion of the influence of the error of the initial position and orientation) is realized, and schematically illustrates the 3D voxel obtained by space division. Is expressed in 2D. [1] shows a schematic example of map data MD (modeled after voxelization) corresponding to the initial position and orientation. The initial position and orientation does not include any error, and the voxel quantization error In a completely ideal situation in which there is no influence or the like, the sensor data SD (modeled after voxelization) acquired by the estimation device 10 should completely match the map data MD. However, as shown in [1], the actually obtained sensor data SD is in the vicinity of the map data MD, but the initial position and orientation error and the voxel quantization error (as shown in the schematic example in FIG. It is affected by the displacement of the voxel division position in the map data and sensor data.

  Therefore, on the premise that such an influence exists in the estimation device 10, an exhaustive search for an assumed range in which the sensor data SD may fluctuate from the original map data MD due to an initial position and orientation error and a voxel quantization error is performed. Therefore, an approach is taken to find the one that is most similar to the original map data MD and therefore corresponds to the highly accurate position and orientation estimation result. In [2] of FIG. 3, a schematic example of the search range in consideration of the posture (rotation component) error in the initial position and posture error is divided by the first dividing unit 13 by a method different from the sensor data SD. It is shown as sensor data SD-R1, SD-R2, SD-R3. In [3] of FIG. 3, a schematic example of a search range in consideration of the voxel quantization error is obtained by dividing the sensor data SD by the first dividing unit 13 by a method different from the sensor data SD-R3 of [2]. -T1, SD-T2, SD-T3 (Note that the search range in consideration of the voxel quantization error is actually set by a sub-voxel unit delimiter. In FIG. 3 [3], a delimiter larger than the sub-voxel unit is used as an emphasis expression for convenience of illustration. (Sensor data SD-T1, SD-T2, SD-T3 are drawn with fluctuation.)

  As will be described in detail later, the first dividing unit 13 actually performs a series of voxel divisions that comprehensively combine the error effects of [2] and [3] in FIG. (In the example shown in FIG. 3, [2] 3 types x [3] 3 types = total 9 types of divisions are made.) In addition, the position (translation component) error in the initial position and orientation error is considered. Is actually not considered by a series of voxel divisions by the first division unit 13, but by the range setting of the map data MD and sensor data SD. That is, the range setting is set assuming that translation component errors are also expected.

  Each of the series of voxel divisions that comprehensively combine the error effects performed by the first division unit 13 includes divisions corresponding to conversion parameters of various positions and orientations, as schematically shown in FIG. It has become. The position / orientation output from the estimation unit 18 of the estimation apparatus 10 is also the position / orientation corresponding to the voxel division determined to be the most similar result by the comparison unit 17.

  FIG. 4 is a flowchart of operations of the modeling device 20 and the estimation device 10 of FIG. 2 according to an embodiment. In step S1, the modeling device 20 models the map point group. The step S1 is realized in such a manner that the map data modeled in the storage unit 33 is stored by performing processing from the second acquisition unit 21 to the output unit 32 in the modeling device 20.

  In step S2, the estimation apparatus 10 estimates its current position and orientation in the modeled map obtained in step S1. As input data for the estimation, the sensor point group around the estimation device 10 acquired by the first acquisition unit 11, and the initial position and orientation of the estimation device 10 acquired by the initial position and orientation estimation unit 31, are used. Step S2 is realized by the operation of each unit of the estimation apparatus 10. Step S2 may be performed in real time at a predetermined rate to estimate the position and orientation of the estimation device 10 in real time. In this case, step S2 is executed at each time when the position and orientation is estimated.

  Note that the estimation device 10 may be configured to be included in an arbitrary moving body such as a vehicle or a drone, and may estimate the position and orientation of the moving body at each time. It may be configured to be provided in a device that does not have a moving unit, and the position and orientation at each time of the device that moves by a separate moving unit may be estimated. The range of the map point group to be modeled in step S1 may be a predetermined range in which the position and orientation are assumed to be estimated in step S2.

  In the following, the details of the operation of each unit in FIG. 2 will be described. However, in the description of the overview, it has been described in parallel as assuming the same or corresponding processing as “first acquisition unit 11 / second acquisition unit 12”. The functional part will be described using the notation used in parallel as appropriate.

<First acquisition unit 11 and second acquisition unit 12>
The “first acquisition unit 11 / second acquisition unit 12” acquires “sensor point group / map point group”. As a specific acquisition method, an existing method such as the aforementioned LiDAR can be used.

<Initial position and orientation acquisition unit 31>
The initial position and orientation acquisition unit 31 acquires the initial position and orientation of the estimation apparatus 10. The acquired initial position and orientation is information including a position parameter as a translation component and a rotation parameter as a rotation component. Specifically, for example, GNSS / INS (global positioning satellite system, inertial navigation system, or a combination of these systems) and estimation results using sensor points at the previous time (ie, estimation results by the estimation unit 18) , Dead reckoning, or manual input. You may acquire using another method.

<First alignment portion 12 and second alignment portion 22>
The 3D point cloud constituting the “sensor point cloud / map point cloud” is generally expressed by an orthogonal coordinate system of (x, y, z). In the “first alignment unit 12 / second alignment unit 22”, a local coordinate system of a given point group (that is, “sensor point group / map point group”) is set to an arbitrary predetermined orientation in the designated real world. Convert to match. For example, the local coordinate system representing the point group may be specified so that the x-axis coincides with 0 ° latitude and the y-axis coincides with 0 ° longitude.

  Here, the same orientation is designated in the second alignment unit 22 in the modeling device 20 responsible for the modeling process of the map point cloud and the first alignment unit 12 in the estimation device 10 responsible for the self-position estimation process. Special mention. Further, it is noted that the voxel division of the “first division unit 13 / second division unit 23” on the rear stage side is performed based on the designated orientation. The first dividing unit 13 performs voxel division after performing rotation conversion. It is noted that the first alignment unit 12 designates the orientation serving as a reference for the rotation conversion.

  Specifically, the first alignment unit 12 and the second alignment unit 22 may give the conversion parameters for specifying the direction as follows.

  In the second alignment unit 22 on the side responsible for the modeling process of the map point cloud, the conversion parameter may be manually specified by an administrator who prepares the map point cloud data, or the second acquisition unit 21 If you can use a GNSS / INS, geomagnetic sensor, or gyroscope when measuring a group, you may use them or other methods.

  On the other hand, in the first alignment unit 12 on the side responsible for the self-position estimation process, it is difficult to give it by manual designation when attempting to secure the processing speed of the real-time estimation. Therefore, if the GNSS / INS, geomagnetic sensor, and gyroscope are available in the first acquisition unit 11, they may be used, and when performing self-localization processing continuously, the posture at the previous time is used. An estimation result may be given as an initial position and orientation, and a rotation parameter included in the initial position and orientation may be used, dead reckoning may be used, or another method may be used.

<Second division part 23>
As a first process, the second dividing unit 23 uses a predetermined reference point (for example, the origin of the local coordinate system) that can be arbitrarily set, and a voxel having a predetermined size as a space in which a map point group with a specified orientation is distributed. To divide. Further, the specified number of voxels in the x, y, and z directions are divided by a rectangular parallelepiped. The rectangular parallelepiped is called a block. At this time, it is noted that the sides of the voxel and the block are aligned along the x axis, the y axis, and the z axis of the local coordinate system, respectively.

Here, the lengths of voxels in the x-axis direction, y-axis direction, and z-axis direction are vlx, vly, and vlz, respectively, and the numbers of voxels in the x-axis direction, y-axis direction, and z-axis direction included in the block are respectively vnx, vny, vnz. At this time, if the lengths of the block in the x-axis, y-axis, and z-axis directions are blx, bly, and blz, respectively, the following relationship holds:
blx = vlx * vnx (Formula 1)
bly = vly * vny (Formula 2)
blz = vlz * vnz (Formula 3)

  FIG. 5 is a diagram showing a schematic example of the relationship between a voxel and a block, and shows an example of one block and voxels included in this when vlx = vly = vlz = 1 and vnx = vny = vnz = 3. ing. FIG. 5 shows an example in which a dotted line represents a voxel, a solid line represents a block, and one block B is set to include 27 voxels.

  Further, as the second process, the second dividing unit 23 defines an ID that can specify the position of the voxel in the block.

For example, if it is defined that the voxel in the block is located at the voxel_idxth, voxel_idyth, and voxel_idzth in the x-axis direction, the y-axis direction, and the z-axis direction, the voxel V shown in gray in the schematic example of FIG. 5 is (voxel_idx, voxel_idy, voxel_idz) = (3,2,1). Alternatively, it can be represented by a single ID, for example using the following formula:
voxel_id = voxel_idx + vnx * voxel_idy + (vnx * vny) * voxel_idz (Formula 4)

  Further, the second dividing unit 23 calculates the center coordinates of each block as a third process. The central coordinates to be calculated give the position of each block in the local coordinate system, and are to be referred to when acquiring a block considered to correspond to the vicinity position by the vicinity acquisition unit 34 described later. Special mention.

<First modeling unit 14 and second modeling unit 24>
The “first modeling unit 14 / second modeling unit 24” represents a point group included in a voxel with a specific model.

  The first modeling unit 14 performs point group modeling in each voxel in a series of division methods divided by the first dividing unit 13 described in detail later, and the second modeling unit 24 performs the second dividing unit 24. Note that the point cloud modeling in the voxel is performed in only one voxel divided by. Since the processing for voxel division data by a series of division methods is the same in the first quantization unit 15 and the first generation unit 16 on the subsequent stage side, this point will be described in the description of each unit 15 and 16. The duplicate description about is omitted.

Here, assuming that N points are included in the voxel, p _i = (x _i , y _i , z _i ) (i = 1, 2,..., N). In one embodiment, as a model expression by the “first modeling unit 14 / second modeling unit 24”, a point group in a voxel is expressed as a coordinate average value (that is, a center of gravity) as in the following (Formula 5). It may be expressed as

  At this time, if the number of points assigned in the voxel is zero, the model calculation cannot be performed, and thus the modeling process is skipped. Alternatively, if the number of points assigned in a voxel is extremely small, there is actually no object and there may be noise, so the threshold for the number of points in the voxel is N_th, and N < In the case of N_th, the processing may be skipped. Note that the skipped voxel is handled by each processing unit on the subsequent stage on the assumption that no model data exists.

<First quantization unit 15 and second quantization unit 25>
The “first quantizing unit 15 / second quantizing unit 25” quantizes the model obtained by the “first modeling unit 14 / second modeling unit 24”. When the model is that of Formula 5, specifically, for example, the quantization may be performed as follows.

  Here, the coordinates of the reference point (reference point for voxel division) defined by “first division unit 13 / second division unit 23” is rp = (rpx, rpy, rpz). Remainders obtained by dividing the respective components obtained by subtracting the rp from the mean in Equation 5 by the voxel lengths vlx, vly, and vlz, respectively, are vsx, vsy, and vsz, respectively. When the values of vsx, vsy and vsz are negative, vlx, vly and vlz are added. Vsx, vsy, vsz obtained by such calculation correspond to the mean coordinates when the corner where all the coordinates in the voxel are the minimum is set as the origin, and the coordinates of the mean are relative to each other in the voxel. It corresponds to what is expressed as a position. Therefore, 0 ≦ vsx <vlx, 0 ≦ vsy <vly, and 0 ≦ vsz <vlz are always satisfied.

Next, the ranges [0, vlx), [0, vly), and [0, vlz) are divided into vqx, vqy, and vqz equally spaced ranges, respectively. And the value obtained by rounding the value obtained by multiplying vxx, vqy, vqz to the quotient (generally a real number in the range of 0 to 1) by dividing vsx, vsy, vsz by vlx, vly, vlz, respectively, to an integer value , Vlx, vly, vlz are assigned IDs, and they are used as mean_idx, mean_idy, mean_idz to obtain quantization results;
mean_idx = vsx / vlx * vqx (mean_idx = 0, 1,…, vqx) (Formula 6)
mean_idy = vsy / vly * vqy (mean_idy = 0, 1,…, vqy) (Formula 7)
mean_idz = vsz / vlz * vqz (mean_idz = 0, 1,…, vqz) (Formula 8)

Alternatively, for example, a quantization result expressed by a single ID may be obtained using the following equation.
mean_id = mean_idx + vqx * mean_idy + (vqx * vqy) * mean_idz (Formula 9)

<Output unit 32>
The output unit 32 outputs data obtained by modeling the given map point cloud (output data of the second quantization unit 25) in units of blocks (block units defined by the second division unit 23). . The data included in each block is the center coordinates of the block, voxel ID for each voxel (voxel_idx, voxel_idy, voxel_idz), and average ID for each voxel (mean_idx, mean_idy, mean_idz). Here, it is noted that the second modeling unit 42 calculation unit does not include information on voxels for which model calculation is skipped.

  As described above, the block unit data output from the output unit 32 is stored in the storage unit 32 and used for reference from the neighborhood acquisition unit 34.

<Nearby acquisition unit 34>
As a first process, the neighborhood acquisition unit 34 becomes the nearest neighbor when the coordinate position in the initial position and orientation acquired by the initial position and orientation acquisition unit 31 is compared with the center coordinates of each block stored in the storage unit 33. Block information is acquired from the storage unit 33.

  In addition, after obtaining the nearest block, in addition to that, the surrounding blocks may be obtained together. For example, the nearest block and 26 blocks located in the upper, lower, left, and right diagonal directions may be selected and acquired, or another method may be used.

  Here, it is desirable that the size of the selected block set is within the measurement range of the sensor that acquired the point cloud of the first acquisition unit 11 in consideration of the error amount of the initial position and orientation. . For example, when the measurement range on the sensor side is a radius of 100 [m], a block having a size of 1000 [m] × 1000 [m] × 1000 [m] is calculated from the map-side data stored in the storage unit 33. Suppose you select a set. At this time, if the data in the space of the same size is compared on the map side and the sensor side, most of the sensor side becomes a space where no data exists. (Specific comparison is made in the comparison unit 17 on the rear stage side.)

The neighborhood acquisition unit 34 regards the acquired block set as one block and updates the voxel ID included in each block as an additional process when acquiring the neighboring blocks in addition to the nearest block. For example, the specific update process may be performed by updating the same ID as the ID assigned by the second dividing unit 23 when the block set is treated as one block. FIG. 6 schematically represents a scene obtained by viewing the scene from which the nearest block and the surrounding 8 blocks on the xy plane are acquired from above (the positive direction of the z-axis). Here, the quadrangle represents a block, and each block is numbered for distinction. Here, the fifth block corresponds to the nearest block of the initial position and orientation. For example, if these nine blocks are regarded as one block and the voxel ID (voxel_idx, voxel_idy) defined as described in the example of FIG. 5 is updated, the update is performed as follows. Just do;
・ No. 1 block: No update ・ No. 2 block: add vnx to all voxel_idx ・ No. 3 block: add 2 * vnx to all voxel_idx ・ No. 4 block: add vny to all voxel_idy ・Block 5: add vnx to all voxel_idx, add vny to all voxel_idy Block 6: add 2 * vnx to all voxel_idx, vny to all voxel_idy Block 7: all voxel_idy 2 * vny is added to the 8th block: vnx is added to all the voxel_idx, 2 * vny is added to all the voxel_idy, and the 9th block is 2 * vnx to all the voxel_idx, 2 * vny to all the voxel_idy In this example, no block is added in the z-axis direction, but voxel_idz can be updated in the same manner when adding.

Further, as a second process, the neighborhood acquisition unit 34 calculates a vector d having a position parameter of the initial position and orientation as a start point and a nearest block center (referred to as MBC _NN ) as an end point as shown in a schematic example in FIG. To do. As shown in FIG. 2, the calculated vector d is used by the first dividing unit 13. FIG. 7 schematically represents a scene of a crossroad as a schematic example of map data contents viewed from above (positive direction of the z-axis). The black solid line represents the road constituting the crossroads, and the black square points represent the position IP of the initial position and orientation. A bold block means the nearest block B in the modeled map data, a black circle means a block center MBC _NN, and a black dotted line means a voxel constituting the block B.

Note that “deemed SBC _NN ” in FIG. 7 is for explaining the first division unit 13 that follows.

<First division 13>
The first dividing unit 13 (Procedure 1) gives various posture transformations to the sensor point group whose direction is defined by the first aligning unit 12, and then (Procedure 2) adds the vector d (described above) to the initial position / posture position. As described above, the coordinate obtained by adding (obtained from the neighborhood obtaining unit 34) is regarded as the nearest block center point (referred to as SBC _NN ) on the sensor side, and (procedure 3) the space is divided by voxels. A schematic example of the considered nearest block center point SBC _NN is as shown in FIG.

  At this time, it is noted that the point group is actually converted only by the rotational transformation in (Procedure 1), and the translational transformation in (Procedure 2) is a virtual transformation. That is, the translational conversion in (Procedure 2) is for reference as a reference position for voxel division in (Procedure 3). Note that only the sensor side (first dividing unit 13) performs posture conversion, and the second dividing unit 23 as a corresponding functional unit does not perform posture conversion on the map side.

With regard to (Procedure 3), the resolution of the voxel with respect to the sensor point group is matched with the resolution of the voxel obtained by dividing the map point group. That is, the voxel size divided by the first dividing unit 13 is the same as the voxel size divided by the second dividing unit 23 with respect to the map data. The space divided by voxels is a single block or block set acquired by the neighborhood acquisition unit 34 (in the case of a block set, the data format is the same as that of a single block as described in the schematic example of FIG. 6). It is limited to a range (referred to as S _s ) in which an arbitrary search range is added to a space of the same size (referred to as S _m ).

FIG. 8 is a diagram illustrating a schematic example of the space S _m and the range S _s in (Procedure 3) corresponding to the example of FIG. Block B in FIG. 7 corresponds to space S _m in FIG.

  Hereinafter, regarding (Procedure 1) and (Procedure 3), the significance thereof is as outlined with reference to FIG. 4, but what kind of specific processing is performed based on what consideration will be described.

<Consideration and specific processing of (Procedure 3)>
In an ideal situation where the initial position and orientation obtained from the initial position and orientation acquisition unit 31 does not include an error, the nearest block center MBC _NN as the map data position and the sensor side regarded as this in (procedure 2) Is the closest block center point SBC _NN . Therefore, when a space having the same size as the map side space S _m is acquired in the sensor side real world data under the matching ideal situation, the real world region included in the space is the same as that of the map side space S _m . Match. Therefore, if this space is modeled by the first modeling unit 14 on the rear stage side in the sensor side data, the model obtained is similar to that of the map side space S _m (the modeling result of the second modeling unit 24). There is a high possibility of doing.

However, in reality, the initial position and orientation includes an error. For example, it is said that GPS may contain an error of several meters to several tens of meters. Therefore, for example, if it is assumed that the initial position and orientation includes an error of 5 m at the maximum, the map side space S _m will be included unless a space obtained by adding ± 5 _{m to} the size of the map side space S _m is searched. You cannot find the same area as the world.

Therefore, when the maximum error of the initial position and orientation to assume E _max, the search space that extends across the same size area of the real world that are thought to correspond to the map side space S _m by the maximum error E _max space And this is called a search space S _s . The maximum error E _max may be given as a fixed value. For example, when information on the magnitude of the initial position and orientation error such as GPS positioning accuracy can be acquired, the maximum error E _max is dynamically set as a value at each time. You may change to For example, the search space S _s expanded only in the x-axis direction and the y-axis direction may be set and not expanded in the z-axis direction.

Furthermore, for convenience of performing a specific search process with a moving step width in units of voxel size, integers of vlx, vly, and vlz are added to corner points of arbitrary voxels in the search space S _s , respectively. Considering a series of voxel cuboids whose positions (positions like a series of grid points) are defined as vertices, calculate the voxel cuboid that forms the smallest space that encompasses the search space S _s , and Let it be a new search space S _vs (further updated to match the voxel grid position). A point where the x component, the y component, and the z component are the smallest in the search space S _vs is set as a reference point when the search space is divided by voxels, and is referred to as a reference point srp. Note that the setting of the reference point srp is not limited to this, an arbitrary predetermined point in the search space S _vs can be set, and the corner point of an arbitrary voxel in the search space S _s before update is directly used as the search space. The reference point srp in S _vs may be used.

The above map-side space S _m (in the real-world sensor-side space, an area considered to match the corresponding map-side space S _m on the assumption that there is no error in the initial position and orientation), the initially set search space S _s , update A schematic example of the search space S _vs is shown in FIG. Here, the boundary portion of the initial search space S _s does not necessarily match the voxel boundary indicated by the dotted line, whereas the updated search space S _vs has its boundary indicated by the voxel boundary (indicated by the dotted line in the figure). ).

From the above consideration, in (Procedure 3), specifically, voxel division is performed based on the set reference point srp in the updated search space S _vs. In order to reduce the influence of the quantization error as described in FIG. 1, the voxel division is based on a series of positions moved in the (x, y, z) directions by sub-voxel length based on the reference point srp. A series of voxel division is performed, details of which will be described later.

<Consideration and specific processing of (Procedure 1)>
The initial position and orientation may include an error in the rotation parameter. The sensor point group is set in a predetermined direction by being rotationally converted by the first alignment unit 12 based on the initial position and orientation, that is, the (x, y, z) axis is defined, but the initial position and orientation If there is an error in the rotation parameter, it is not possible to find the same area as the real world area included in the map side space S _m even if the position parameter does not include an error. This is because, as shown in a schematic example in [1] of FIG. 3, if the map side data and the sensor side data are divided into voxels in different forms, the model data obtained is also different.

  Therefore, in order to find a solution that eliminates the situation where the directions are different as shown in [1] of FIG. 3, the following processing is specifically performed in (Procedure 1).

That is, in (Procedure 1), a point group obtained by rotating a sensor point group with a specific pattern is generated, and these are further processed in (Procedure 2) and (Procedure 3). At this time, the sensor point group rotationally converted by the first aligning unit 12 is set as SPC _0, and the conversion of r_size is performed in (Procedure 1), and the generated point group is set as SPC _i (i = 1, 2, …, R_size). Here, it is noted that when the rotation process is performed on the sensor point group, the sensor point group is rotated around the origin (that is, the sensor position) of the local coordinate system of the sensor point group. After the rotation, the translation movement of (Procedure 2) is applied.

<Details of voxel division in (Procedure 3)>
The voxel division in (Procedure 3) is performed in the same manner for each of a series of SPC _i (i = 0, 1, 2,..., R_size) obtained in (Procedure 1). Specifically, The following (Procedure 3-1) and (Procedure 3-2) are as follows.

(Procedure 3-1)
For the target sensor point group SPC _i , the updated search space S _vs is divided into voxels based on the reference point srp. Here, in order to reduce the voxel quantization error (ie, in order to make it possible to find the reduced result in the comparison unit 17 on the subsequent stage), the parallel by the step width smaller than the resolution of the voxel is used. The movement is given to the reference point srp and the updated search space S _vs (that is, the position shifted from the reference point srp by the step width is set as the voxel division position in the search space S _vs ). The search space S _vs is divided by voxels.

Specifically, when the step widths in the x-axis direction, y-axis direction, and z-axis direction are set to rsx, rsy, and rsz, respectively, the number of quotient size patterns obtained by dividing vlx, vly, and vlz, respectively. Divide the updated search space S _vs by voxels with rsnx, rsny, rsnz;
rsnx = vlx / rsx (Formula 10)
rsny = vly / rsy (Formula 11)
rsnz = vlz / rsz (Formula 12)

For example, if vlx = 2 [m] and rsx = 0.2 [m], no conversion (when no translation of step width is given) is 1 pattern, 9 patterns giving translation in the X-axis direction in increments of rsx The updated search space S _vs is divided into voxels in a total of 10 patterns. When a similar pattern is given in the Y-axis direction and combined with the x-axis direction, a total of 100 patterns are obtained. When the same processing is performed in the Z-axis direction and combined, there are 1000 patterns in total. In the case of this example, some of the above patterns have a difference from the reference point for voxel quantization on the map side within 0.2 [m]. The pattern number rsnx * rsny * rsnz at this time is referred to as a reference point step number rs_size.

(Procedure 3-2)
First, the purpose of (Procedure 3-2) will be described. That is, as schematically shown in FIG. 9, the updated search space S _vs (corresponding to the sensor-side point group, which is a process that is eventually realized in the comparison unit 17 on the rear stage side. The search space S _vs moves within the map side space S _m within the total number of combinations of the method of voxel division in (Procedure 3-1) and the method of rotation transformation applied in (Procedure 1). This is because it is possible to perform the process of searching while searching and finding the most matching part without the need to “search while moving”. For this reason, in (Procedure 3-2), a series of search target blocks in the search space S _vs (in relation to the movement amount (Δx, Δy, Δz) in the search specified as the three-dimensional step width) ( A process of preparing in advance the same size as the map side space S _m is performed.

That is, in FIG. 9, two examples of search positions when searching the search space S _{vs. the} map side space S _m are shown as positions P1 and P2. At position P1, there is block B1 data as partial data in the search space S _vs , and the similarity to the map side space S _m data is obtained, and at position P2, block B2 data is obtained as partial data in the search space S _vs. Therefore, the similarity with the map side space S _m data is obtained. In (Procedure 3-2), such a process is performed in which such blocks B1, B2, etc. are individually extracted after associating information indicating that they are at positions P1, P2, etc. in the search space S _vs. Become.

In one embodiment of the present invention, as schematically shown in FIG. 9, the point cloud data on the sensor side forms a search space (searched “space”) S _vs , and the map side point cloud data Note that the map-side space S _m based is on the search “to do” side. (Note that the “search” of the map-side space S _m as a local region from within the entire map point cloud data that can generally be vast has already been completed using the initial position and orientation in the neighborhood acquisition unit 34.)

  In particular, the comparison unit 17, which will be described in detail later, performs processing for comparing the data strings of the same size between the map-side data and the sensor-side data to determine the similarity. It is the (Procedure 3-2) that makes it possible to prepare blocks of the same size that can be calculated according to the respective movement amounts as schematically shown in FIG. .

Therefore, specifically (procedure 3-2), voxel_id indicating the voxel position in the block is given to each voxel as reflecting the difference in the search position as schematically shown in FIG. As shown in FIG. 9, since the updated search space S _vs has a different size from the map side space S _m , if the updated search space S _vs is regarded as a single block, the positional relationship of the map side space S _m Inconsistent IDs are calculated. Therefore, a virtual block having the same size as the map side space S _m (block searched in FIG. 9) is applied, and an ID indicating the voxel position is given to each voxel for each virtual block. (Thus, even in the same voxel, IDs can be assigned as different relative positions from different virtual blocks. This corresponds to overlapping search ranges between virtual blocks.) Thus, the map side space S _{In order} to cover the entire range that can be covered by moving in the updated search space S _vs , the virtual block is translated in increments of vlx, vly, and vlz (in increments of voxel size), respectively. Give voxel_id to the voxels in the virtual block. In other words, the search by parallel movement is set to be performed by movement in units of voxel size. Also, the center coordinates of each block are calculated.

Here, the number of virtual block patterns designated by the positions of the blocks in the x-axis direction, the y-axis direction, and the z-axis direction matches the number of search patterns as shown in FIG. Specifically, the value obtained by subtracting the size of the map side space S _m from the size of the updated search space S _vs for each direction is added to the quotient when dividing by vlx, vly, and vlz, respectively. Value. For example, the updated search space S _vs is a cube with one side of 10 [m], the map side space S _m is a cube with one side of 5 [m], and vlx = vly = vlz = 1 [m] Then, the number of pattern positions of the blocks in the x-axis direction, the y-axis direction, and the z-axis direction to cover the entire range of the updated search space S _vs is six. In this case, a total of 216 patterns of virtual block information are obtained. Such a total number of patterns is called a voxel ID shifting number vs_size.

The designation of the position of the block in the x-axis direction, the y-axis direction, and the z-axis direction in the above is performed by specifying the position of the vector d, that is, the center block position of the search space S _vs (before expansion as shown in FIG. 8). By specifying the difference as a difference from the block position of the map-side space S _{m in} an ideal situation without errors, the difference between the position and orientation at the time of the self-position and posture estimation by the estimation unit 18 on the subsequent stage side is specified. Note that it can be used as information for correction (information of a part of the translation parameter).

<Summary of Splitting Process of First Splitting Unit 13 by (Procedure 1) to (Procedure 3)>
As a result of the comprehensive division as described above (procedure 1) to (procedure 3), finally, the first division unit 13 performs {(r_size + 1) * rs_size * for the given sensor point group. vs_size} block division results. In addition, a conversion ID corresponding to itself is associated with each obtained block. In particular, the conversion ID is associated with the details of the conversion method applied in (Procedure 1) and (Procedure 3).

  The conversion according to (Procedure 3-1) corresponds to the difference (step width) in the division position in sub-voxel units, and the translation corresponding to the difference in the division position from the reference point srp in the case of no conversion. It is noted that the conversion is linked as translational conversion when the voxel in the case of no conversion is matched with the voxel with the minimum absolute value movement width. Therefore, it is noted that each component of the translational transformation is configured with a size of 1/2 or less of the size of each side of the voxel.

Here, with respect to the reference point srp in the case of no conversion, the voxel common to the search space S _vs and the map side space S _m (the block considered to correspond to the center block) as shown in FIG. You may set as a vertex position. That is, when the voxel is divided in the situation shown in FIG. 8, the translation information by the step width may be specified by specifying the step width when the translation by the step width is zero (no conversion).

<First generator 16 and second generator 26>
As the first process, the “first generator 16 / second generator 26” generates a data string based on a set of voxel ID / average ID pairs in a given block. First, find the maximum number of voxels vn that the block may contain. When only the nearest block is acquired by the neighborhood acquisition unit 34, the following equation is obtained;
vn = vnx * vny * vnz (Formula 13)
Similarly, when the neighborhood obtaining unit 34 obtains the blocks together with the surrounding blocks, similarly, the maximum number of voxels vn that the block may contain when the blocks are regarded as one block is calculated. Blocks that do not reach the maximum number of voxels vn are modeled by the “first modeling unit 14 / second modeling unit 24” because the block size is not small, but there are few or no points in the voxel. Note that the processing includes skipped voxels.

  After that, as the second process, the “first generator 16 / second generator 26” generates a one-dimensional array of size vn, initializes it with a numerical value outside the range of the average ID, for example, −1, and then blocks A data string is obtained by substituting the average ID calculated from the voxel for the dimension corresponding to the voxel ID included therein. Here, the average ID remains unassigned to the dimension corresponding to the voxel for which the model was not calculated. As is clear from the setting method of the voxel ID, the dimension corresponding to the voxel ID included in the block means that the element of the one-dimensional array corresponding to the voxel position in the block (assigned in the element). Average ID). Referencing two-dimensional arrays of two different blocks, the fact that the average IDs of the same dimension (elements at the same position) are similar is that point cloud modeled data at the same position in the two blocks Means that they are similar. Accordingly, the fact that the one-dimensional arrays of two blocks are similar means that the overall arrangement of point groups included in the two blocks is similar. The similarity is digitized as a score in the comparison unit 17 described later.

  Further, in the second generation unit 26 that has obtained the data string on the sensor side, the conversion ID corresponding to the data string itself (the conversion ID linked last in the first dividing unit 13) is further linked.

<Comparison unit 17>
The comparison unit 17 includes a single data sequence on the map side obtained from the second generation unit 26 and {(r_size + 1) * rs_size * vs_size} data sequence groups obtained from the first generation unit 16 Calculate the similarity between. Here, it is noted that the corresponding conversion ID is associated with each data string on the sensor side.

  Specifically, for each dimension of the data string, it is determined whether the included elements, that is, the average ID value matches, and if it is right, the score is added to the conversion ID linked to the data string on the sensor side. To do. Here, when both elements are (−1) which means that the model is a voxel for which the model has not been calculated, the score may be added or may not be added. For efficient calculation, processing may be skipped for dimensions where the map element is (-1).

  Finally, the conversion IDs are sorted in descending order of similarity score. For efficiency of calculation, only the top k conversion IDs may be acquired.

<Estimation unit 18>
The estimation unit 18 receives the initial position and orientation obtained by the initial position and orientation acquisition unit 31 and the conversion ID obtained by the comparison unit 17 as the maximum similarity, and calculates the sensor's own position and orientation. That is, the sensor's own position and orientation obtained by measuring the sensor data acquired by the first acquisition unit 11 is calculated.

In this calculation, a position / orientation conversion parameter corresponding to the input conversion ID, that is, a translation parameter and a rotation parameter are acquired, and one attitude conversion matrix constituted by them is calculated. Here, the rotation parameter corresponds to the rotation conversion applied in (procedure 1) in the first dividing unit 13 corresponding to the conversion ID. Further, the translation parameter is a translation conversion that reflects the difference in division position from the reference point srp in the case of no conversion in (Procedure 3-1) in the first division unit 13 corresponding to the conversion ID (each of the above-described translation parameters). (Translational transformation in which the component is composed of 1/2 or less of the size of each side of the voxel) and the x-axis direction, y-axis direction, and z-axis direction in the voxel unit of (Procedure 3-2) the amount of movement corresponds to the translation transformation the translational transform given by (the amount of movement of the central block position S _m of the search space S _vs described above), was synthesized. Then, the result of applying the position / orientation transformation matrix to the initial position / orientation is output as a self-position / orientation. The multiplication of the position / orientation transformation matrix corresponds to correcting the initial position / orientation including an error. Further, in the calculation of the position / orientation transformation matrix, as in the procedure in the first dividing unit 13, the matrix may be calculated on the assumption that the rotational transformation is first performed and then the translational transformation is performed.

  As described above, according to the present invention, the position and orientation of the estimation device 10 can be obtained at high speed based on voxel-based modeling. Furthermore, the result obtained at high speed can be obtained by excluding the influence of the quantization error and the initial position / posture error in the voxel-based modeling.

  Hereinafter, various supplementary items (including other embodiments) in the present invention will be described.

(1) In the “first modeling unit 14 / second modeling unit 24”, in addition to the average position coordinate mean as described above, an arbitrary predetermined amount is used to model a point group in a voxel. Can be adopted. The model may be a single type or a combination of multiple types.

  For example, a standard deviation may be used as one of the models. The standard deviation σ of the 3D point cloud can be calculated by

  Then, by normalizing σ using its own L2 norm, the range of possible values of each component is [0, 1]. Therefore, by dividing each component into equal intervals, quantization can be performed in the same manner as the average.

  Further, when the 3D point group acquired by the sensor has luminance information in the corresponding pixel, luminance average may be used as one of the models. Since the luminance value acquired by the sensor is normally normalized to a specific range, for example, [0,255], it can be quantized in the same manner as the average.

  Further, when the 3D point group acquired by the sensor has color information in the corresponding pixel, a color average may be used as one of the models. Since the color information acquired by the sensor is usually normalized to a specific range, for example, [0,255] for each channel such as RGB, it can be quantized in the same manner as the average.

(2) The “first quantizing unit 15 / second quantizing unit 25” performs predetermined quantization corresponding to the model type obtained by the “first modeling unit 14 / second modeling unit 24”. You can do it.

(3) In the “first generation unit 16 / second generation unit 26”, a combination of IDs based on a plurality of models can be expressed by a unique ID that represents them. By using it as an element of the data string generated in the above, it is possible to perform the same processing as in the case of a single model having only the average coordinates mean. That is, in the process of substituting the average ID calculated from the voxel for the dimension corresponding to the voxel ID included in the block, the average ID is calculated based on a plurality of models and substituted. Good. At this time, an average ID composed of a plurality of models based on a plurality of models may be substituted into a dimension corresponding to the voxel ID.

(4) In one embodiment of the present invention, a continuous amount may be obtained by inverse quantization of a quantized model parameter of map data and used in data generation processing. At this time, the sensor side does not quantize the model parameter and uses a continuous amount. In the data string comparison processing, the distance between the corresponding continuous quantities (for example, error square) may be cumulatively added as the dissimilarity, and the data string having the smallest dissimilarity may be selected.

  That is, the present invention can be modified in such a manner that “first quantization unit 15 / second quantization unit 25” is skipped.

(5) In one embodiment of the present invention, the processes from the division process to the data string comparison process in the self-position estimation process may be performed repeatedly according to the coarse / fine strategy. That is, in the first processing, the first division unit 13 obtains a rough self-position estimation result using coarse rotation and coarse step width, and the search space S _sv is reduced based on the result. After that, the first division unit 13 in the second process may obtain a highly accurate self-position estimation result by using finer rotation and finer step width. As a result, there is a possibility that the total amount of calculation can be reduced as compared with the case where high-precision self-position estimation is performed by one process.

  (6) Although the present invention has been described by taking application to self-position estimation processing as an example, it is not limited to query point groups (those whose voxel division positions are undetermined) and map point groups that are not limited to specific sensor point groups. Targeted reference point cloud (already voxel-divided at a predetermined voxel division position and stored in the storage unit 33) realizes matching that absorbs indefiniteness of the voxel division position between the two point groups A modular application is also possible. When the modular application is performed, information corresponding to the initial position and orientation is given in advance, a query point group and a reference point group are prepared, and the same processing as described above may be performed. In particular, the exhaustive division process by the first division unit 13 automatically detects voxel division with reduced quantization error or the like in the voxel division of the query point group having indefiniteness relative to the voxel division of the general reference point group. Has an effect.

When applied modularly, the embodiment in the self-position estimation process may be simplified and applied as follows. That is, it is known that the initial position and orientation acquired by the first acquisition unit 11 has a predetermined accuracy (accuracy determined to be sufficient), and the data and neighborhood acquisition based on the sensor point group aligned by the first alignment unit 12 The comparison with the map point cloud data acquired by the unit 34 corresponds to an embodiment of a simplified self-position estimation process in the case where only the difference in the voxel division position needs to be dealt with. That is, in (Procedure 1) in the first dividing unit 13, the application of a plurality of rotation transformations is omitted and one sensor point group rotationally transformed in the first alignment unit 12 is used only for SPC ₀ (Procedure 3). In (Procedure 3-1), only the division according to the reference point step number rs_size is performed, and the division according to the voxel ID shifting number vs_size in (Procedure 3-2) is omitted. ) Block division may be performed in accordance with the reference point step number rs_size.

  Exactly, regardless of whether or not the specific application target of the present invention is self-position estimation, if it is known that the translational component of the initial position and orientation has a predetermined accuracy, (Step 3-2) ) Is omitted, the first division unit 13 has (r_size + 1) * rs_size blocks in total (r_size + 1) as in (Procedure 1) and rs_size as reference point steps in (Procedure 3-1). If it is known that the rotation component of the initial position and orientation has a predetermined accuracy, the first dividing unit 13 can omit the (procedure 1) by (procedure 3-1). Block division may be performed in accordance with rs_size * vs_size as a whole based on the number of reference point steps rs_size and the number of voxel ID shifting steps (step 3-2) vs_size.

Of the embodiments that are simplified from the above viewpoints, in the embodiment that omits the voxel ID shifting number vs_size in (Procedure 3-2), the space Ss and the space described in FIG. Note that no extension to Svs is required. However, in order to apply the division according to the reference point step number rs_size in (Procedure 3-1) to the space S _m (corresponding to the sensor side data), a predetermined range outside the space S _m (predetermined within the voxel width) Range) will be referenced.

  (7) The estimation device 10 and the modeling device 20 can be realized as a computer having a general configuration. That is, a CPU (Central Processing Unit), a main storage device that provides a work area for the CPU, an auxiliary storage device that can be configured with a hard disk, SSD, etc., an input interface that receives input from the user such as a keyboard, mouse, touch panel, etc., network The estimation device 10 and the modeling device 20 can be configured by a general computer including a communication interface for performing communication by connecting to a display, a display for displaying, a camera, and a bus for connecting them. Furthermore, the processing of each unit of the estimation device 10 and the modeling device 20 shown in FIG. 2 can be realized by a CPU that reads and executes a program for executing the processing, but any part of the processing is separately performed. It may be realized in a dedicated circuit or the like (including GPU).

DESCRIPTION OF SYMBOLS 10 ... Estimation apparatus, 11 ... First acquisition part, 12 ... First alignment part, 13 ... First division part, 14 ... First modeling part, 15 ... First quantization part, 16 ... First generation part, 17 ... comparison unit, 18 ... estimation unit, 26 ... second generation unit, 31 ... initial position and orientation acquisition unit, 34 ... neighbor acquisition unit
20 ... modeling device, 21 ... first acquisition unit, 22 ... second alignment unit, 23 ... second division unit, 24 ... second modeling unit, 25 ... second quantization unit, 32 ... output unit, 33 ... Memory

Claims (12)

A first division unit that applies a plurality of division methods having different voxel division positions including a difference in sub-voxel units for a query point group;
A first modeling unit that obtains query point group modeling data obtained by modeling a query point group in a voxel for each of the plurality of division methods applied;
Compare the reference point cloud modeling data obtained by applying a predetermined voxel division method to the reference point cloud and modeling the query point cloud in the voxel, and the query point cloud modeling data for each of the plurality of division methods And a comparison unit that obtains query point cloud modeling data in a division method determined to be similar.   The first division unit further performs a plurality of rotation transformations on the query point group, and a plurality of voxel division positions that are different from each other including the difference in the sub-voxel unit with respect to the query point group for each of the applied rotation transformations. The estimation apparatus according to claim 1, wherein the division method is applied. The reference point cloud modeling data is obtained for a reference block of a predetermined size in which voxels are aggregated,
The first modeling unit obtains the query point cloud modeling data for a query block in which voxels are gathered in a size larger than the predetermined size,
The first dividing unit further performs a plurality of translational movement conversions on a predetermined position range in the query block and occupies the size of the reference block, and in the destination range for each applied translational movement conversion The estimation apparatus according to claim 1, wherein a plurality of division methods including different sub-voxel units and different voxel positions are applied to a query point group. Furthermore, a first quantization unit that quantizes the query point cloud modeled data to obtain quantized query point cloud modeled data,
In the comparison unit, quantized reference point group modeling data obtained by applying a predetermined voxel dividing method to a reference point group and modeling and quantizing a query point group in the voxel, and the first quantization unit The quantized query point group modeled data for each of the plurality of segmentation methods obtained in step (1) is compared, and quantized query point group modeled data in a segmentation method determined to be similar is obtained. Item 4. The estimation device according to any one of Items 1 to 3. The query point cloud is acquired by a sensor,
The reference point group is acquired in a field to be measured by the sensor,
An initial position and orientation acquisition unit for acquiring an initial position and orientation when the sensor acquires the query point group;
When the sensor acquires the query point group as a result of correcting the initial position and orientation based on the division method of the query point cloud modeling data in the division method determined to be most similar by the comparison unit The estimation apparatus according to any one of claims 1 to 4, further comprising an estimation unit that estimates a position and orientation in the field.   A proximity acquisition unit that acquires the reference point group modeling data to be compared by the comparison unit as belonging to the vicinity of the initial position and orientation from a series of reference point groups in the field; The estimation apparatus according to claim 5. The reference point cloud modeling data is obtained by the neighborhood acquisition unit for a reference block of a predetermined size in which voxels are aggregated,
The first modeling unit obtains the query point cloud modeling data for a query block in which voxels are gathered in a size larger than the predetermined size,
The first dividing unit further performs a plurality of translational movement conversions on a predetermined position range in the query block and occupies the size of the reference block, and in the destination range for each applied translational movement conversion Applying a plurality of division methods with different voxel positions including a difference in the sub-voxel unit to the query point group,
In the estimation unit, it is assumed that the position in the initial position and orientation is corrected based on the division method determined to be the most similar by the comparison unit and the translational movement transformation of the query point cloud modeling data in the translational transformation. The estimation apparatus according to claim 6, wherein the position and orientation in the field when the sensor acquires the query point group is estimated. The first division unit further performs a plurality of rotation transformations on the query point group, and a plurality of voxel division positions that are different from each other including the difference in the sub-voxel unit with respect to the query point group for each of the applied rotation transformations. Applying the splitting method
In the estimation unit, the division method determined to be most similar by the comparison unit and the posture at the initial position and posture are corrected based on the rotation transformation of the query point cloud modeling data in the rotation transformation, The estimation apparatus according to claim 6, wherein a position and orientation in the field when the sensor acquires the query point group is estimated.   In the estimation unit, the position in the initial position and posture is corrected based on the voxel division position in the division method of the query point cloud modeling data in the division method determined to be most similar by the comparison unit, The estimation apparatus according to claim 6, wherein the position and orientation in the field when the sensor acquires the query point group is estimated.   In the first modeling unit, the query point cloud modeling data obtained by modeling the query point cloud in the voxel is converted into a coordinate average in the query point cloud, a coordinate standard deviation, a luminance average of pixels corresponding to the point cloud, and a point cloud. The estimation apparatus according to claim 1, wherein the estimation apparatus is obtained by including at least one of color averages of corresponding pixels.   11. The estimation apparatus according to claim 1, wherein the comparison unit compares the reference point cloud modeled data and the query point cloud modeled data after making the data strings of the same size.   A program that causes a computer to function as the estimation device according to any one of claims 1 to 11.