Position and posture estimation device

Abstract

To provide a position / attitude estimation device capable of estimating with high accuracy a post pallet position and a post pallet attitude with respect to a forklift.SOLUTION: A position / attitude estimation device 21 comprise: a laser sensor 12 configured to irradiate laser toward the front face of a post pallet 5 and receive laser reflected light so as to detect a distance to the post pallet 5 and acquire point group data; a plane detection unit 17 configured to detect a plane corresponding to a front face 7a of a pallet part 7 and to a front face 8a of a column part 8 based on the point group data; and an estimation calculation unit 18 configured to perform calculation for estimating position and attitude of the post pallet 5 with respect to a forklift 1 using the plane corresponding to the front faces 7a, 8a. The estimation calculation unit 18 extracts a transverse line corresponding to the pallet part 7 and a vertical line corresponding to the column part 8 based on the plane corresponding to the front faces 7a, 8a, and calculates the position and the attitude of the post pallet 5 with respect to the forklift 1 based on the transverse line and the vertical line.SELECTED DRAWING: Figure 3

Description

[Technical field]

[0001] The present invention relates to a position and orientation estimation device. [Background technology]

[0002] A known conventional position and orientation estimation device is the technology described in Patent Document 1, for example. The position and orientation estimation device described in Patent Document 1 includes a monocular camera that detects a pallet, and a controller connected to the monocular camera. The controller includes a pallet detection unit that detects the pallet based on image data captured by the monocular camera and creates framed captured image data in which a frame line is specified to surround the front surface of the pallet and two pallet holes in the captured image data, and an estimation calculation unit that estimates the position and orientation of the pallet using the framed captured image data. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent Publication No. 2021-24718 Summary of the Invention [Problem to be solved by the invention]

[0004] One type of pallet used in loading and unloading operations by a forklift is a post pallet that can be stacked with cargo placed on it. For such a post pallet, the position and orientation relative to the forklift must also be estimated with high accuracy.

[0005] An object of the present invention is to provide a position and orientation estimation device that can estimate with high accuracy the position and orientation of a post pallet relative to a forklift. [Means for solving the problem]

[0006] One aspect of the present invention is a position and attitude estimation device that is mounted on a forklift and estimates the position and attitude of a post pallet which has a pallet section on which cargo is placed and four pillar sections erected at the front, back, left and right corners of the pallet section. The device comprises a laser distance detection unit that detects the distance to the post pallet by irradiating a laser toward the front of the post pallet and receiving the reflected laser light to obtain point cloud data, a plane detection unit that detects a plane corresponding to the front faces of the pallet section and the pillar sections based on the point cloud data obtained by the laser distance detection unit, and an estimation calculation unit that performs an estimation calculation of the position and attitude of the post pallet relative to the forklift using the plane corresponding to the front faces of the pallet section and the pillar sections detected by the plane detection unit. The estimation calculation unit extracts horizontal lines corresponding to the pallet section and vertical lines corresponding to the pillar sections based on the plane corresponding to the front faces of the pallet section and the pillar sections, and calculates the position and attitude of the post pallet relative to the forklift based on the horizontal and vertical lines.

[0007] In such a position and orientation estimation device, a laser distance detection unit irradiates a laser toward the front surface of the post pallet, thereby acquiring point cloud data corresponding to the post pallet. Then, based on the point cloud data corresponding to the post pallet, a plane corresponding to the front surface of the pallet portion and the pillar portion of the post pallet is detected. Then, based on the plane corresponding to the front surface of the pallet portion and the pillar portion, a horizontal line corresponding to the pallet portion and a vertical line corresponding to the pillar portion are extracted. Then, based on the horizontal line and the vertical line, the position and orientation of the post pallet relative to the forklift are calculated. At this time, since the pillar portion extends long in the vertical direction, it is easy to obtain high accuracy in the vertical position and angle of the post pallet. As a result, the position and orientation of the post pallet relative to the forklift can be estimated with high accuracy.

[0008] The position and orientation estimation device further includes an imaging unit that images the front of the post pallet and acquires image data, a post pallet recognition unit that recognizes the post pallet based on the image data acquired by the imaging unit, and a point cloud processing unit that extracts point cloud data corresponding to the post pallet recognized by the post pallet recognition unit from the point cloud data acquired by the laser distance detection unit, and the plane detection unit may detect a plane corresponding to the front of the pallet portion and the column portion based on the point cloud data extracted by the point cloud processing unit.

[0009] In this configuration, the post pallet is recognized based on image data acquired by the imaging unit, and point cloud data corresponding to the recognized post pallet is extracted from the point cloud data acquired by the laser distance detection unit. Image data has a higher resolution than point cloud data. This increases the recognition accuracy of the post pallet. Therefore, the position and orientation of the post pallet relative to the forklift can be estimated with even higher accuracy.

[0010] The estimation calculation unit may calculate the posture of the post pallet relative to the forklift by calculating the posture of the pillar part based on the inclination and dimensions of the vertical line, and may also calculate the position of the post pallet relative to the forklift by finding the intersection between the horizontal and vertical lines and calculating the position of the pallet part based on the position of the intersection.

[0011] In this configuration, the posture of the pillars is calculated based on the inclination and dimensions of the vertical lines corresponding to the pillars, and the position of the pallet is calculated based on the intersection of the horizontal line corresponding to the pallet and the vertical line corresponding to the pillars. Therefore, the position and posture of the post pallet relative to the forklift can be easily calculated.

[0012] The plane detection unit may detect a plane corresponding to the front of the pallet portion and the column portion by obtaining second point cloud data in which the point cloud has been removed from the first point cloud data corresponding to the post pallet, performing plane approximation on the second point cloud data to a thickness greater than a specified thickness to obtain a first plane, and then extracting third point cloud data included in the first plane from the first point cloud data, and performing plane approximation on the third point cloud data to a specified thickness to obtain a second plane.

[0013] In this configuration, the second point cloud data from which the point clouds have been removed is subjected to plane approximation to a thickness greater than the specified thickness to obtain the first plane, thereby dealing with variations in the second point cloud data when the plane approximation is repeated. Then, the third point cloud data included in the first plane is extracted from the first point cloud data, and the third point cloud data is subjected to plane approximation to a specified thickness to obtain the second plane, making it difficult for chips to occur in the plane corresponding to the front faces of the pallet part and the pillar part, and unnecessary point clouds not included in the plane corresponding to the front faces of the pallet part and the pillar part are deleted. In addition, the deviation of the position and angle of the plane corresponding to the front faces of the pallet part and the pillar part is suppressed. As a result, the position and attitude of the post pallet relative to the forklift can be estimated with even higher accuracy.

[0014] The estimation calculation unit may perform linear approximation on the planar point cloud data contained in the plane corresponding to the front surface of the pallet section and the column section to obtain a straight line, and then extract the point cloud data that is on the straight line from the planar point cloud data, thereby extracting horizontal and vertical lines.

[0015] In this configuration, a straight line is obtained by performing linear approximation on the plane point cloud data included in the plane corresponding to the front faces of the pallet and pillars, and then the point cloud data on the straight line is extracted from the plane point cloud data, which makes it difficult for the horizontal line corresponding to the pallet and the vertical line corresponding to the pillars to be chipped, and also deletes unnecessary points that are not included in the horizontal and vertical lines. Therefore, the position and orientation of the post pallet relative to the forklift can be estimated with even higher accuracy.

[0016] The estimation calculation unit may transform the coordinate system of the planar point cloud data by rotating the planar point cloud data so that the planar point cloud data contained in the plane corresponding to the front of the pallet section and the pillar section is virtually facing the laser distance detection unit, and then calculate the position and attitude of the post pallet relative to the forklift based on the horizontal and vertical lines, and then rotate the planar point cloud data in the opposite direction to return the position and attitude of the post pallet to its original state, thereby determining the final position and attitude of the post pallet relative to the forklift.

[0017] In this configuration, the plane point cloud data included in the plane corresponding to the front of the pallet and the pillar is rotated so that it virtually faces the laser distance detector, and the intersection points of the horizontal and vertical lines are found two-dimensionally while ignoring the front-to-rear direction of the post pallet. This allows the intersection points of the horizontal and vertical lines to be extracted with high accuracy. Therefore, the position and orientation of the post pallet relative to the forklift can be estimated with even higher accuracy.

[0018] After calculating the position and posture of the post pallet relative to the forklift, the estimation calculation unit makes a final determination as to whether the plane detected by the plane detection unit corresponds to the front surface of the post pallet based on the inclination and dimensions of the horizontal and vertical lines.If it is determined that the plane detected by the plane detection unit does not correspond to the front surface of the post pallet, it may delete the point cloud data contained in the plane detected by the plane detection unit and cause the plane detection unit to re-execute the detection process of the plane corresponding to the front surfaces of the pallet section and column section.

[0019] In this configuration, when it is determined that the detected plane does not correspond to the front surface of the post pallet, the point cloud data included in the plane is deleted and the plane detection process is executed again, so that an accurate plane corresponding to the front surfaces of the pallet section and the columns is obtained. Therefore, the position and orientation of the post pallet relative to the forklift can be estimated with even higher accuracy. Effect of the Invention

[0020] According to the present invention, the position and orientation of a post pallet relative to a forklift can be estimated with high accuracy. [Brief description of the drawings]

[0021] [Figure 1] 1 is a schematic plan view showing a forklift equipped with a position and orientation estimation device according to an embodiment of the present invention, together with a post pallet. [Diagram 2] FIG. 2 is a front view of the post pallet shown in FIG. 1. [Diagram 3] 1 is a schematic configuration diagram showing a driving control device equipped with a position and attitude estimation device according to an embodiment of the present invention; [Figure 4] 4A to 4C are diagrams illustrating point cloud data acquired by a laser sensor and point cloud data extracted by a point cloud processing unit. [Diagram 5] 4 is a flowchart showing the procedure of a plane detection process executed by a plane detection unit shown in FIG. 3. [Figure 6] FIG. 1 is a conceptual diagram showing in a simplified manner a plane detection process in which normal plane approximation is performed on point cloud data. [Figure 7] 4 is a simplified conceptual diagram showing a plane detection process executed by the plane detection unit shown in FIG. 3. [Figure 8] 4 is a flowchart showing a procedure of an estimation calculation process executed by an estimation calculation unit shown in FIG. 3. [Figure 9] FIG. 13 is a conceptual diagram showing how coordinate conversion of planar point cloud data is performed. [Figure 10] 13 is a diagram showing horizontal lines corresponding to the pallet portion, vertical lines corresponding to the column portions, and the intersections of the horizontal and vertical lines. [Figure 11] 9 is a flowchart showing details of the line extraction processing procedure in step S122 shown in FIG. 8. [Figure 12] FIG. 13 is a conceptual diagram showing in a simplified manner a process of performing straight-line approximation on plane point cloud data. [Figure 13] FIG. 2 is a conceptual diagram showing the roll angle, yaw angle, and pitch angle of a post pallet. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0022] Hereinafter, an embodiment of the present invention will be described in detail with reference to the drawings.

[0023] Fig. 1 is a schematic plan view showing a forklift equipped with a position and attitude estimation device according to an embodiment of the present invention, together with a post pallet. In Fig. 1, the forklift 1 includes a vehicle body 2 and a loading device 3 disposed at the front side of the vehicle body 2 for loading and unloading. The loading device 3 includes a mast 4 attached to the front end of the vehicle body 2, and a pair of left and right forks 6 attached to the mast 4 so as to be movable up and down, and for lifting up the post pallet 5.

[0024] 2, the post pallet 5 has a pallet section 7 that is square in plan view, and four pillar sections 8 erected at each of the four corners, front, back, left and right, of the pallet section 7. The front-to-back direction of the post pallet 5 corresponds to the X direction, the left-to-right direction of the post pallet 5 corresponds to the Y direction, and the up-down direction of the post pallet 5 corresponds to the Z direction.

[0025] The pallet unit 7 is a loading platform on which cargo M is placed. The pallet unit 7 has a front surface 7a that faces the forklift 1 when the post pallet 5 is lifted by the forks 6. The pallet unit 7 is provided with two fork holes 9 into which the pair of forks 6 are inserted. The fork holes 9 extend from the front surface 7a to the rear surface 7b of the pallet unit 7. The shape of the fork holes 9 is rectangular when viewed from the front.

[0026] The pillar portion 8 has a quadrangular pillar shape. The pillar portion 8 is foldably connected to a corner of the pallet portion 7. The pillar portion 8 has a front surface 8a. The front surface 8a is on the same side as the front surface 7a of the pallet portion 7.

[0027] When each pillar section 8 stands up against the pallet section 7, such post pallets 5 can be stacked with luggage M placed on the pallet section 7. In this case, the pallet section 7 of the upper post pallet 5 is placed on the top surfaces of each pillar section 8 of the lower post pallet 5.

[0028] Fig. 3 is a schematic diagram showing the configuration of a driving control device equipped with a position and attitude estimation device according to an embodiment of the present invention. In Fig. 3, the driving control device 10 is a device that automatically drives the forklift 1 to a loading position when the forklift 1 loads a post pallet 5 loaded on a truck bed T (see Fig. 1). The loading position is a position where each fork 6 of the forklift 1 is inserted into a fork hole 9 of the post pallet 5.

[0029] The driving control device 10 is mounted on the forklift 1. The driving control device 10 includes a camera 11, a laser sensor 12, a controller 13, and a drive unit 14.

[0030] The camera 11 is an imaging unit that captures an image of the front (front side) of the post pallet 5 to obtain image data. As the camera 11, for example, a monocular camera or a stereo camera is used.

[0031] The laser sensor 12 is a laser distance detection unit that detects the distance to the post pallet 5 by emitting a laser toward the front surface of the post pallet 5 and receiving the reflected light of the laser to obtain point cloud data. A point cloud is a collection of laser reflection points on an object including the post pallet 5. The laser sensor 12 emits a 2D or 3D laser with a high point density. As the laser sensor 12, for example, a LIDAR or a laser range finder is used.

[0032] The controller 13 is composed of a CPU, RAM, ROM, an input / output interface, etc. The controller 13 receives image data acquired by the camera 11 and point cloud data acquired by the laser sensor 12, executes predetermined processing, and controls the drive unit 14. Although not shown in the figure, the drive unit 14 has, for example, a travel motor that rotates the drive wheels of the forklift 1 and a steering motor that steers the steering wheels of the forklift 1.

[0033] The controller 13 has a post pallet recognition unit 15 , a point cloud processing unit 16 , a plane detection unit 17 , an estimation calculation unit 18 , a path generation unit 19 , and a driving control unit 20 .

[0034] Here, the post pallet recognition unit 15, point cloud processing unit 16, plane detection unit 17 and estimation calculation unit 18 cooperate with the camera 11 and laser sensor 12 to configure a position and orientation estimation device 21 of this embodiment. The position and orientation estimation device 21 is a device that estimates the position and orientation of the post pallet 5 relative to the forklift 1 when the forklift 1 is positioned in front of the post pallet 5.

[0035] The post pallet recognition unit 15 recognizes the post pallet 5 based on the image data acquired by the camera 11. The post pallet recognition unit 15 recognizes the approximate position and dimensions of the post pallet 5 by, for example, an image processing technique using deep learning.

[0036] The point cloud processing unit 16 extracts point cloud data corresponding to the post pallet 5 recognized by the post pallet recognition unit 15 from the point cloud data acquired by the laser sensor 12. For example, when point cloud data Dp as shown in Fig. 4(a) is acquired by the laser sensor 12, when the post pallet 5 is recognized by the post pallet recognition unit 15, point cloud data Dpp corresponding to the post pallet 5 is obtained as shown in Fig. 4(b). The range of the point cloud data Dpp is somewhat narrower in the left-right direction (Y direction) than that of the point cloud data Dp.

[0037] The plane detection unit 17 detects a plane corresponding to the front surface 7a of the pallet section 7 and the front surface 8a of the column section 8 in the post pallet 5 (hereinafter sometimes simply referred to as the front surfaces 7a, 8a) based on the point cloud data corresponding to the post pallet 5 extracted by the point cloud processing unit 16.

[0038] The plane detection unit 17 detects a plane corresponding to the front surfaces 7a and 8a, for example, by fitting a plane to the point cloud data and finding the coefficients of a plane equation. At this time, the plane detection unit 17 randomly picks up some points of the point cloud data and fits the plane, for example, using RANSAC (Random sample consensus).

[0039] Fig. 5 is a flowchart showing the procedure of the plane detection process executed by the plane detection unit 17. In Fig. 5, the plane detection unit 17 first acquires point cloud data (hereinafter, referred to as first point cloud data) corresponding to the post pallet 5 extracted by the point cloud processing unit 16 (step S101).

[0040] Next, the plane detection unit 17 obtains second point cloud data (see FIG. 7(b)) (described later) by removing points on unnecessary planes by plane detection from the first point cloud data (see FIG. 7(a)) (step S102). Note that the initial data of the second point cloud data is, for example, the same point cloud data as the first point cloud data.

[0041] Next, the plane detection unit 17 performs plane approximation on the second point cloud data to a thickness greater than the specified thickness (see FIG. 7(c)), and obtains the first plane (step S103). The specified thickness is the dimension of the pallet unit 7 and the column unit 8 in the depth direction (front-back direction).

[0042] Next, the plane detection unit 17 extracts point cloud data (referred to as third point cloud data) included in the first plane from the first point cloud data (step S104). Next, the plane detection unit 17 performs plane approximation to a specified thickness on the third point cloud data (see FIG. 7(d)) ​​to obtain a second plane (step S105). That is, the plane detection unit 17 performs plane approximation on the third point cloud data thinner than in step S103.

[0043] Next, the plane detection unit 17 extracts point cloud data (fourth point cloud data) included in the second plane from the first point cloud data (step S106), and records the number of reflection points of the fourth point cloud data included in the second plane (step S107).

[0044] Next, the plane detection unit 17 determines whether the plane detection process has been performed a predetermined number of times (e.g., 100 times) (step S108). If the plane detection unit 17 determines that the plane detection process has not been performed the predetermined number of times, it sets the point cloud data from which unnecessary reflection points on the plane have been deleted as the second point cloud data (step S112), and executes the above steps S102 to S107 again.

[0045] When the plane detection unit 17 determines that the plane detection process has been performed a predetermined number of times, it selects the second plane corresponding to the fourth point cloud data having the largest number of recorded reflection points (step S109). Then, the plane detection unit 17 calculates the normal vector of the selected second plane (step S110).

[0046] The plane detection unit 17 judges whether the selected second plane is a plane likely to be the front surface of the post pallet 5 based on the normal vector of the second plane (step S111). For example, it is possible to judge whether the second plane is a plane likely to be the front surface of the post pallet 5 based on the direction of the normal vector.

[0047] When the plane detection unit 17 determines that the selected second plane is not a plane likely to be the front surface of the post pallet 5, it executes the above steps S101 to S110 again.

[0048] When the plane detection unit 17 determines that the selected second plane is likely to be the front surface of the post pallet 5, it determines the selected second plane as the plane corresponding to the front surfaces 7a, 8a, and detects the orientation of the second plane (step S113).

[0049] Fig. 6 is a conceptual diagram showing in a simplified manner the plane detection process that performs normal plane approximation on point cloud data. By performing the plane detection process on the point cloud data corresponding to the post palette 5 extracted by the point cloud processing unit 16, the point cloud data Dp0 shown in Fig. 6(a) is obtained. The point cloud data Dp0 is made up of a plurality of reflection points P0.

[0050] When plane approximation as shown in Fig. 6(b) is performed on such point cloud data Dp0, a point cloud corresponding to the front surface of the post pallet 5 is obtained. However, due to variations in the point cloud caused by errors in the laser sensor 12 and the material of the post pallet 5, it is not possible to obtain all of the point clouds, and there are gaps (see K in the figure) in the detected plane. Circles P are reflection points extracted by the plane approximation. Triangles Q are reflection points that were not extracted by the plane approximation.

[0051] Furthermore, when a plane approximation as shown in Figure 6(c) is performed on the point cloud data Dp0, it is determined that the detected plane is not a plane corresponding to the front surface of the post pallet 5, and all reflection points P0 of the point cloud data Dp0 contained in the plane are erased.

[0052] In this state, when plane approximation is performed by the next plane detection process, the reflection point P0 on the left side of the point cloud data Dp0 is missing, as shown in Fig. 6(d). Therefore, an accurate plane corresponding to the front surface of the post pallet 5 cannot be obtained. The white circle R indicates the erased reflection point.

[0053] Therefore, first, as shown in FIG. 7(a), the first point cloud data Dp1 corresponding to the post pallet 5 extracted by the point cloud processing unit 16 is acquired, and as shown in FIG. 7(b), the second point cloud data Dp2 is acquired by performing a plane detection process on the first point cloud data Dp1. The first point cloud data Dp1 is point cloud data in which no points are missing. The second point cloud data Dp2 is point cloud data in which points are missing in the process of erasing other planes. The first point cloud data Dp1 and the second point cloud data Dp2 are composed of a plurality of reflection points P1.

[0054] 7C, the second point cloud data Dp2 is approximated to a plane having a thickness greater than the specified thickness to obtain a first plane F1, which allows most of the points in the second point cloud data Dp2 to be obtained.

[0055] Then, the third point cloud data Dp3 included in the first plane F1 is extracted from the first point cloud data Dp1. The third point cloud data Dp3 includes almost all of the points on the front surface of the post pallet 5. Therefore, the third point cloud data Dp3 forms a plane that almost corresponds to the front surfaces 7a, 8a. However, since the first plane F1 extends obliquely, the angle error of the post pallet 5 becomes large.

[0056] 7(d), the third point cloud data Dp3 is approximated to a specified thickness to obtain a second plane F2. Since the second plane F2 extends at a correct angle, the accurate attitude (yaw angle) of the front surface of the post pallet 5 can be obtained.

[0057] Returning to Figure 3, the estimation calculation unit 18 performs an estimation calculation of the position and posture of the post pallet 5 relative to the forklift 1 using the plane corresponding to the front surface 7a of the pallet section 7 and the front surface 8a of the column section 8 detected by the plane detection unit 17.

[0058] The estimation calculation unit 18 extracts horizontal lines corresponding to the pallet section 7 and vertical lines corresponding to the pillar section 8 based on the plane corresponding to the front surfaces 7a, 8a, and calculates the position and posture of the post pallet 5 relative to the forklift 1 based on the horizontal and vertical lines.

[0059] Fig. 8 is a flowchart showing the procedure of the estimation calculation process executed by the estimation calculation unit 18. In Fig. 8, the estimation calculation unit 18 first rotates the plane point cloud data included in the plane corresponding to the front faces 7a, 8a so that the plane point cloud data virtually faces the laser sensor 12 (step S121). In other words, the XYZ coordinate system of the plane point cloud data is converted so that the plane point cloud data virtually faces the laser sensor 12.

[0060] Therefore, even if the planar point cloud data Df is positioned at an angle to the laser irradiation direction from the laser sensor 12, as shown in Figure 9(a), the planar point cloud data Df will virtually face directly toward the laser irradiation direction from the laser sensor 12, as shown in Figure 9(b).

[0061] Next, the estimation calculation unit 18 extracts a horizontal line corresponding to the pallet unit 7 and a vertical line corresponding to the column portion 8 based on the plane corresponding to the front surfaces 7a, 8a (step S122). At this time, the estimation calculation unit 18 extracts two horizontal lines La corresponding to the upper and lower end faces of the pallet unit 7, and two vertical lines Lb corresponding to the column portions 8 arranged on both the left and right sides of the pallet unit 7, as shown in Fig. 10. The vertical line Lb is, for example, the center line of the plane corresponding to the front surface 8a of the column portion 8. The center line is a line extending in the up-down direction (Z direction) from the center in the left-right direction (Y direction) on the plane corresponding to the front surface 8a of the column portion 8.

[0062] The estimation calculation unit 18 uses, for example, RANSAC (described above), to randomly pick up some points of the plane point cloud data and fit a straight line to them, thereby extracting horizontal and vertical straight lines.

[0063] Fig. 11 is a flowchart showing details of the straight line extraction process procedure in step S 122. In Fig. 11, the estimation calculation unit 18 first performs straight line approximation on the plane point cloud data after the rotation process to obtain a straight line (step S151).

[0064] Next, the estimation calculation unit 18 extracts point cloud data on a straight line from the planar point cloud data after the rotation process (step S152). Next, the estimation calculation unit 18 records the number of reflection points of the point cloud data on the straight line (step S153).

[0065] Next, the estimation calculation unit 18 judges whether the straight line approximation process for the planar point cloud data has been performed a predetermined number of times (for example, 100 times) (step S154). When the estimation calculation unit 18 judges that the straight line approximation process for the planar point cloud data has not been performed a predetermined number of times, it executes the above steps S151 to S153 again.

[0066] When the estimation calculation unit 18 determines that the straight line approximation process has been performed a predetermined number of times on the planar point cloud data, it selects a straight line corresponding to the point cloud data with the largest number of recorded reflection points (step S155).Then, the estimation calculation unit 18 determines the selected straight line as the straight line corresponding to the horizontal line and the vertical line (step S156).

[0067] For example, when the plane point cloud data Df1 shown in Fig. 12(a) is obtained, if the plane point cloud data Df1 is linearly approximated in an odd way, the point cloud may be missing as shown in Fig. 12(b) and (c). Note that the plane point cloud data Df1 is made up of multiple reflection points P1.

[0068] Therefore, as shown in Figures 12(c) and (d), linear approximation is performed on the point cloud data Df2 in which there are missing points, and then, as shown in Figure 12(e), points on the straight line L are extracted from the plane point cloud data Df1. This allows appropriate horizontal and vertical lines to be extracted, making it less likely that errors will occur in the subsequent calculation of the position and orientation of the post pallet 5.

[0069] Returning to Fig. 8, after performing the above-mentioned step S122, the estimation calculation unit 18 calculates the intersection points between the horizontal lines and the vertical lines (step S123). At this time, as shown in Fig. 10, four intersection points I between two horizontal lines La and two vertical lines Lb are obtained.

[0070] Next, the estimation calculation unit 18 calculates the attitude of the post pallet 5 by calculating the attitude of the pillar portion 8 based on the inclination and dimensions of the two vertical lines (step S124). As the attitude of the post pallet 5, the roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 are calculated. The roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 are calculated from the vertical lines corresponding to the pillar portion 8, as shown in Figure 13. Figure 13(a) is a front view of the pillar portion 8. Figure 13(b) is a plan view of the pillar portion 8. Figure 13(c) is a side view of the pillar portion 8.

[0071] The estimation calculation unit 18 prestores reference values ​​such as the dimensions and inclination of the two vertical lines, and calculates the roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 by comparing the dimensions and inclination of the two vertical lines with the reference values. Since the pillars 8 extend long in the vertical direction, the accuracy of the roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 is easy to achieve. However, if the pillars 8 are not completely fixed to the pallet unit 7, the values ​​of the roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 are likely to vary. For this reason, the accuracy of the roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 can be improved by taking the average value of the values ​​of the two vertical lines.

[0072] Next, the estimation calculation unit 18 calculates the position of the post pallet 5 by calculating the position of the pallet unit 7 based on the positions of the four intersections obtained in step S123 (step S125). Specifically, the estimation calculation unit 18 calculates the center position G (see FIG. 2) of the front surface 7a of the pallet unit 7. The center position G of the front surface 7a of the pallet unit 7 is expressed by three-dimensional position coordinates (x0, y0, z0).

[0073] Next, the estimation calculation unit 18 rotates the data of the position and attitude of the post pallet 5 obtained in steps S124 and S125 in reverse so as to return it to the original XYZ coordinate system (step S126). This determines the final position and attitude of the post pallet 5 relative to the forklift 1. At this time, the rotated XYZ coordinate system as shown in Fig. 9(b) is converted to the original XYZ coordinate system as shown in Fig. 9(a). Therefore, the coordinates (x0, y0, z0) of the center position G of the pallet unit 7 in the rotated XYZ coordinate system become the coordinates (x, y, z) of the center position G of the pallet unit 7 in the original XYZ coordinate system.

[0074] Next, the estimation calculation unit 18 makes a final decision as to whether or not the plane detected by the plane detection unit 17 corresponds to the front surface of the post pallet 5 based on data such as the dimensions and inclinations of the horizontal and vertical lines and the intersections of the horizontal and vertical lines (step S127). When the estimation calculation unit 18 determines that the detected plane corresponds to the front surface of the post pallet 5, it outputs data on the position and attitude of the post pallet 5 to the path generation unit 19 (step S128).

[0075] When the estimation calculation unit 18 determines that the detected plane does not correspond to the front surface of the post pallet 5, it erases the plane point cloud data included in the detected plane (step S129). Then, the estimation calculation unit 18 issues a plane redetection instruction to the plane detection unit 17 (step S130). Upon receiving the plane redetection instruction, the plane detection unit 17 detects the plane corresponding to the front surface 7a of the pallet unit 7 and the front surface 8a of the column unit 8 again.

[0076] Returning to FIG. 3, the path generating unit 19 generates a travel path to the loading position (described above) where the forklift 1 loads the goods, based on the data on the position and attitude of the post pallet 5 obtained by the estimation calculation unit 18.

[0077] The travel control unit 20 controls the drive unit 14 so as to automatically drive the forklift 1 to the loading position according to the travel route generated by the route generation unit 19.

[0078] As described above, in this embodiment, the laser sensor 12 irradiates the laser toward the front surface of the post pallet 5, thereby acquiring point cloud data corresponding to the post pallet 5. Then, based on the point cloud data corresponding to the post pallet 5, a plane corresponding to the front surface 7a of the pallet portion 7 and the front surface 8a of the pillar portion 8 in the post pallet 5 is detected. Then, based on the plane corresponding to the front surfaces 7a, 8a, a horizontal line corresponding to the pallet portion 7 and a vertical line corresponding to the pillar portion 8 are extracted. Then, based on the horizontal line and the vertical line, the position and posture of the post pallet 5 relative to the forklift 1 are calculated. At this time, since the pillar portion 8 extends long in the vertical direction, the accuracy of the vertical position and angle of the post pallet 5 is easily obtained. As a result, the position and posture of the post pallet 5 relative to the forklift 1 is estimated with high accuracy.

[0079] Furthermore, in this embodiment, the post pallet 5 is recognized based on image data acquired by the camera 11, and point cloud data corresponding to the recognized post pallet 5 is extracted from the point cloud data acquired by the laser sensor 12. The image data has a higher resolution than the point cloud data. This increases the recognition accuracy of the post pallet 5. As a result, the position and attitude of the post pallet 5 relative to the forklift 1 can be estimated with even higher accuracy.

[0080] In this embodiment, the posture of the pillars 8 is calculated based on the inclination and dimensions of the vertical lines corresponding to the pillars 8, and the position of the pallet unit 7 is calculated based on the position of the intersection between the horizontal line corresponding to the pallet unit 7 and the vertical line corresponding to the pillars 8. Therefore, the position and posture of the post pallet 5 relative to the forklift 1 can be easily calculated.

[0081] In addition, in this embodiment, the first plane is obtained by performing plane approximation to the second point cloud data from which the point cloud has been removed to a thickness greater than the specified thickness, so that it is possible to deal with variations in the second point cloud data when the plane approximation is repeated. Then, the third point cloud data included in the first plane is extracted from the first point cloud data, and the third point cloud data is subjected to plane approximation to a specified thickness to obtain the second plane, so that chipping is less likely to occur in the planes corresponding to the front surface 7a of the pallet unit 7 and the front surface 8a of the column unit 8, and unnecessary point clouds not included in the planes corresponding to the front surfaces 7a, 8a are deleted. In addition, the position and angle of the planes corresponding to the front surfaces 7a, 8a are suppressed. As a result, the position and attitude of the post pallet 5 relative to the forklift 1 are estimated with higher accuracy.

[0082] In this embodiment, a straight line is obtained by performing linear approximation on the plane point cloud data included in the plane corresponding to the front surface 7a of the pallet unit 7 and the front surfaces 8a of the pillar units 8, and then the point cloud data on the straight line is extracted from the plane point cloud data, thereby making it difficult for the horizontal line corresponding to the pallet unit 7 and the vertical line corresponding to the pillar units 8 to be chipped, and eliminating unnecessary point clouds not included in the horizontal and vertical lines. Therefore, the position and attitude of the post pallet 5 relative to the forklift 1 can be estimated with even higher accuracy.

[0083] In this embodiment, the plane point cloud data included in the plane corresponding to the front surface 7a of the pallet unit 7 and the front surface 8a of the column unit 8 is rotated so that the plane point cloud data virtually faces the laser sensor 12, thereby ignoring the front-rear direction (X direction) of the post pallet 5 and determining the intersection points of the horizontal and vertical lines two-dimensionally. Therefore, the intersection points of the horizontal and vertical lines are extracted with high accuracy. Therefore, the position and posture of the post pallet 5 relative to the forklift 1 can be estimated with even higher accuracy.

[0084] Furthermore, in this embodiment, when it is determined that the detected plane does not correspond to the front surface of the post pallet 5, the point cloud data included in the plane is deleted and the plane detection process is executed again, so that an accurate plane corresponding to the front surface 7a of the pallet section 7 and the front surfaces 8a of the pillar sections 8 is obtained. Therefore, the position and attitude of the post pallet 5 relative to the forklift 1 can be estimated with even higher accuracy.

[0085] Furthermore, in this embodiment, even if it is difficult to find the fork holes 9 of the pallet part 7 with the camera 11, for example, because there is dirt in the fork holes 9 of the pallet part 7 or because the fork holes 9 of the pallet part 7 are not visible in a dark place, it is possible to estimate the position and attitude of the post pallet 5 relative to the forklift 1. Also, even if a package M is placed on the pallet part 7 of the post pallet 5, it is possible to estimate the position and attitude of the post pallet 5 including the package M.

[0086] The present invention is not limited to the above embodiment. For example, in the above embodiment, the post pallet 5 is recognized based on image data acquired by the camera 11, and the plane corresponding to the front surface 7a of the pallet unit 7 and the front surface 8a of the column unit 8 is detected based on point cloud data corresponding to the recognized post pallet 5 among the point cloud data acquired by the laser sensor 12, but the present invention is not limited to such a form. For example, when the position of the post pallet 5 is determined, the plane corresponding to the front surfaces 7a, 8a may be detected based only on the point cloud data acquired by the laser sensor 12 without using the camera 11. In this case, the configuration of the position and orientation estimation device 21 is simplified.

[0087] In the above embodiment, a pair of fork holes 9 are provided on the left and right sides of the pallet section 7 of the post pallet 5, but the present invention is not limited to this form and can also be applied to post pallets in which the pallet section does not have fork holes. In this case, one horizontal line corresponding to the pallet section and two vertical lines corresponding to the columns may be extracted based on a plane corresponding to the front faces of the pallet section and the columns.

[0088] In addition, in the above embodiment, the position and posture of the post pallet 5 placed on the truck bed T are estimated, but the present invention is not limited to that form and can also be applied to a post pallet placed on a road surface, a floor surface, etc. [Explanation of symbols]

[0089] 1...forklift, 5...post pallet, 7...pallet section, 7a...front, 8...pillar section, 8a...front, 11...camera (imaging section), 12...laser sensor (laser distance detection section), 15...post pallet recognition section, 16...point cloud processing section, 17...plane detection section, 18...estimation calculation section, 21...position and orientation estimation device, Dp...point cloud data, Dpp...point cloud data, Dp1...first point cloud data, Dp2...second point cloud data, Dp3...third point cloud data, Df...plane point cloud data, Df1...plane point cloud data, Df2...point cloud data, F1...first plane, F2...second plane, L...straight line, La...horizontal line, Lb...vertical line, I...intersection.

Claims

1. A position and orientation estimation device that estimates the position and orientation of a post pallet that is mounted on a forklift and has a pallet unit on which cargo is placed and four pillar units erected at the front, rear, left, and right corners of the pallet unit, comprising: a laser distance detection unit that detects a distance to the post pallet by irradiating a laser toward a front surface of the post pallet and receiving reflected light of the laser, thereby acquiring point cloud data; A plane detection unit that detects a plane corresponding to a front surface of the pallet portion and the column portion based on the point cloud data acquired by the laser distance detection unit; an estimation calculation unit that estimates a position and an attitude of the post pallet relative to the forklift using a plane corresponding to a front surface of the pallet unit and the column unit detected by the plane detection unit, the estimation calculation unit extracts horizontal lines corresponding to the pallet unit and vertical lines corresponding to the pillar unit based on a plane corresponding to the front faces of the pallet unit and the pillar unit, and calculates a position and attitude of the post pallet relative to the forklift based on the horizontal lines and the vertical lines; The estimation calculation unit calculates the attitude of the pillar part based on the inclination and dimensions of the vertical line, thereby calculating the attitude of the post pallet relative to the forklift, and also calculates the position of the post pallet relative to the forklift by finding the intersection between the horizontal line and the vertical line and calculating the position of the pallet part based on the position of the intersection.This is a position and attitude estimation device.

2. A position and attitude estimation device that is mounted on a forklift and estimates the position and attitude of a post pallet having a pallet section on which cargo is placed and four pillar sections erected at the front, rear, left and right corners of the pallet section, a laser distance detection unit that detects a distance to the post pallet by irradiating a laser toward a front surface of the post pallet and receiving reflected light of the laser, thereby acquiring point cloud data; A plane detection unit that detects a plane corresponding to a front surface of the pallet portion and the column portion based on the point cloud data acquired by the laser distance detection unit; an estimation calculation unit that estimates a position and an attitude of the post pallet relative to the forklift using a plane corresponding to a front surface of the pallet unit and the column unit detected by the plane detection unit, the plane detection unit obtains second point cloud data in which point clouds have been removed from first point cloud data corresponding to the post pallet, performs plane approximation on the second point cloud data to a thickness greater than a specified thickness to obtain a first plane, then extracts third point cloud data included in the first plane from the first point cloud data, and performs plane approximation on the third point cloud data to the specified thickness to obtain a second plane, thereby detecting a plane corresponding to the front faces of the pallet portion and the column portion; The estimation calculation unit extracts horizontal lines corresponding to the pallet unit and vertical lines corresponding to the pillar unit based on a plane corresponding to the front surface of the pallet unit and the pillar unit, and calculates the position and attitude of the post pallet relative to the forklift based on the horizontal lines and vertical lines.

3. A position and attitude estimation device that is mounted on a forklift and estimates the position and attitude of a post pallet having a pallet section on which a load is placed and four pillar sections erected at the front, rear, left and right corners of the pallet section, a laser distance detection unit that detects a distance to the post pallet by irradiating a laser toward a front surface of the post pallet and receiving reflected light of the laser, thereby acquiring point cloud data; A plane detection unit that detects a plane corresponding to a front surface of the pallet portion and the column portion based on the point cloud data acquired by the laser distance detection unit; an estimation calculation unit that estimates a position and an attitude of the post pallet relative to the forklift using a plane corresponding to a front surface of the pallet unit and the column unit detected by the plane detection unit, the estimation calculation unit extracts horizontal lines corresponding to the pallet unit and vertical lines corresponding to the pillar unit based on a plane corresponding to the front faces of the pallet unit and the pillar unit, and calculates a position and attitude of the post pallet relative to the forklift based on the horizontal lines and the vertical lines; The estimation calculation unit performs linear approximation on planar point cloud data included in a plane corresponding to the front surface of the pallet section and the column section to obtain a straight line, and then extracts point cloud data that is on the straight line from the planar point cloud data, thereby extracting the horizontal line and the vertical line.

4. A position and attitude estimation device that is mounted on a forklift and estimates the position and attitude of a post pallet having a pallet section on which a load is placed and four pillar sections erected at the front, rear, left and right corners of the pallet section, a laser distance detection unit that detects a distance to the post pallet by irradiating a laser toward a front surface of the post pallet and receiving reflected light of the laser, thereby acquiring point cloud data; A plane detection unit that detects a plane corresponding to a front surface of the pallet portion and the column portion based on the point cloud data acquired by the laser distance detection unit; an estimation calculation unit that estimates a position and an attitude of the post pallet relative to the forklift using a plane corresponding to a front surface of the pallet unit and the column unit detected by the plane detection unit, the estimation calculation unit extracts horizontal lines corresponding to the pallet unit and vertical lines corresponding to the pillar unit based on a plane corresponding to the front faces of the pallet unit and the pillar unit, and calculates a position and attitude of the post pallet relative to the forklift based on the horizontal lines and the vertical lines; The estimation calculation unit rotates the planar point cloud data so that the planar point cloud data included in the plane corresponding to the front of the pallet section and the column section is virtually facing the laser distance detection unit, thereby transforming the coordinate system of the planar point cloud data, and calculates the position and attitude of the post pallet relative to the forklift based on the horizontal and vertical lines, and then rotates the position and attitude of the post pallet back to the original coordinate system, thereby determining the final position and attitude of the post pallet relative to the forklift. This is a position and attitude estimation device.

5. A position and attitude estimation device that is mounted on a forklift and estimates the position and attitude of a post pallet having a pallet section on which a load is placed and four pillar sections erected at the front, rear, left and right corners of the pallet section, a laser distance detection unit that detects a distance to the post pallet by irradiating a laser toward a front surface of the post pallet and receiving reflected light of the laser, thereby acquiring point cloud data; A plane detection unit that detects a plane corresponding to a front surface of the pallet portion and the column portion based on the point cloud data acquired by the laser distance detection unit; an estimation calculation unit that estimates a position and an attitude of the post pallet relative to the forklift using a plane corresponding to a front surface of the pallet unit and the column unit detected by the plane detection unit, the estimation calculation unit extracts horizontal lines corresponding to the pallet unit and vertical lines corresponding to the pillar unit based on a plane corresponding to the front faces of the pallet unit and the pillar unit, and calculates a position and attitude of the post pallet relative to the forklift based on the horizontal lines and the vertical lines; After calculating the position and attitude of the post pallet relative to the forklift, the estimation calculation unit makes a final determination as to whether the plane detected by the plane detection unit corresponds to the front surface of the post pallet based on the inclination and dimensions of the horizontal and vertical lines, and if it determines that the plane detected by the plane detection unit does not correspond to the front surface of the post pallet, it deletes the point cloud data contained in the plane detected by the plane detection unit and causes the plane detection unit to re-execute the detection process of the plane corresponding to the front surfaces of the pallet section and the column section.This is a position and attitude estimation device.

6. An imaging unit that captures an image of the front surface of the post pallet to acquire image data; a post pallet recognition unit that recognizes the post pallet based on image data acquired by the imaging unit; a point cloud processing unit that extracts point cloud data corresponding to the post pallet recognized by the post pallet recognition unit from the point cloud data acquired by the laser distance detection unit, 6. The position and orientation estimation device according to claim 1, wherein the plane detection unit detects a plane corresponding to a front surface of the pallet portion and the column portion based on the point cloud data extracted by the point cloud processing unit.

Images