Position and orientation detection device

Abstract

The present invention provides a position and orientation detection device that can improve the accuracy of detecting the position and orientation of an object while reducing effort and cost. [Solution] The position and orientation detection device 10 includes a template storage unit 12 that stores data corresponding to the front surface 3a of the pallet 3 as a two-dimensional template T, a laser sensor 11 and a point cloud acquisition unit 21 that acquire three-dimensional point cloud data including the front surface 3a of the pallet 3, a matching unit 23 that matches the template T with the point cloud data, and a position and orientation estimation unit 24 that estimates the position and orientation of the pallet 3. The matching unit 23 translates and rotates the template T in three dimensions with respect to the point cloud data to find the position and orientation of the template T that best matches the point cloud data, and the position and orientation estimation unit 24 determines the position and orientation of the template T that best matches the point cloud data as the position and orientation of the pallet 3.

Description

Technical Field

[0001] The present invention relates to a position and orientation detection device.

Background Art

[0002] As a conventional position and orientation detection device, for example, the technique described in Patent Document 1 is known. The position and orientation detection device described in Patent Document 1 includes a laser sensor that detects the distance to a pallet to be handled, a camera that images the pallet to be handled, a pallet recognition unit that recognizes the pallet based on the image data of the camera, a data extraction unit that extracts measurement point data corresponding to the pallet from the measurement point data of the laser sensor, a plane equation calculation unit that calculates the plane equation of the front surface of the pallet based on the measurement point data corresponding to the pallet, and an estimation calculation unit that estimates the position and orientation of the pallet using the plane equation of the front surface of the pallet. The pallet recognition unit recognizes the pallet by an image processing technique using deep learning.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0004] However, in the method of recognizing a pallet using deep learning as in the above conventional technology, it is necessary to perform learning for each type of pallet. For this reason, a great deal of labor is required, and it may be time-consuming and costly.

[0005] An object of the present invention is to provide a position and orientation detection device that can improve the detection accuracy of the position and orientation of an object while reducing labor and costs.

Means for Solving the Problems

[0006] (1) One aspect of the present invention is a position and orientation detection device for detecting the position and orientation of an object, comprising: a template storage unit that stores data corresponding to the front surface of the object as a two-dimensional or three-dimensional template; a point cloud data acquisition unit that acquires three-dimensional point cloud data including the front surface of the object; a matching unit that matches the template stored in the template storage unit with the point cloud data acquired by the point cloud data acquisition unit; and a position and orientation estimation unit that estimates the position and orientation of the object according to the matching result by the matching unit, wherein the matching unit translates and / or rotates the template in three dimensions with respect to the point cloud data to determine the position and orientation of the template that best matches the template with respect to the point cloud data, and the position and orientation estimation unit determines the position and orientation of the template that best matches the template with respect to the point cloud data as the position and orientation of the object.

[0007] In such a position and orientation detection device, data corresponding to the front of an object is stored as a 2D or 3D template, and 3D point cloud data including the front of the object is acquired. Then, the template and point cloud data are matched, and the position and orientation of the object are estimated according to the matching result. Here, the template is translated and / or rotated in 3D relative to the point cloud data to find the position and orientation of the template that best matches the point cloud data. Then, the position and orientation of the template that best matches the point cloud data is determined as the position and orientation of the object. In this way, the position and orientation of the object are estimated using a template that has been pre-stored as data corresponding to the front of the object. For example, when estimating the position and orientation of an object using deep learning, a great deal of effort is required to create the training data. However, creating a template requires less effort than creating training data. This reduces effort and cost while improving the accuracy of detecting the position and orientation of the object.

[0008] (2) In (1) above, the template is gridded data, and each grid in the template contains a numerical value indicating whether or not the object exists as an object. The matching unit grids the point cloud data with the same grid size as the template, sets a numerical value in each grid of the point cloud data according to the presence or absence of a point cloud, then translates and / or rotates the template in three dimensions relative to the point cloud data, calculates a score representing the degree of agreement between the template and the point cloud data, finds the position and orientation of the template that yields the highest score, and the position and orientation estimation unit may determine the position and orientation of the template that yields the highest score as the position and orientation of the object.

[0009] In this configuration, the template is gridded, and each grid cell in the template contains a numerical value indicating whether or not the object exists as a physical object. The point cloud data is also gridded with the same grid size as the template, and each grid cell in the point cloud data contains a numerical value indicating the presence or absence of points. Then, the template is translated and / or rotated in three dimensions relative to the point cloud data, and a score representing the degree of agreement between the template and the point cloud data is calculated. The position and orientation of the template that yields the highest score is then determined. Therefore, the position and orientation of the template that best matches the point cloud data can be obtained with high accuracy.

[0010] (3) In (2) above, in the template, positive values ​​are set in grids where the object exists as an object, and negative values ​​are set in grids where the object does not exist as an object. The matching unit sets a positive value when a point cloud exists in the grid of the point cloud data, and a negative value when a point cloud does not exist in the grid of the point cloud data. Then, the template is translated and / or rotated in three dimensions with respect to the point cloud data, the values ​​in the corresponding grids of the template and the point cloud data are multiplied together, and the score is calculated by adding up all the multiplied values.

[0011] In this configuration, in the template, positive values ​​are set in grids where the object exists as an object, and negative values ​​are set in grids where the object does not exist as an object. Also, positive values ​​are set when a point cloud exists in the grid of the point cloud data, and negative values ​​are set when a point cloud does not exist in the grid of the point cloud data. Then, the template is translated and / or rotated in 3D relative to the point cloud data, the values ​​in the corresponding grids of the template and point cloud data are multiplied together, and the score is calculated by adding up all the multiplied values. Therefore, the position and orientation of the template that best matches the point cloud data can be easily obtained using a simple calculation formula.

[0012] (4) In (2) or (3) above, the template storage unit stores a first template having a specified grid size and a second template having a grid size smaller than the grid size of the first template as data corresponding to the front of the object, the matching unit grids the point cloud data with the same grid size as the first template, sets a numerical value in each grid of the point cloud data according to the presence or absence of a point cloud, then translates and / or rotates the first template in three dimensions with respect to the point cloud data, calculates a first score representing the degree of agreement between the first template and the point cloud data, and selects the first template that has the highest first score The system includes a first alignment processing unit that determines the position and orientation of the template, and a second alignment processing unit that, after executing the first alignment processing unit, grids the point cloud data with the same grid size as the second template, sets a numerical value in each grid of the point cloud data according to the presence or absence of a point cloud, then translates and / or rotates the second template in three dimensions relative to the point cloud data, calculates a second score representing the degree of agreement between the second template and the point cloud data, and determines the position and orientation of the second template that yields the highest second score. The position and orientation estimation unit may determine the position and orientation of the second template that yields the highest second score as the position and orientation of the object.

[0013] In this configuration, the first alignment processing unit first calculates a first score representing the degree of agreement between the first template, which has a specified grid size, and the point cloud data, and determines the position and orientation of the first template that yields the highest first score. Thus, the approximate position and orientation of the object are obtained. Subsequently, the second alignment processing unit calculates a second score representing the degree of agreement between the second template, which has a grid size smaller than that of the first template, and the point cloud data, and determines the position and orientation of the second template that yields the highest second score. Thus, the precise position and orientation of the object are obtained. Consequently, the detection accuracy of the object's position and orientation is improved.

[0014] (5) In any of (1) to (4) above, the position and orientation detection device further comprises an object extraction unit that extracts an object point cloud range in which the front of the object exists in the point cloud data acquired by the point cloud data acquisition unit, and the matching unit may perform matching between a template stored in the template storage unit and enlarged point cloud data including the object point cloud range extracted by the object extraction unit.

[0015] In this configuration, the point cloud data extracts the area where the front of the object exists, and matching is performed between the template and the enlarged point cloud data containing the object's point cloud area. Consequently, the processing load on the matching unit is reduced, and the matching accuracy between the template and the point cloud data is improved.

[0016] (6) In any of (1) to (5) above, the object may be a pallet having two fork holes opening on the front.

[0017] In this configuration, by using data corresponding to the front surface of a pallet with two fork holes as a two-dimensional or three-dimensional template, the accuracy of detecting the pallet's position and orientation is improved while reducing effort and cost.

[0018] (7) In any of the above (1) to (6), the point cloud data acquisition unit may have a 3D laser sensor that irradiates a laser toward the front surface of the object and receives the reflected light of the laser to detect the distance to the object.

[0019] In such a configuration, by using a 3D laser sensor that is strong against changes in brightness, the robustness against changes in the surrounding environment is improved, and three-dimensional point cloud data including the front surface of the object is acquired with high precision.

Effect of the Invention

[0020] According to the present invention, it is possible to improve the detection accuracy of the position and orientation of the object while reducing labor and cost.

Brief Description of the Drawings

[0021] [Figure 1] It is a schematic configuration diagram showing a load handling control device including a position and orientation detection device according to an embodiment of the present invention. [Figure 2] It is a front view showing a flat pallet and a post pallet as pallets. [Figure 3] It is a diagram showing an example of a template. [Figure 4] It is a diagram showing an example of numerical values set in each grid of the template. [Figure 5] It is a flowchart showing the procedure of the matching process executed by the initial alignment processing unit shown in FIG. 1. [Figure 6] It is a flowchart showing the procedure of the matching process executed by the fine alignment processing unit shown in FIG. 1. [Figure 7] It is a diagram showing an example of input point cloud data acquired by the laser sensor and the point cloud acquisition unit and enlarged point cloud data extracted by the pallet extraction unit. [Figure 8] It is a diagram showing an example of the position and orientation of the pallet together with the point cloud data and the template.

Mode for Carrying Out the Invention

[0022] Embodiments of the present invention will be described in detail below with reference to the drawings.

[0023] Figure 1 is a schematic diagram showing a load handling control device equipped with a position and orientation detection device according to one embodiment of the present invention. The load handling control device 1 is a device that uses a forklift 2 to load a pallet 3 (see Figure 2). The forklift 2, although not shown, has a pair of left and right forks for holding the pallet 3.

[0024] Pallet 3, although not shown in the illustration, is placed on the bed or floor of a truck, for example. As shown in Figure 2, flat pallets 3A and post pallets 3B are used as pallets 3.

[0025] The flat pallet 3A shown in Figure 2(a) is made of plastic or wood. The flat pallet 3A has a square shape in plan view. A load M is placed on the flat pallet 3A. The flat pallet 3A is provided with two fork holes 5 into which the forks of the forklift 2 are inserted. The fork holes 5 open on the front surface 3a of the flat pallet 3A. The shape of the fork holes 5 is rectangular in front view.

[0026] The post pallet 3B shown in Figure 2(b) has a square-shaped pallet section 6 in plan view, and four column sections 7 erected at the front, back, left, and right corners of the pallet section 6. A load M is placed on the pallet section 6. The pallet section 6 is provided with two fork holes 5 into which the forks of a forklift 2 are inserted. The column sections 7 are rectangular prisms. The column sections 7 are foldably connected to the corners of the pallet section 6.

[0027] In Figure 1, the load handling control device 1 is mounted on the forklift 2. The load handling control device 1 comprises a laser sensor 11, a template storage unit 12, a pallet detection controller 13, and a higher-level controller 14. The functions of the pallet detection controller 13 and the higher-level controller 14 may be combined into a single controller.

[0028] The laser sensor 11, template storage unit 12, and pallet detection controller 13 constitute the position and orientation detection device 10 of this embodiment, which detects the position and orientation of the target object, the pallet 3.

[0029] The laser sensor 11 irradiates a laser beam onto the front surface 3a of the pallet 3 to be picked up and detects the distance to the pallet 3 as three-dimensional point cloud data by receiving the reflected laser light. The point cloud is a collection of laser reflection points on the object, including the pallet 3. For example, a 3D LiDAR or a laser rangefinder can be used as the laser sensor 11.

[0030] As shown in Figure 3, the template storage unit 12 stores data corresponding to the front surface 3a of the pallet 3 as a two-dimensional template T. When the pallet 3 is a post-pallet 3B, a template T with the column section 7 omitted is used. The template T is created in advance, for example, from a CAD drawing. Note that the template T may also be created manually.

[0031] Template T is gridded data. Template T is a CSV (Comma-Separated Values) data file in a text file format where data is separated by commas, as shown in Figure 4, for example. Template T has multiple grids g.

[0032] Within each grid g of template T, a numerical value is set to indicate whether or not pallet 3 exists as an object. In template T, a positive value is set within the grid g where pallet 3 exists as an object. That is, a positive value is set within the grid g corresponding to the front 3a of pallet 3 in template T. In template T, a negative value is set within the grid g where pallet 3 does not exist as an object, in other words, within the grid g where empty space exists. That is, a negative value is set within the grid g corresponding to the fork hole 5 of pallet 3 in template T. In template T, zero is set within the grid g that is the boundary between an object and space, or within the grid g that clearly does not correspond to pallet 3.

[0033] In this case, the larger the absolute value of the positive number set in the grid g of template T, the higher the probability that the front surface 3a of pallet 3 exists. The larger the absolute value of the negative number set in the grid g of template T, the higher the probability that the fork hole 5 of pallet 3 exists. By assigning positive and negative signs to the numbers set in the grid g of template T in this way, three-dimensional matching becomes possible for pallet 3 that has a fork hole 5.

[0034] Furthermore, the template storage unit 12 stores two templates: an initial alignment template T1 (first template) and a precision alignment template T2 (second template), which correspond to the front surface 3a of the pallet 3. For convenience, only the precision alignment template T2 is shown in Figure 4, and the initial alignment template T1 is not illustrated.

[0035] The initial alignment template T1 has a standard grid size (e.g., 20mm x 20mm). The precision alignment template T2 has a smaller grid size than the initial alignment template T1 (e.g., 10mm x 10mm).

[0036] The pallet detection controller 13 consists of a CPU, RAM, ROM, and an input / output interface, etc. The pallet detection controller 13 includes a point cloud acquisition unit 21, a pallet extraction unit 22, a matching unit 23, and a position and orientation estimation unit 24.

[0037] The point cloud acquisition unit 21 acquires the point cloud data detected by the laser sensor 11 as input point cloud data D (see Figure 7(a)). The point cloud acquisition unit 21 works in cooperation with the laser sensor 11 to form a point cloud data acquisition unit that acquires three-dimensional point cloud data including the front surface 3a of the pallet 3.

[0038] The pallet extraction unit 22 extracts the pallet point cloud range Rp (see Figure 7(a)) in the input point cloud data D acquired by the point cloud acquisition unit 21, where the front surface 3a of the pallet 3 exists, based on information about the placement location of the pallet 3 sent from the higher-level controller 14, which will be described later. It then acquires enlarged point cloud data Dp (see Figure 7(b)) which includes the pallet point cloud range Rp. The pallet point cloud range Rp in the enlarged point cloud data Dp is larger than the pallet point cloud range Rp in the input point cloud data D. The pallet point cloud range Rp corresponds to the object point cloud range. The pallet extraction unit 22 also constitutes an object extraction unit that extracts the object point cloud range where the front surface of the object exists.

[0039] The pallet extraction unit 22, using the input point cloud data D acquired by the point cloud acquisition unit 21, sets the pallet point cloud range Rp where, for example, the front surface 3a of pallet 3 is located as an ROI (Region of Interest), extracts the point cloud within the ROI, and excludes the point cloud outside the ROI.

[0040] The matching unit 23 performs matching between the template T stored in the template storage unit 12 and the enlarged point cloud data Dp, which includes the palette point cloud range Rp extracted by the palette extraction unit 22.

[0041] The matching unit 23 translates and rotates the template T in three dimensions relative to the enlarged point cloud data Dp to determine the position and orientation of the template T that best matches the enlarged point cloud data Dp.

[0042] The matching unit 23 grids the enlarged point cloud data Dp with the same grid size as the template T and sets a numerical value in each grid g of the enlarged point cloud data Dp according to the presence or absence of point clouds. Then, the matching unit 23 translates and rotates the template T in three dimensions relative to the enlarged point cloud data Dp, calculates a score representing the degree of agreement between the template T and the enlarged point cloud data Dp, and determines the position and orientation of the template T that yields the highest score.

[0043] At this time, the matching unit 23 sets a positive value if a point cloud exists within the grid g of the enlarged point cloud data Dp, and a negative value if a point cloud does not exist within the grid g of the enlarged point cloud data Dp. Subsequently, the matching unit 23 translates and rotates the template T in three dimensions relative to the enlarged point cloud data Dp, multiplies the values ​​within the corresponding grids g of the template T and the enlarged point cloud data Dp, and calculates a score by adding up all the multiplied values.

[0044] The matching unit 23 includes an initial alignment processing unit 23a (first alignment processing unit) and a precision alignment processing unit 23b (second alignment processing unit).

[0045] The initial alignment processing unit 23a grids the enlarged point cloud data Dp (enlarged point cloud data Dp for initial alignment) acquired by the palette extraction unit 22 with the same grid size as the initial alignment template T1, and sets a numerical value in each grid g of the enlarged point cloud data Dp according to the presence or absence of point clouds. Subsequently, the initial alignment processing unit 23a translates and rotates the initial alignment template T1 in three dimensions relative to the enlarged point cloud data Dp, calculates an initial alignment score (first score) that represents the degree of agreement between the initial alignment template T1 and the enlarged point cloud data Dp, and determines the position and orientation of the initial alignment template T1 that yields the highest initial alignment score.

[0046] Figure 5 is a flowchart showing the procedure for the matching process performed by the initial alignment processing unit 23a.

[0047] In Figure 5, the initial alignment processing unit 23a first grids the enlarged point cloud data Dp for initial alignment extracted by the pallet extraction unit 22 in three dimensions (procedure S101). At this time, the initial alignment processing unit 23a grids the enlarged point cloud data Dp for initial alignment with a predetermined grid size (here, 20 mm × 20 mm × 20 mm).

[0048] Then, the initial alignment processing unit 23a sets a numerical value in each grid g of the enlarged point cloud data Dp for initial alignment according to the presence or absence of point clouds (procedure S102). At this time, if a point cloud exists in the grid g of the enlarged point cloud data Dp, the initial alignment processing unit 23a sets "+1" in the grid g. If a point cloud does not exist in the grid g of the enlarged point cloud data Dp, the initial alignment processing unit 23a sets "-1" in the grid g.

[0049] Furthermore, the initial alignment processing unit 23a reads out the initial alignment template T1 stored in the template storage unit 12 (procedure S103). Subsequently, the initial alignment processing unit 23a translates and rotates the initial alignment template T1 in three dimensions relative to the enlarged point cloud data Dp for initial alignment, and calculates the position and orientation of the template T1 that best matches the enlarged point cloud data Dp (procedure S104).

[0050] During the translation of template T1, template T1 is moved in three axial directions (XYZ directions) at grid-size intervals. During the rotation of template T1, template T1 is rotated around three axes (roll direction, pitch direction, and yaw direction) at specified angular intervals (e.g., 2 degrees). In other words, the matching accuracy of the initial positioning processing unit 23a is ±10 mm and ±1 degree.

[0051] At this time, the initial alignment processing unit 23a calculates the initial alignment score by multiplying the numerical values ​​in the grid g of the initial alignment template T1 by the numerical values ​​in the corresponding grid g of the enlarged point cloud data Dp for initial alignment, and adding up the multiplied values ​​for all grids g. In other words, the initial alignment score is expressed by the following formula. Score += numerical value in each grid of the enlarged point cloud data × numerical value in each grid of the template…(A)

[0052] The initial alignment processing unit 23a then calculates the position and orientation of template T1, which has the highest initial alignment score, as the position and orientation of template T1 that best matches the enlarged point cloud data Dp for initial alignment.

[0053] After executing the initial alignment processing unit 23a, the precision alignment processing unit 23b grids the enlarged point cloud data Dp for precision alignment with the same grid size as the precision alignment template T2, and sets a numerical value in each grid g of the enlarged point cloud data Dp according to the presence or absence of point clouds. Subsequently, the precision alignment processing unit 23b translates and rotates the precision alignment template T2 in three dimensions relative to the enlarged point cloud data Dp, calculates a precision alignment score (second score) that represents the degree of agreement between the template T2 and the enlarged point cloud data Dp, and determines the position and orientation of the precision alignment template T2 that yields the highest precision alignment score.

[0054] The enlarged point cloud data Dp for precise alignment is the point cloud data located near the initial alignment template T1 after matching by the initial alignment processing unit 23a. Furthermore, the size of the palette point cloud range Rp included in the enlarged point cloud data Dp for precise alignment may be equal to the size of the palette point cloud range Rp included in the enlarged point cloud data Dp for initial alignment, or it may be larger than the size of the palette point cloud range Rp included in the enlarged point cloud data Dp for initial alignment.

[0055] Figure 6 is a flowchart showing the procedure for the matching process performed by the precision alignment processing unit 23b.

[0056] In Figure 6, the precision alignment processing unit 23b first grids the enlarged point cloud data Dp for precision alignment in three dimensions (procedure S111). At this time, the precision alignment processing unit 23b grids the enlarged point cloud data Dp for precision alignment with a grid size smaller than the grid size in the initial alignment processing unit 23a (here, 10mm × 10mm × 10mm).

[0057] Then, the precision alignment processing unit 23b sets a numerical value in each grid g of the enlarged point cloud data Dp for precision alignment, depending on whether or not a point cloud exists (procedure S112). At this time, the method for setting the numerical value depending on whether or not a point cloud exists is the same as in procedure S102 shown in Figure 5.

[0058] Furthermore, the precision alignment processing unit 23b reads out the precision alignment template T2 stored in the template storage unit 12 (procedure S113). Subsequently, the precision alignment processing unit 23b translates and rotates the precision alignment template T2 in three dimensions to calculate the position and orientation of the template T2 that best matches the enlarged point cloud data Dp for precision alignment (procedure S114).

[0059] During the translation of template T2, template T2 is moved in the three axes at grid-size intervals. During the rotation of template T2, template T2 is rotated around the three axes at intervals of an angle smaller than the rotation angle in the initial alignment processing unit 23a (e.g., 0.1 degrees). In other words, the matching accuracy of the precision alignment processing unit 23b is ±5 mm and ±0.05 degrees.

[0060] At this time, the precision alignment processing unit 23b calculates the precision alignment score by multiplying the numerical values ​​in each grid g of the precision alignment template T2 (see Figure 4) by the corresponding numerical values ​​in the grid g of the enlarged point cloud data Dp, and then adding up the multiplied values ​​for all grids g. In other words, the precision alignment score is also expressed by equation (A) above.

[0061] The precision alignment processing unit 23b calculates the position and orientation of template T2, which best matches the enlarged point cloud data Dp for precision alignment, based on the highest score for precision alignment.

[0062] Returning to Figure 1, the position and orientation estimation unit 24 estimates the position and orientation of the pallet 3 according to the matching results from the matching unit 23. The position and orientation estimation unit 24 determines the position and orientation of the pallet 3 to be the position and orientation of the template T that best matches the enlarged point cloud data Dp.

[0063] The position and orientation estimation unit 24 determines the position and orientation of pallet 3 to be the position and orientation of template T that yields the highest score calculated by the matching unit 23. Specifically, the position and orientation estimation unit 24 determines the position and orientation of pallet 3 to be the position and orientation of template T2 for precision alignment that yields the highest score calculated by the precision alignment processing unit 23b.

[0064] In the position and orientation detection device 10 described above, a laser is first shone from the laser sensor 11 toward the area including the front surface 3a of the pallet 3 to be picked up, thereby acquiring three-dimensional input point cloud data D, for example, as shown in Figure 7(a).

[0065] Then, in the input point cloud data D acquired by the laser sensor 11, the pallet point cloud range Rp in which the front surface 3a of the pallet 3 is located is extracted. As shown in Figure 7(b), the enlarged point cloud data Dp, which includes the pallet point cloud range Rp, is acquired as point cloud data for initial alignment. The dimensions of the enlarged point cloud data Dp are, for example, approximately ±500 mm of the dimensions of the pallet 3 in the three-axis direction.

[0066] Then, the enlarged point cloud data Dp for initial alignment is gridded, and matching is performed between the enlarged point cloud data Dp and the initial alignment template T1. Specifically, a numerical value is set in each grid g of the enlarged point cloud data Dp for initial alignment according to the presence or absence of point clouds, and an initial alignment score expressed by equation (A) above is calculated. The value with the highest initial alignment score is then calculated as the position and orientation of template T1 in which the enlarged point cloud data Dp for initial alignment and the initial alignment template T1 best match.

[0067] Next, the enlarged point cloud data Dp for precision alignment is gridded, and the enlarged point cloud data Dp is matched with the precision alignment template T2. Specifically, a numerical value is assigned to each grid g of the enlarged point cloud data Dp for precision alignment according to the presence or absence of point clouds, and a precision alignment score expressed by equation (A) above is calculated. The value with the highest precision alignment score is then calculated as the position and orientation of template T2 that best matches the enlarged point cloud data Dp for precision alignment.

[0068] Then, the position and orientation of template T2 that yields the highest precision alignment score are determined as the position and orientation of palette 3. The position and orientation of palette 3 are represented, for example, as shown in Figure 8, by a normal vector V that protrudes from the center of template T2 in the enlarged point cloud data Dp so as to yield the highest precision alignment score.

[0069] Returning to Figure 1, the higher-level controller 14 consists of a CPU, RAM, ROM, and input / output interfaces, etc. Based on the position and orientation of the pallet 3 detected by the pallet detection controller 13, the higher-level controller 14 controls the travel drive unit and the cargo handling drive unit (neither shown) of the forklift 2 to perform a cargo picking operation.

[0070] Specifically, the higher-level controller 14 estimates the self-position of the forklift 2 using, for example, point cloud data from the laser sensor 11, and controls the drive unit to move the forklift 2 to a position where the forks of the forklift 2 are inserted into the fork holes 5 of the pallet 3, based on the self-position of the forklift 2. Then, the higher-level controller 14 controls the cargo handling drive unit to lift the pallet 3 by raising the forks that are inserted into the fork holes 5 of the pallet 3, and controls the drive unit to move the forklift 2 to the loading target (for example, the bed of a truck) in that state.

[0071] As described above, in this embodiment, data corresponding to the front surface 3a of the pallet 3 is stored as a two-dimensional template T, and three-dimensional magnified point cloud data Dp including the front surface 3a of the pallet 3 is acquired. Then, the template T and the magnified point cloud data Dp are matched, and the position and orientation of the pallet 3 are estimated according to the matching result. Here, the template T is translated and rotated in three dimensions relative to the magnified point cloud data Dp, and the position and orientation of the template T that best matches the magnified point cloud data Dp is determined. The position and orientation of the template T that best matches the magnified point cloud data Dp is then determined as the position and orientation of the pallet 3. In this way, the position and orientation of the pallet 3 are estimated using the template T that has been pre-stored as data corresponding to the front surface 3a of the pallet 3. For example, when estimating the position and orientation of the pallet 3 using deep learning, a great deal of effort is required to create the training data. However, creating the template T requires less effort than creating the training data. This reduces effort and cost while improving the detection accuracy of the position and orientation of the pallet 3.

[0072] In this embodiment, the template T is gridded, and each grid g of the template T contains a numerical value indicating whether or not the palette 3 exists as an object. The enlarged point cloud data Dp is also gridded with the same grid size as the template T, and each grid g of the enlarged point cloud data Dp contains a numerical value indicating the presence or absence of a point cloud. Subsequently, the template T is translated and rotated in three dimensions relative to the enlarged point cloud data Dp, and a score representing the degree of agreement between the template T and the enlarged point cloud data Dp is calculated. The position and orientation of the template T that yields the highest score are then determined. Therefore, the position and orientation of the template T that best matches the enlarged point cloud data Dp can be obtained with high accuracy.

[0073] Furthermore, in this embodiment, in template T, positive values ​​are set in grid g where pallet 3 exists as an object, and negative values ​​are set in grid g where pallet 3 does not exist as an object. Also, if a point cloud exists in the grid g of the enlarged point cloud data Dp, a positive value is set, and if no point cloud exists in the grid g of the enlarged point cloud data Dp, a negative value is set. Subsequently, template T is translated and rotated in three dimensions relative to the enlarged point cloud data Dp, the numerical values ​​in the corresponding grid g of template T and enlarged point cloud data Dp are multiplied together, and the score is calculated by adding all the multiplied values ​​together. Therefore, the position and orientation of template T that best matches the enlarged point cloud data Dp can be easily obtained using a simple calculation formula.

[0074] Furthermore, in this embodiment, the initial alignment processing unit 13a first calculates an initial alignment score representing the degree of agreement between the initial alignment template T1 having a predetermined grid size and the enlarged point cloud data Dp, and determines the position and orientation of the initial alignment template T1 that yields the highest initial alignment score. Thus, the approximate position and orientation of the pallet 3 are obtained. Subsequently, the precision alignment processing unit 13b calculates a precision alignment score representing the degree of agreement between the precision alignment template T2 having a grid size smaller than the grid size of the initial alignment template T1 and the enlarged point cloud data Dp, and determines the position and orientation of the precision alignment template T2 that yields the highest precision alignment score. Thus, the precise position and orientation of the pallet 3 are obtained. Consequently, the detection accuracy of the position and orientation of the pallet 3 is improved.

[0075] Furthermore, in this embodiment, the palette point cloud range Rp in which the front surface 3a of the palette 3 exists is extracted from the input point cloud data D, and matching is performed between the template T and the enlarged point cloud data Dp which includes the palette point cloud range Rp. As a result, the processing load of the matching unit 23 is reduced, and the matching accuracy between the template T and the enlarged point cloud data Dp is improved.

[0076] Furthermore, in this embodiment, by using data corresponding to the front surface 3a of the pallet 3 having two fork holes 5 as a two-dimensional template T, the effort required to manually create the template T can be reduced.

[0077] Furthermore, in this embodiment, by using a 3D laser sensor 11 that is resistant to changes in brightness, robustness to changes in the surrounding environment is improved, and 3D input point cloud data D including the front surface 3a of the pallet 3 is acquired with high accuracy.

[0078] The present invention is not limited to the embodiments described above. For example, in the above embodiment, the initial alignment processing unit 23a performs matching of the enlarged point cloud data Dp with the initial alignment template T1, and then the precision alignment processing unit 23b performs matching of the enlarged point cloud data Dp with the precision alignment template T2, but the invention is not limited to such a configuration. For example, as long as the detection accuracy of the position and orientation of the pallet 3 is ensured, the number of times the enlarged point cloud data Dp and template T are matched may be as few as one.

[0079] Furthermore, in the above embodiment, the template T is translated and rotated in three dimensions relative to the enlarged point cloud data Dp to determine the position and orientation of the template T that best matches the enlarged point cloud data Dp, but the embodiment is not limited to this. Alternatively, the template T may be translated or rotated in three dimensions relative to the enlarged point cloud data Dp to determine the position and orientation of the template T that best matches the enlarged point cloud data Dp.

[0080] Furthermore, in the above embodiment, the pallet point cloud range Rp in which the front surface 3a of the pallet 3 exists is extracted from the input point cloud data D acquired by the laser sensor 11, and matching is performed between the enlarged point cloud data Dp including the pallet point cloud range Rp and the template T. However, the embodiment is not limited to this form. Matching may also be performed between the input point cloud data D acquired by the laser sensor 11 and the template T.

[0081] Furthermore, although the input point cloud data D is acquired using the laser sensor 11 in the above embodiment, the form is not particularly limited, and a stereo camera or ToF camera, etc., may be used instead of the laser sensor 11, as long as three-dimensional point cloud data can be acquired.

[0082] Furthermore, in the above embodiment, a score representing the degree of agreement between the enlarged point cloud data Dp and the template T is calculated, and the position and orientation of the pallet 3 are estimated based on the value of this score. However, the reliability of detecting the pallet 3 can also be calculated using the value of this score.

[0083] Furthermore, in the above embodiment, the position and orientation of the pallet 3 are detected when the forklift 2 unloads the pallet 3. However, the present invention is also applicable to detecting the position and orientation of the pallet 3 in a work environment other than cargo handling by the forklift 2.

[0084] Furthermore, although a two-dimensional template T is used in the above embodiment, the form is not limited to this, and a three-dimensional template may also be used. In this case, the calculation processing by the matching unit 23 when matching the template with the point cloud data can be simplified.

[0085] Furthermore, in the above embodiment, the position and orientation of the pallet 3 are detected as the object, but the present invention can be applied to objects other than the pallet 3 as long as the position and orientation of the object whose shape is known in advance is detected. Here, when detecting the position and orientation of an object with a complex shape in the depth direction, it is effective to use a three-dimensional template.

[0086] Furthermore, although the position and orientation detection device 10 is mounted on the forklift 2 in the above embodiment, the present invention can be applied to vehicles other than the forklift 2, as well as to equipment other than vehicles, as long as it detects the position and orientation of an object. [Explanation of symbols]

[0087] 3...Pallet (object), 3a...Front view, 5...Fork hole, 10...Position and orientation detection device, 11...Laser sensor (point cloud data acquisition unit), 12...Template storage unit, 21...Point cloud acquisition unit (point cloud data acquisition unit), 22...Pallet extraction unit (object extraction unit), 23...Matching unit, 23a...Initial positioning processing unit (first positioning processing unit), 23b...Precise positioning processing unit (second positioning processing unit), 24...Position and orientation estimation unit, D...Input point cloud data (point cloud data), Dp...Enlarged point cloud data (point cloud data), g...Grid, Rp...Pallet point cloud range (object point cloud range), T...Template, T1...Template (first template), T2...Template (second template).

Claims

1. A position and orientation detection device for detecting the position and orientation of an object, A template storage unit that stores data corresponding to the front surface of the object as a two-dimensional or three-dimensional template, A point cloud data acquisition unit acquires three-dimensional point cloud data including the front surface of the object, A matching unit that performs matching between the template stored in the template storage unit and the point cloud data acquired by the point cloud data acquisition unit, The system includes a position and orientation estimation unit that estimates the position and orientation of the object according to the matching result from the matching unit, The matching unit translates and / or rotates the template in three dimensions relative to the point cloud data to determine the position and orientation of the template that best matches the point cloud data. The position and orientation estimation unit is a position and orientation detection device that determines the position and orientation of the template that best matches the point cloud data as the position and orientation of the object.

2. The aforementioned template is gridded data, Within each grid of the aforementioned template, a numerical value is set to indicate whether or not the object exists as a physical object. The matching unit grids the point cloud data with the same grid size as the template, sets a numerical value in each grid of the point cloud data according to the presence or absence of a point cloud, then translates and / or rotates the template in three dimensions relative to the point cloud data, calculates a score representing the degree of agreement between the template and the point cloud data, and determines the position and orientation of the template that yields the highest score. The position and orientation detection device according to claim 1, wherein the position and orientation estimation unit determines the position and orientation of the template that yields the highest score as the position and orientation of the object.

3. In the template, positive values ​​are set in the grids where the object exists as an object, and negative values ​​are set in the grids where the object does not exist as an object. The position and attitude detection device according to claim 2, wherein the matching unit sets a positive value when a point cloud exists within the grid of the point cloud data, sets a negative value when a point cloud does not exist within the grid of the point cloud data, then translates and / or rotates the template in three dimensions with respect to the point cloud data, multiplies the numerical values ​​in the corresponding grids of the template and the point cloud data, and calculates the score by adding up all the multiplied values.

4. The template storage unit stores, as data corresponding to the front surface of the object, a first template having a predetermined grid size and a second template having a grid size smaller than the grid size of the first template. The matching unit is A first alignment processing unit that grids the point cloud data with the same grid size as the first template, sets a numerical value in each grid of the point cloud data according to the presence or absence of a point cloud, then translates and / or rotates the first template in three dimensions relative to the point cloud data, calculates a first score representing the degree of agreement between the first template and the point cloud data, and determines the position and orientation of the first template that yields the highest first score, The system includes a second alignment processing unit which, after executing the first alignment processing unit, grids the point cloud data with the same grid size as the second template, sets a numerical value in each grid of the point cloud data according to the presence or absence of a point cloud, then translates and / or rotates the second template in three dimensions relative to the point cloud data, calculates a second score representing the degree of agreement between the second template and the point cloud data, and determines the position and orientation of the second template that yields the highest second score. The position and orientation detection device according to claim 2, wherein the position and orientation estimation unit determines the position and orientation of the second template that yields the highest second score as the position and orientation of the object.

5. The system further includes an object extraction unit that extracts the object point cloud range in which the front surface of the object exists in the point cloud data acquired by the point cloud data acquisition unit, The position and orientation detection device according to claim 1, wherein the matching unit performs matching between the template stored in the template storage unit and the enlarged point cloud data including the point cloud range of the object extracted by the object extraction unit.

6. The position and orientation detection device according to claim 1, wherein the object is a pallet having two fork holes opening on its front surface.

7. The position and orientation detection device according to claim 1, wherein the point cloud data acquisition unit has a 3D laser sensor that detects the distance to the object by irradiating a laser toward the front of the object and receiving the reflected light of the laser.

Images