Position and posture estimation device

Abstract

To provide a position posture estimation device that can estimate a position and posture of a pallet with respect to a fork while the pallet is lifted by the fork.SOLUTION: A position posture estimation device 31 comprises a distance measuring sensor 22 that detects a distance from a lower part of a pallet 12 lifted by a fork 9 to a bottom surface 12b of the pallet 12, a corner detecting unit 32 that detects a position of a corner on the bottom surface 12b of the pallet 12 based on the detection data of the distance measuring sensor 22, and an estimation calculation unit 29 that performs estimation calculation of the position and posture of the pallet 12 with respect to the fork 9 based on the positions of the corner on the bottom surface 12b of the pallet 12 detected by the corner detecting unit 32.SELECTED DRAWING: Figure 4

Description

[Technical field]

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

[0002] Known conventional position and orientation estimation devices include, for example, the technology described in Patent Document 1. The position and orientation estimation device described in Patent Document 1 acquires distance data using a range sensor that irradiates laser light and measures the distance to surrounding objects from the reflected light of the irradiated laser light, converts the distance data into a group of observation points in a three-dimensional space, and identifies the position and orientation of a pallet placed in front of a forklift based on the group of observation points. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2017-178567 A Summary of the Invention [Problem to be solved by the invention]

[0004] However, the position and orientation estimation device described in the above prior art is a technology for estimating the position and orientation of a pallet placed in front of a forklift before the pallet is lifted by the forks of the forklift, and does not assume that the position and orientation of the pallet will be estimated when the pallet has been lifted by the forks. Also, in the position and orientation estimation device described in the above prior art, because a range sensor is attached to the fork, it is difficult to estimate the position and orientation of the pallet relative to the fork based on the measurement values ​​of the range sensor when the pallet is lifted by the fork.

[0005] An object of the present invention is to provide a position and orientation estimation device that can estimate the position and orientation of a pallet with respect to the forks in a state in which the pallet is lifted by the forks. [Means for solving the problem]

[0006] One aspect of the present invention is a position and orientation estimation device that estimates the position and orientation of a pallet lifted by the forks of a forklift, and includes a distance detection unit that detects the distance from below the pallet lifted by the forks to the underside of the pallet, a corner detection unit that detects the positions of corners on the underside of the pallet based on detection data from the distance detection unit, and an estimation calculation unit that performs an estimation calculation of the position and orientation of the pallet relative to the forks based on the positions of the corners on the underside of the pallet detected by the corner detection unit.

[0007] In such a position and orientation estimation device, the distance detection unit detects the distance from the bottom of the pallet to the bottom surface of the pallet when the pallet is lifted by the forks of the forklift. Then, the positions of the corners on the bottom surface of the pallet are detected based on the detection data from the distance detection unit. Then, an estimation calculation is performed on the position and orientation of the pallet relative to the forks based on the positions of the corners on the bottom surface of the pallet. Since the distance detection unit detects the distance from the bottom of the pallet to the bottom surface of the pallet in this manner, the positions of the corners on the bottom surface of the pallet are detected regardless of whether or not a load is placed on the top surface of the pallet. This makes it possible to estimate the position and orientation of the pallet relative to the forks when the pallet is lifted by the forks. In addition, by detecting the distance from the bottom of the pallet to the bottom surface of the pallet by the distance detection unit, it is not necessary to place a distance detection unit on the outside of the body of the forklift in order to detect the distance to the side of the pallet. Therefore, the position and orientation of the pallet relative to the forks can be estimated without increasing the vehicle width of the entire forklift.

[0008] The corner detection unit may estimate a number of straight lines corresponding to adjacent side surfaces of the pallet based on the detection data of the distance detection unit, calculate the position of the intersection of the multiple straight lines as the position of the corner on the underside of the pallet, and the estimation calculation unit may perform an estimation calculation of the position and orientation of the pallet relative to the forks based on the position of the intersection. In this configuration, the position of the intersection of the multiple straight lines corresponding to adjacent side surfaces of the pallet is detected as the position of the corner on the underside of the pallet. Therefore, even if the corners of the actual pallet are rounded, the position and orientation of the pallet relative to the forks can be estimated with high accuracy.

[0009] The corner detection unit may detect the positions of the left and right corners on the underside of the pallet based on detection data from the distance detection unit, and the estimation calculation unit may perform estimation calculation of the position and orientation of the pallet relative to the forks based on the positions of the left and right corners on the underside of the pallet. In this configuration, by detecting the positions of the left and right corners on the underside of the pallet, it is possible to estimate with high accuracy the position and orientation of the pallet relative to the forks even if the size of the pallet is unknown.

[0010] The corner detection unit may estimate a first straight line corresponding to a first side surface of the pallet perpendicular to the fork insertion direction and two second straight lines corresponding to second side surfaces on both the left and right sides adjacent to the first side surface based on the detection data of the distance detection unit, and calculate the positions of the two intersection points of the first straight line and the two second straight lines as the positions of the left and right corners on the underside of the pallet, and the estimation calculation unit may perform an estimation calculation of the position and attitude of the pallet relative to the forks based on the positions of the two intersection points. In such a configuration, the positions of the two intersection points of the first straight line corresponding to the first side surface of the pallet and the two second straight lines corresponding to the second side surfaces on both the left and right sides of the pallet are detected as the positions of the left and right corners on the underside of the pallet. Therefore, even if the corners of the actual pallet are rounded, the position and attitude of the pallet relative to the forks can be estimated with high accuracy.

[0011] The estimation calculation unit may calculate the center position of the pallet based on the positions of the two intersections on the left and right, and calculate the position of the pallet relative to the forks from the center position of the pallet. In this configuration, by calculating the center position of the pallet, the position of the pallet relative to the forks can be estimated by simple calculation processing.

[0012] The estimation calculation unit may calculate the yaw angle of the pallet based on the positions of the two left and right intersections, the first line, or the second line, and calculate the attitude of the pallet relative to the forks from the yaw angle of the pallet. In this configuration, by calculating the yaw angle of the pallet, the attitude of the pallet relative to the forks can be estimated by simple calculation processing.

[0013] The corner detection unit may detect the position of a corner on the underside of the pallet that is closer to the forklift body, and the estimation calculation unit may perform an estimation calculation of the position and orientation of the pallet relative to the forks based on the position of the corner on the underside of the pallet that is closer to the vehicle body. In this configuration, the position of the corner closest to the vehicle body out of the four corners on the underside of the pallet is used in the estimation calculation of the position and orientation of the pallet, so that the position and orientation of the pallet relative to the forks can be estimated with high accuracy. Effect of the Invention

[0014] According to the present invention, the position and orientation of the pallet relative to the forks can be estimated when the pallet is lifted by the forks. [Brief description of the drawings]

[0015] [Figure 1] 1 is a perspective view showing a forklift to which a position and orientation estimation device according to an embodiment of the present invention is applied; [Diagram 2] 2 is a side view showing a state in which the forks of the forklift shown in FIG. 1 have lifted up a pallet. [Diagram 3] 2 is a plan view showing a state in which the forks of the forklift shown in FIG. 1 have lifted up the pallet. [Figure 4] 1 is a block diagram showing a configuration of a cargo handling control device including a position and attitude estimation device according to an embodiment of the present invention; [Diagram 5] 5 is a flowchart showing a procedure of a control process executed by a controller shown in FIG. 4. [Figure 6] A diagram showing a method for detecting both left and right corners on the underside of a pallet based on detection data from a distance measuring sensor. [Figure 7] FIG. 2 is a plan view showing a state in which a forklift is placing a load in a load storage area. [Figure 8] A diagram showing a modified example of a method for detecting both left and right corners on the underside of a pallet based on detection data from a distance measuring sensor. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0016] Hereinafter, an embodiment of the present invention will be described in detail with reference to the drawings. In the drawings, the same or equivalent elements are designated by the same reference numerals, and duplicated explanations will be omitted.

[0017] Fig. 1 is a perspective view showing a forklift to which a position and attitude estimation device according to an embodiment of the present invention is applied. In Fig. 1, the forklift 1 includes a traveling device 2 and a loading device 3 that is disposed in front of the traveling device 2 and performs loading and unloading.

[0018] The traveling device 2 has a body 4, front wheels 5 which are a pair of left and right drive wheels arranged at the front of the body 4, and rear wheels 6 which are a pair of left and right steered wheels arranged at the rear of the body 4.

[0019] The cargo handling device 3 has a mast 7 erected at the front end of the vehicle body 4, a pair of left and right forks 9 attached to the mast 7 via lift brackets 8, a lift cylinder 10 for raising and lowering the forks 9, and a tilt cylinder 11 for tilting the mast 7. The forks 9 hold a pallet 12, as shown in Figures 2 and 3.

[0020] The pallet 12 is a loading platform for placing cargo M on. The type of pallet 12 includes a flat pallet and a sheet pallet. The pallet 12 has four corners 13. Each corner 13 is rounded by R processing. The pallet 12 also has two fork holes 14 into which the forks 9 are inserted.

[0021] The pallet 12 has an upper surface 12a, a lower surface 12b, and four side surfaces 12c to 12f. The upper surface 12a and the lower surface 12b face each other. The upper surface 12a and the lower surface 12b have a substantially rectangular shape. The side surfaces 12c to 12f are provided between the upper surface 12a and the lower surface 12b.

[0022] The side surfaces 12c and 12d face each other. The side surfaces 12c and 12d are first side surfaces perpendicular to the insertion direction of the fork 9. In other words, the fork hole 14 extends in the opposing direction of the side surfaces 12c and 12d. The side surface 12c is the side surface on the near side (front side) with respect to the body 4 of the forklift 1. The side surface 12d is the side surface on the far side (rear side) with respect to the body 4.

[0023] The side surfaces 12e and 12f face each other in a direction perpendicular to the opposing direction of the side surfaces 12c and 12d. The side surfaces 12e and 12f are second side surfaces on both the left and right sides adjacent to the side surfaces 12c and 12d, respectively. The side surfaces 12e and 12f are parallel to the insertion direction of the fork 9.

[0024] Fig. 4 is a block diagram showing the configuration of a cargo handling control device equipped with a position and attitude estimation device according to an embodiment of the present invention. In Fig. 2, the cargo handling control device 20 is a device that places a pallet 12 held by forks 9 in a predetermined cargo placement area Z (see Fig. 7), that is, performs cargo placement. The cargo handling control device 20 places the cargo by automatically operating a forklift 1.

[0025] The cargo handling control device 20 includes an instruction switch 21, two distance measuring sensors 22, a controller 23, and a drive unit 24. The distance measuring sensor 22, the controller 23, and the drive unit 24 are mounted on the forklift 1.

[0026] The instruction switch 21 is a manually operated switch for an operator to instruct a load placing operation. The instruction switch 21 may be mounted on the driver's seat of the forklift 1, or may be a portable terminal or the like carried by the operator.

[0027] 1 to 3, the distance measuring sensors 22 are disposed on both the left and right sides of the forklift 1. The distance measuring sensors 22 are attached, for example, to a position above the front wheels 5 of the vehicle body 4. The distance measuring sensors 22 are distance detection units that detect the distance from the bottom of the pallet 12 lifted by the forks 9 to the underside 12b of the pallet 12.

[0028] A LIDAR, a laser range finder, a Time of Flight (TOF) camera, or the like is used as the distance measurement sensor 22. The distance measurement sensor 22 irradiates a laser light L toward the underside 12b of the pallet 12 and receives the reflected light of the laser light L to detect the distance to the underside 12b of the pallet 12 and obtains point cloud data. The point cloud data is detection data of the distance measurement sensor 22. The point cloud is a collection of reflection points of the laser light L.

[0029] The distance measuring sensor 22 disposed on the left side of the forklift 1 irradiates the laser light L mainly to a left area on the underside 12b of the pallet 12. The distance measuring sensor 22 disposed on the right side of the forklift 1 irradiates the laser light L mainly to a right area on the underside 12b of the pallet 12.

[0030] The controller 23 is composed of a CPU, RAM, ROM, an input / output interface, etc. When a load-placing operation is instructed by the instruction switch 21, the controller 23 estimates the position and attitude of the pallet 12 relative to the forks 9 based on the detection signal of the distance measuring sensor 22, and controls the lift cylinder 10 and the drive unit 24 to place the load on the pallet 12.

[0031] Although not specifically shown, the drive unit 24 has, for example, a travel motor that rotates the front wheels 5 that are drive wheels, and a steering motor that steers the rear wheels 6 that are steered wheels.

[0032] The controller 23 has an ascent control unit 25 , a pallet point cloud extraction unit 26 , a straight line estimation unit 27 , an intersection calculation unit 28 , an estimation calculation unit 29 , and a load placement control unit 30 .

[0033] The distance measurement sensor 22, the pallet point cloud extraction unit 26, the straight line estimation unit 27, the intersection calculation unit 28 and the estimation calculation unit 29 of the controller 23 constitute a position and orientation estimation device 31 that estimates the position and orientation of the pallet 12 lifted by the forks 9 of the forklift 1.

[0034] When a load-placing operation is instructed by the instruction switch 21, the ascent control section 25 controls the lift cylinder 10 so that the pallet 12 lifted by the forks 9 rises to a detection position higher than the distance measuring sensor 22. The detection position is a position where the distance measuring sensor 22 detects the distance to the underside 12b of the pallet 12, as shown in Fig. 2, and thereby the corners of the underside 12b of the pallet 12 can be detected.

[0035] The pallet point cloud extraction unit 26 extracts a point cloud corresponding to the lower surface 12b of the pallet 12 based on the point cloud data acquired by the distance measurement sensor 22.

[0036] The straight line estimation unit 27 estimates multiple (here, three) straight lines corresponding to adjacent side surfaces 12c, 12e, and 12f of the pallet 12 based on the point cloud corresponding to the bottom surface 12b of the pallet 12 extracted by the pallet point cloud extraction unit 26.

[0037] The intersection calculation unit 28 calculates the positions of the intersections of the multiple straight lines extracted by the straight line estimation unit 27 as the positions of the corners on the lower surface 12b of the pallet 12.

[0038] The pallet point cloud extraction unit 26, the straight line estimation unit 27 and the intersection calculation unit 28 constitute a corner detection unit 32 that detects the positions of corners on the lower surface 12b of the pallet 12 based on the detection data of the distance measurement sensor 22.

[0039] The estimation calculation unit 29 performs estimation calculation of the position and attitude of the pallet 12 relative to the forks 9, based on the positions of the corners on the underside 12b of the pallet 12 detected by the corner detection unit 32. The position and attitude of the pallet 12 are two-dimensional position and attitude of the pallet 12 with the specified position of the forklift 1 as the origin.

[0040] The loading control unit 30 controls the lift cylinder 10 and the drive unit 24 so that the pallet 12 is placed in the appropriate position and attitude in the loading area Z (see Figure 7) based on the position and attitude of the pallet 12 relative to the forks 9 estimated by the estimation calculation unit 29.

[0041] Fig. 5 is a flowchart showing the procedure of the control process executed by the controller 23. In Fig. 5, the controller 23 first determines whether or not a loading operation of the pallet 12 held by the forks 9 has been instructed based on the operation signal of the instruction switch 21 (step S101).

[0042] When the controller 23 determines that a load-placing operation of the pallet 12 held by the forks 9 has been instructed, it controls the lift cylinder 10 so that the pallet 12 is lifted to the detection position (described above) (step S102).

[0043] Next, the controller 23 causes the distance measuring sensor 22 to irradiate the laser light L onto the lower surface 12b of the pallet 12, thereby acquiring detection data from the distance measuring sensor 22 (step S103).

[0044] Then, the controller 23 extracts a point cloud corresponding to the lower surface 12b of the pallet 12 based on the detection data of the distance measurement sensor 22 (step S104). At this time, the controller 23 extracts a point cloud G corresponding to the lower surface 12b of the pallet 12 from the point cloud data acquired by the distance measurement sensor 22, as shown in FIG. 6(a).

[0045] Next, the controller 23 estimates three straight lines corresponding to the side surfaces 12c, 12e, and 12f of the pallet 12 based on the point group corresponding to the bottom surface 12b of the pallet 12 (step S105). At this time, the controller 23 estimates a straight line L1 (first straight line) corresponding to the side surface 12c of the pallet 12 and two straight lines L2 and L3 (second straight lines) corresponding to the sides 12e and 12f of the pallet 12, for example, using RANSAC (Random sample consensus) or a least squares method, as shown in FIG. 6(b).

[0046] Next, the controller 23 obtains two intersections, one on the left and one on the right, between a line corresponding to the side surface 12c of the pallet 12 and a line corresponding to the side surfaces 12e, 12f of the pallet 12, and calculates the positions of the two intersections as the positions of the corners on the bottom surface 12b of the pallet 12 (step S106). Here, as shown in Fig. 6(b), the positions of intersections P1, P2 between a line L1 corresponding to the side surface 12c of the pallet 12 and lines L2, L3 corresponding to the side surfaces 12e, 12f of the pallet 12 are calculated as the positions of the corners on the bottom surface 12b of the pallet 12.

[0047] Next, the controller 23 calculates the center position of the pallet 12 based on the positions of the two intersections on the left and right (step S107). The center position of the pallet 12 corresponds to the center position C in the vehicle width direction of the front edge of the lower surface 12b of the pallet 12 with respect to the vehicle body 4, as shown in FIG. 6(b).

[0048] Furthermore, the controller 23 calculates the yaw angle of the pallet 12 based on the intersection between the two left and right intersection positions (step S108). The yaw angle of the pallet 12 is the rotation angle of the pallet 12 about the up-down axis.

[0049] Next, the controller 23 calculates the position of the pallet 12 relative to the forks 9 from the center position of the pallet 12, and calculates the attitude of the pallet 12 relative to the forks 9 from the yaw angle of the pallet 12 (step S109).

[0050] Next, the controller 23 determines whether the position and attitude of the pallet 12 relative to the forks 9 deviate from a reference value (step S110). The reference value is a value that ensures that the position and attitude of the pallet 12 when placed is within an allowable range.

[0051] When the controller 23 determines that the position and attitude of the pallet 12 relative to the forks 9 deviate from the reference values, the controller 23 controls the drive unit 24 to correct the position and attitude of the pallet 12 (step S111). For example, the controller 23 controls the drive unit 24 to change the travel route of the forklift 1 to correct the position and attitude of the pallet 12.

[0052] If the forklift 1 is equipped with a side shift cylinder for shifting the forks 9 to both the left and right sides, the position of the pallet 12 relative to the forks 9 may be corrected by controlling the side shift cylinder.

[0053] When the controller 23 determines in step S110 that the position and attitude of the pallet 12 relative to the forks 9 have not deviated from the reference values, or after executing step S111, it controls the drive unit 24 so that the forklift 1 travels to the loading area Z (see Figure 7), and controls the lift cylinder 10 so that the forks 9 are lowered to place the pallet 12 at the loading area Z (step S112).

[0054] In the above, the ascent control unit 25 executes the above steps S101 and S102. The pallet point cloud extraction unit 26 executes the above steps S103 and S104. The straight line estimation unit 27 executes the above step S105. The intersection calculation unit 28 executes the above step S106. The estimation calculation unit 29 executes the above steps S107 to S109. The loading control unit 30 executes the above steps S110 to S112.

[0055] However, when placing the pallet 12, if the position and posture of the pallet 12 relative to the forks 9 are misaligned as shown in Figure 7, even if the forklift 1 is automatically driven to the front of the loading area Z, when the forks 9 are lowered by the lift cylinder 10, the pallet 12 will be misaligned from its correct position and posture in the loading area Z.

[0056] In order to solve such a problem, it is conceivable to estimate the position and orientation of the pallet 12 relative to the forks 9 by irradiating the top surface 12a of the pallet 12 with a laser light L from the distance measurement sensor 22. However, as shown in Fig. 7, when a large load M is placed on the top surface 12a of the pallet 12, the top surface 12a of the pallet 12 is hidden by the load M, and the pallet 12 is not detected by the distance measurement sensor 22. Therefore, the position and orientation of the pallet 12 relative to the forks 9 cannot be estimated.

[0057] It is also possible to estimate the position and attitude of the pallet 12 relative to the forks 9 by irradiating the side surfaces 12e, 12f of the pallet 12 with laser light L from the distance measurement sensor 22. In this case, however, it is necessary to dispose the distance measurement sensor 22 on the outer side of the pallet 12 in the vehicle width direction, and therefore the position of the distance measurement sensor 22 may have to be on the outer side of the body 4 of the forklift 1 in the vehicle width direction. This may result in the overall width of the forklift 1 becoming wider.

[0058] Therefore, in this embodiment, when the pallet 12 is lifted by the forks 9 of the forklift 1, the distance measuring sensor 22 detects the distance from the bottom of the pallet 12 to the bottom surface 12b of the pallet 12. Then, based on the detection data of the distance measuring sensor 22, the positions of the corners on the bottom surface 12b of the pallet 12 are detected. Then, based on the positions of the corners on the bottom surface 12b of the pallet 12, an estimation calculation is performed for the position and attitude of the pallet 12 relative to the forks 9. Since the distance measuring sensor 22 detects the distance from the bottom of the pallet 12 to the bottom surface 12b of the pallet 12 in this manner, the positions of the corners on the bottom surface 12b of the pallet 12 are detected regardless of whether or not the goods M are placed on the top surface 12a of the pallet 12. As a result, when the pallet 12 is lifted by the forks 9, the position and attitude of the pallet 12 relative to the forks 9 can be estimated. In addition, by detecting the distance from below the pallet 12 to the underside 12b of the pallet 12 with the distance measuring sensor 22, it is not necessary to place the distance measuring sensor 22 on the outside of the body 4 of the forklift 1 in order to detect the distance to the sides 12e, 12f of the pallet 12. Therefore, the position and attitude of the pallet 12 relative to the forks 9 can be estimated without increasing the width of the entire forklift 1.

[0059] Furthermore, in this embodiment, the positions of the left and right corners of the underside 12b of the pallet 12 are detected based on detection data from the distance measurement sensor 22, and an estimation calculation is performed of the position and orientation of the pallet 12 relative to the forks 9 based on the positions of the left and right corners of the underside 12b of the pallet 12. By detecting the positions of the left and right corners of the underside 12b of the pallet 12 in this manner, the position and orientation of the pallet 12 relative to the forks 9 can be estimated with high accuracy even if the size of the pallet 12 is unknown.

[0060] In this embodiment, based on the detection data of the distance measuring sensor 22, a straight line L1 corresponding to the side surface 12c of the pallet 12 perpendicular to the insertion direction of the fork 9 and straight lines L2 and L3 corresponding to the left and right side surfaces 12e and 12f adjacent to the side surface 12c are estimated, and the positions of two intersection points P1 and P2 between the straight line L1 and the straight lines L2 and L3 are calculated as the positions of the left and right corners of the lower surface 12b of the pallet 12. Therefore, the positions of two intersection points P1 and P2 between the straight line L1 corresponding to the side surface 12c of the pallet 12 and the straight lines L2 and L3 corresponding to the left and right side surfaces 12e and 12f of the pallet 12 are detected as the positions of the left and right corners of the lower surface 12b of the pallet 12. Therefore, even if the corners of the actual pallet 12 are rounded, the position and attitude of the pallet 12 relative to the fork 9 can be estimated with high accuracy.

[0061] Furthermore, in this embodiment, the center position C of the pallet 12 is calculated based on the positions of the two left and right intersection points P1, P2, and the position of the pallet 12 relative to the forks 9 is calculated from the center position C of the pallet 12. By calculating the center position C of the pallet 12 in this manner, the position of the pallet 12 relative to the forks 9 can be estimated by simple calculation processing.

[0062] Furthermore, in this embodiment, the yaw angle of the pallet 12 is calculated based on the positions of the two left and right intersection points P1, P2, and the attitude of the pallet 12 relative to the forks 9 is calculated from the yaw angle of the pallet 12. By calculating the yaw angle of the pallet 12 in this manner, the attitude of the pallet 12 relative to the forks 9 can be estimated by simple calculation processing.

[0063] Furthermore, in this embodiment, the position of the corner on the underside 12b of the pallet 12 that is on the near side with respect to the vehicle body 4 of the forklift 1 is detected, and an estimation calculation is performed for the position and attitude of the pallet 12 relative to the forks 9 based on the position of the corner on the underside 12b of the pallet 12 that is on the near side with respect to the vehicle body 4. Therefore, the position of the corner closest to the vehicle body 4 out of the four corners on the underside 12b of the pallet 12 is used in the estimation calculation for the position and attitude of the pallet 12, so that the position and attitude of the pallet 12 relative to the forks 9 can be estimated with even higher accuracy.

[0064] The present invention is not limited to the above embodiment. For example, in the above embodiment, the yaw angle of the pallet 12 is calculated based on the positions of the intersections P1 and P2 between the straight line L1 corresponding to the side surface 12c of the pallet 12 and the straight lines L2 and L3 corresponding to the side surfaces 12e and 12f of the pallet 12. However, the present invention is not limited to this embodiment, and the yaw angle of the pallet 12 may be calculated based on at least one of the straight lines L1 to L3.

[0065] In the above embodiment, the distance measuring sensor 22 is disposed on both the left and right sides of the forklift 1, but the present invention is not limited to such an embodiment. The number of distance measuring sensors 22 may be one as long as it is possible to detect the distance from the bottom of the pallet 12 lifted by the forks 9 to the underside 12b of the pallet 12 and to detect the positions of the corners on both the left and right sides of the underside 12b of the pallet 12. In this case, the distance measuring sensor 22 may be disposed on either the left or right side of the forklift 1, or may be disposed in the center of the forklift 1 in the vehicle width direction.

[0066] In the above embodiment, the positions of both the left and right corners of the underside 12b of the pallet 12 are detected based on the detection data of the distance measuring sensor 22, but this is not particularly limited to this embodiment. If the length and width of the pallet 12 are known, as shown in Figure 8, the position of only one of the left and right corners of the underside 12b of the pallet 12 may be detected based on the detection data of one distance measuring sensor 22.

[0067] 8, a straight line L1 corresponding to the side surface 12c of the pallet 12 and a straight line L2 corresponding to the side surface 12f of the pallet 12 are estimated, the position of the intersection point P of these straight lines L1, L2 is calculated, and the position and attitude of the pallet 12 relative to the forks 9 are estimated based on the position of the intersection point P. Specifically, a center position C of the pallet 12 is calculated based on the position of the intersection point P and the vertical and horizontal sizes of the pallet 12, and a yaw angle of the pallet 12 is calculated based on at least one of the straight lines L1, L2.

[0068] In this modified example, the position of intersection P of straight lines L1, L2 corresponding to adjacent side surfaces 12c, 12f of the pallet 12 is detected as the position of a corner on the underside 12b of the pallet 12. Therefore, even if the corners 13 of the actual pallet 12 are rounded, the position and attitude of the pallet 12 relative to the forks 9 can be estimated.

[0069] In addition, in the above embodiment, the position of the corner on the front side (front side) of the underside 12b of the pallet 12 relative to the vehicle body 4 of the forklift 1 is detected, but this is not particularly limited to this form, and the position of the corner on the rear side (rear side) of the underside 12b of the pallet 12 relative to the vehicle body 4 may also be detected.

[0070] In addition, in the above embodiment, the center position C of the pallet 12 is calculated, and the position of the pallet 12 relative to the forks 9 is calculated from the center position C of the pallet 12, but this is not limited to a particular form, and the position of the pallet 12 relative to the forks 9 may be calculated from a position other than the center position C of the pallet 12, for example, from the positions of both ends of the pallet 12.

[0071] In addition, in the above embodiment, when the instruction switch 21 instructs the loading operation of the pallet 12, the pallet 12 is lifted by the lift cylinder 10. However, this is not limited to this form. For example, when the loading operation of the pallet 12 is instructed by a management device of a higher-level system that manages the automatic operation of the forklift 1, the pallet 12 may be lifted by the lift cylinder 10. [Explanation of symbols]

[0072] 1...forklift, 4...vehicle body, 9...fork, 12...pallet, 12b...underside, 12c...side (first side), 12e, 12f...side (second side), 22...distance measuring sensor (distance detection unit), 26...pallet point cloud extraction unit, 27...straight line estimation unit, 28...intersection calculation unit, 29...estimation calculation unit, 31...position and orientation estimation device, 32...corner detection unit, C...center position, L1...straight line (first line), L2, L3...straight lines (second line), P...intersection, P1, P2...intersection.

Claims

1. A position and orientation estimation device that estimates a position and orientation of a pallet lifted by the forks of a forklift, comprising: a distance detection unit that detects a distance from a lower portion of the pallet lifted by the forks to a lower surface of the pallet; A corner detection unit that detects the position of a corner on the underside of the pallet based on detection data from the distance detection unit; an estimation calculation unit that performs an estimation calculation of the position and attitude of the pallet relative to the forks based on the positions of the corners on the underside of the pallet detected by the corner detection unit, The corner detection unit estimates a plurality of straight lines corresponding to adjacent side surfaces of the pallet based on the detection data of the distance detection unit, and calculates the position of an intersection of the plurality of straight lines as the position of a corner on the underside of the pallet, The estimation calculation unit is a position and orientation estimation device that performs estimation calculation of the position and orientation of the pallet relative to the forks based on the position of the intersection.

2. A position and orientation estimation device that estimates the position and orientation of a pallet lifted by the forks of a forklift, comprising: a distance detection unit that detects a distance from a lower portion of the pallet lifted by the forks to a lower surface of the pallet; A corner detection unit that detects the position of a corner on the underside of the pallet based on detection data from the distance detection unit; an estimation calculation unit that performs an estimation calculation of the position and attitude of the pallet relative to the forks based on the positions of the corners on the underside of the pallet detected by the corner detection unit, The corner detection unit detects the positions of the left and right corners on the underside of the pallet based on the detection data of the distance detection unit, The estimation calculation unit is a position and orientation estimation device that performs estimation calculation of the position and orientation of the pallet relative to the forks based on the positions of the left and right corners of the underside of the pallet.

3. The corner detection unit estimates a first straight line corresponding to a first side surface of the pallet perpendicular to the insertion direction of the fork and two second straight lines corresponding to second side surfaces on both the left and right sides adjacent to the first side surface based on the detection data of the distance detection unit, and calculates the positions of two intersections between the first straight line and the two second straight lines as the positions of the left and right corners on the underside of the pallet, 3. The position and orientation estimation device according to claim 2, wherein the estimation calculation unit estimates the position and orientation of the pallet relative to the forks based on the positions of the two intersections on the left and right.

4. 4. The position and orientation estimation device according to claim 3, wherein the estimation calculation unit calculates a center position of the pallet based on the positions of the two intersections on the left and right, and calculates the position of the pallet with respect to the forks from the center position of the pallet.

5. 5. The position and orientation estimation device according to claim 3, wherein the estimation calculation unit calculates a yaw angle of the pallet based on the positions of the two left and right intersections, the first line, or the second line, and calculates the attitude of the pallet with respect to the forks from the yaw angle of the pallet.

6. The corner detection unit detects the position of a corner on the underside of the pallet that is on the front side with respect to a vehicle body of the forklift, 6. The position and orientation estimation device according to claim 1, wherein the estimation calculation unit estimates the position and orientation of the pallet relative to the forks based on the position of a corner of the underside of the pallet that is on the front side relative to the vehicle body.

7. A position and orientation estimation device that estimates a position and orientation of a pallet lifted by the forks of a forklift, comprising: a distance detection unit that detects a distance from a lower portion of the pallet lifted by the forks to a lower surface of the pallet; A corner detection unit that detects the position of a corner on the underside of the pallet based on detection data from the distance detection unit; an estimation calculation unit that performs an estimation calculation of the position and attitude of the pallet relative to the forks based on the positions of the corners on the underside of the pallet detected by the corner detection unit, The corner detection unit detects the position of a corner on the underside of the pallet that is on the front side with respect to a vehicle body of the forklift, The estimation calculation unit is a position and orientation estimation device that performs an estimation calculation of the position and orientation of the pallet relative to the forks based on the position of the corner of the underside of the pallet that is on the front side relative to the vehicle body.

Images