JP7669902B2 - Position and orientation estimation device - Google Patents

Description

The present invention relates to a position and orientation estimation device.

A known example of a conventional position and orientation estimation device is 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 a laser beam and measures the distance to surrounding objects from the reflected light of the irradiated laser beam, 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.

JP 2017-178567 A

However, the position and orientation estimation device described in the above prior art is a technology that estimates 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 once it has been lifted by the forks. Furthermore, in the position and orientation estimation device described in the above prior art, because the range sensor is attached to the forks, it is difficult to estimate the position and orientation of the pallet relative to the forks based on the measurement values of the range sensor once the pallet has been lifted by the forks.

The object of the present invention is to provide a position and orientation estimation device that can estimate the position and orientation of a pallet relative to the forks when the pallet is lifted by the forks.

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 the bottom of 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 data detected by 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.

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 way, 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. Also, 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 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 width of the entire forklift.

The corner detection unit may estimate multiple straight lines corresponding to adjacent side surfaces of the pallet based on detection data from the distance detection unit and 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 such a configuration, the position of the intersection of 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.

The corner detection unit may detect the positions of the left and right corners on the underside of the pallet based on the detection data from the distance detection unit, 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 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 the position and orientation of the pallet relative to the forks with high accuracy even if the size of the pallet is unknown.

The corner detection unit may estimate a first line corresponding to a first side of the pallet perpendicular to the fork insertion direction and two second lines corresponding to second side surfaces on both the left and right sides adjacent to the first side based on the detection data of the distance detection unit, and calculate the positions of the two intersections of the first line and the two second 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 intersections. In this configuration, the positions of the two intersections of the first line corresponding to the first side of the pallet and the two second 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.

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.

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.

The corner detection unit detects the position of the corner on the underside of the pallet that is closest 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 closest 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.

According to the present invention, it is possible to estimate the position and orientation of the pallet relative to the forks when the pallet is lifted by the forks.

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; FIG. 2 is a side view showing a state in which the forks of the forklift shown in FIG. 1 have lifted up a pallet. FIG. 2 is a plan view showing a state in which the forks of the forklift shown in FIG. 1 have lifted up the pallet. 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; 5 is a flowchart showing a procedure of a control process executed by a controller shown in FIG. 4 . 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. FIG. 2 is a plan view showing a state in which a forklift is placing a load in a load storage area. 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.

The following describes in detail an embodiment of the present invention with reference to the drawings. In the drawings, identical or equivalent elements are given the same reference numerals, and duplicate descriptions are omitted.

Figure 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. In Figure 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.

The traveling device 2 has a vehicle body 4, a pair of left and right front wheels 5 that are drive wheels located at the front of the vehicle body 4, and a pair of left and right rear wheels 6 that are steered wheels located at the rear of the vehicle body 4.

The loading 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 that raises and lowers the forks 9, and a tilt cylinder 11 that tilts the mast 7. The forks 9 hold a pallet 12, as shown in Figures 2 and 3.

The pallet 12 is a loading platform for placing cargo M. The pallet 12 can be a flat pallet or a sheet pallet. The pallet 12 has four corners 13. Each corner 13 is rounded. The pallet 12 also has two fork holes 14 into which the forks 9 are inserted.

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 roughly rectangular shape. The side surfaces 12c to 12f are provided between the upper surface 12a and the lower surface 12b.

The sides 12c and 12d face each other. The sides 12c and 12d are first sides perpendicular to the insertion direction of the fork 9. In other words, the fork hole 14 extends in the opposing direction of the sides 12c and 12d. The side 12c is the side facing the front side of the body 4 of the forklift 1. The side 12d is the side facing the rear side of the body 4.

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

Figure 4 is a block diagram showing the configuration of a cargo handling control device equipped with a position and orientation estimation device according to one embodiment of the present invention. In Figure 2, the cargo handling control device 20 is a device that places the pallet 12 held by the forks 9 in a specified cargo placement area Z (see Figure 7). The cargo handling control device 20 places the cargo by automatically driving the forklift 1.

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.

The instruction switch 21 is a manually operated switch that allows the operator to give instructions for the loading operation. The instruction switch 21 may be mounted in the driver's seat of the forklift 1, or may be a mobile terminal carried by the operator.

The distance measurement sensors 22 are arranged on both the left and right sides of the forklift 1 as shown in Figs. 1 to 3. The distance measurement sensors 22 are attached, for example, to a position above the front wheels 5 of the vehicle body 4. The distance measurement 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.

The distance sensor 22 may be a LIDAR, a laser range finder, or a Time of Flight (ToF) camera. The distance sensor 22 irradiates 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 obtain point cloud data. The point cloud data is detection data of the distance sensor 22. The point cloud is a collection of reflection points of the laser light L.

The distance measurement sensor 22 located on the left side of the forklift 1 irradiates the laser light L mainly on the left area of the underside 12b of the pallet 12. The distance measurement sensor 22 located on the right side of the forklift 1 irradiates the laser light L mainly on the right area of the underside 12b of the pallet 12.

The controller 23 is composed of a CPU, RAM, ROM, an input/output interface, etc. When a loading 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 measurement sensor 22, and controls the lift cylinder 10 and the drive unit 24 to load the pallet 12.

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

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.

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.

When the instruction switch 21 instructs the load placement operation, the ascent control unit 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 measurement sensor 22. As shown in FIG. 2, the detection position is a position where the distance measurement sensor 22 can detect the distance to the underside 12b of the pallet 12 and detect the corners of the underside 12b of the pallet 12.

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

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.

The intersection calculation unit 28 calculates the position of the intersection of the multiple straight lines extracted by the straight line estimation unit 27 as the position of the corner on the underside 12b of the pallet 12.

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 position of the corner on the underside 12b of the pallet 12 based on the detection data of the distance measurement sensor 22.

The estimation calculation unit 29 performs an 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 positions and attitudes of the pallet 12 with the specified position of the forklift 1 as the origin.

The load placement 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 orientation in the load placement area Z (see Figure 7) based on the position and orientation of the pallet 12 relative to the forks 9 estimated by the estimation calculation unit 29.

Figure 5 is a flowchart showing the steps of the control process executed by the controller 23. In Figure 5, the controller 23 first determines whether 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).

When the controller 23 determines that a loading operation of the pallet 12 held by the forks 9 has been instructed, it controls the lift cylinder 10 so that the pallet 12 rises to the detection position (described above) (step S102).

Next, the controller 23 acquires detection data from the distance measurement sensor 22 by irradiating laser light L from the distance measurement sensor 22 onto the underside 12b of the pallet 12 (step S103).

Then, the controller 23 extracts a point cloud corresponding to the bottom 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 bottom surface 12b of the pallet 12 from the point cloud data acquired by the distance measurement sensor 22, as shown in FIG. 6(a).

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

Then, the controller 23 determines two intersections between a line corresponding to the side surface 12c of the pallet 12 and lines corresponding to the sides 12e and 12f of the pallet 12, and calculates the positions of the two intersections as the positions of the corners on the underside 12b of the pallet 12 (step S106). Here, as shown in FIG. 6(b), the positions of intersections P1 and P2 between a line L1 corresponding to the side surface 12c of the pallet 12 and lines L2 and L3 corresponding to the sides 12e and 12f of the pallet 12 are calculated as the positions of the corners on the underside 12b of the pallet 12.

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

The controller 23 also calculates the yaw angle of the pallet 12 based on the intersection of the two left and right intersection positions (step S108). The yaw angle of the pallet 12 is the rotation angle around the up-down axis of the pallet 12.

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

The controller 23 then determines whether the position and orientation of the pallet 12 relative to the forks 9 deviates from a reference value (step S110). The reference value is a value that ensures that the position and orientation of the pallet 12 when placed is within an acceptable range.

When the controller 23 determines that the position and attitude of the pallet 12 relative to the forks 9 deviate from the reference values, it 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 correct the position and attitude of the pallet 12 by changing the travel path of the forklift 1.

If the forklift 1 is equipped with a side shift cylinder that shifts the forks 9 to the left and right, the position of the pallet 12 relative to the forks 9 may be corrected by controlling the side shift cylinder.

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 FIG. 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).

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 load placement control unit 30 executes the above steps S110 to S112.

However, when loading the pallet 12, if the position and posture of the pallet 12 relative to the forks 9 are misaligned as shown in FIG. 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.

To solve this 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 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.

It is also possible to estimate the position and orientation of the pallet 12 relative to the forks 9 by irradiating the sides 12e, 12f of the pallet 12 with laser light L from the distance measurement sensor 22. However, in this case, it is necessary to place 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.

Therefore, in this embodiment, when the pallet 12 is lifted by the forks 9 of the forklift 1, the distance 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 from the distance 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. In this way, since the distance sensor 22 detects the distance from the bottom of the pallet 12 to the bottom surface 12b of the pallet 12, the positions of the corners on the bottom surface 12b of the pallet 12 are detected regardless of whether or not a cargo M is 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 using the distance measurement sensor 22 to detect the distance from the bottom of the pallet 12 to the bottom surface 12b of the pallet 12, it is not necessary to place the distance measurement sensor 22 on the outside of the body 4 of the forklift 1 to detect the distance to the sides 12e and 12f of the pallet 12. Therefore, the position and orientation of the pallet 12 relative to the forks 9 can be estimated without increasing the width of the entire forklift 1.

In addition, in this embodiment, the positions of the left and right corners of the underside 12b of the pallet 12 are detected based on the detection data of the distance measurement sensor 22, and an estimate of the position and orientation of the pallet 12 relative to the forks 9 is calculated 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.

In this embodiment, based on the detection data of the distance measurement sensor 22, a straight line L1 corresponding to the side 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 the two left and right 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 on the bottom surface 12b of the pallet 12. Therefore, the positions of the two left and right intersection points P1 and P2 between the straight line L1 corresponding to the side 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 on the bottom 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.

In addition, 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.

In addition, 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 and 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.

In addition, in this embodiment, the position of the corner on the underside 12b of the pallet 12 that is closer to the vehicle body 4 of the forklift 1 is detected, and an estimation calculation of the position and attitude of the pallet 12 relative to the forks 9 is performed based on the position of the corner on the underside 12b of the pallet 12 that is closer 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 of 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.

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 form, and the yaw angle of the pallet 12 may be calculated based on at least one of the straight lines L1 to L3.

In addition, in the above embodiment, the distance measuring sensor 22 is disposed on both the left and right sides of the forklift 1, but this is not limited to a particular form. 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.

In addition, in the above embodiment, the positions of the corners on both the left and right sides of the underside 12b of the pallet 12 are detected based on the detection data of the distance measurement sensor 22, but this is not limited to a particular form. If the length and width of the pallet 12 are known, as shown in FIG. 8, the position of only one corner on either the left or right side of the underside 12b of the pallet 12 may be detected based on the detection data of one distance measurement sensor 22.

In the modified example shown in FIG. 8, a straight line L1 corresponding to side 12c of pallet 12 and a straight line L2 corresponding to side 12f of pallet 12 are estimated, the position of intersection P of these straight lines L1 and L2 is calculated, and the position and attitude of the pallet 12 relative to the forks 9 are estimated based on the position of intersection P. Specifically, the center position C of the pallet 12 is calculated based on the position of intersection P and the length and width of the pallet 12, and the yaw angle of the pallet 12 is calculated based on at least one of the straight lines L1 and L2.

In this modified example, the position of the intersection P of the straight lines L1 and L2 corresponding to the adjacent side surfaces 12c and 12f of the pallet 12 is detected as the position of the 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 orientation of the pallet 12 relative to the forks 9 can be estimated.

In addition, in the above embodiment, the position of the corner on the lower surface 12b of the pallet 12 on the near side (front side) 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 lower surface 12b of the pallet 12 on the far side (rear side) relative to the vehicle body 4 may also be detected.

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, the positions of both ends of the pallet 12.

In the above embodiment, when the instruction switch 21 instructs the loading operation of the pallet 12, the lift cylinder 10 lifts the pallet 12. However, this is not limited to this form. For example, the lift cylinder 10 may lift the pallet 12 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.

1...forklift, 4...vehicle body, 9...fork, 12...pallet, 12b...underside, 12c...side (first side), 12e, 12f...side (second side), 22...distance 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 (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 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 . 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 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. 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. 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. 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. 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 position and orientation estimation device according to any one of claims 1 to 5 , 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. 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