JP2022128063A - Position and posture estimation device - Google Patents

Abstract

To provide a position and posture estimation device capable of accurately estimating the position and posture of a pallet to be loaded or unloaded with respect to a forklift.SOLUTION: A position and posture estimation device 24 comprises a laser sensor 11 that detects the distance to a pallet 5 by irradiating the pallet 5 with a laser and receiving the reflected laser light, a pallet recognition unit 15 that recognizes the pallet 5 based on the measurement point data of the laser sensor 11, a data extraction unit 16 that extracts the measurement point data corresponding to the pallet 5 from the measurement point data of the laser sensor 11, a plane equation calculation unit 18 that calculates a plane equation of a front surface 5a of the pallet 5 based on the extracted measurement point data, and an estimation calculation unit 20 that estimates the position and posture of the pallet 5 using the plane equation of the front surface 5a of the pallet 5. The estimation calculation unit 20 calculates the yaw angle, pitch angle, and roll angle of the pallet 5 as the posture of the pallet 5.SELECTED DRAWING: Figure 2

Description

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

As a conventional position/orientation estimation device, for example, the technology described in Patent Document 1 is known. The position and orientation estimation device described in Patent Document 1 includes a ranging sensor that measures a distance to an object, an object recognition unit that recognizes an object based on the detection value of the ranging sensor, and a detection value of the ranging sensor. A fork pocket determination unit that detects a plurality of pockets existing in an object based on the fork pocket rule and determines two pockets as fork pockets using the fork pocket rule, and a plurality of pockets that are determined as fork pockets using the pallet rule A pallet determination unit that determines an object having a pocket of 1 to be a pallet, and a position calculation unit that calculates the coordinates of the fork pocket and the pallet.

JP 2018-20881 A

In the prior art described above, although the positions of the fork pocket and the pallet can be estimated based on the rules for the fork pocket and the pallet, the posture of the pallet cannot be estimated with high accuracy.

SUMMARY OF THE INVENTION An object of the present invention is to provide a position/orientation estimating apparatus capable of estimating the position and orientation of a pallet to be handled with respect to a forklift with high accuracy.

One aspect of the present invention is a position/orientation estimating device for estimating the position and orientation of a pallet to be handled with respect to a forklift having a pair of forks, which irradiates a laser toward the pallet and receives reflected light of the laser. A laser distance detector for detecting the distance to the pallet, a pallet recognition part for recognizing the pallet based on the data detected by the laser distance detector, and a pallet recognition part for recognizing the data detected by the laser distance detector. A data extractor that extracts detection data corresponding to the extracted pallet, a plane equation calculator that calculates the plane equation of the front surface of the pallet based on the detected data extracted by the data extractor, and a plane equation calculator. an estimation calculation unit for estimating the position and attitude of the pallet using the plane equation of the front surface of the pallet, and the estimation calculation unit calculates the yaw angle, pitch angle and roll angle of the pallet as the attitude of the pallet.

In such a position and orientation estimation device, the laser distance detection unit irradiates the pallet with a laser beam and receives the reflected light of the laser beam, thereby detecting the distance to the pallet. Then, the pallet is recognized based on the detection data of the laser distance detection section, and the detection data corresponding to the pallet is extracted from the detection data of the laser distance detection section. Then, a plane equation of the front surface of the pallet is calculated based on the extracted detection data, and the position and orientation of the pallet are estimated using the plane equation of the front surface of the pallet. At this time, the yaw angle, pitch angle and roll angle of the pallet are calculated as the attitude of the pallet. This increases the accuracy of estimating the pallet orientation. In this manner, not only the position of the pallet to be handled but also the attitude of the pallet to be handled can be estimated with high accuracy. In addition, since a camera that is vulnerable to direct sunlight is not used, it is possible to reduce the cost of the position/orientation estimation device and to recognize pallets that are less affected by sunlight.

The position/orientation estimating device further includes a fork hole detection unit that detects two fork holes provided in the pallet and into which a pair of forks are inserted based on the plane equation of the front surface of the pallet. The yaw angle and pitch angle of the pallet may be calculated based on the plane equation of , and the roll angle of the pallet may be calculated based on the positional relationship between the two fork holes detected by the fork hole detection unit.

With such a configuration, the yaw and pitch angles of the pallet can be obtained simply and reliably by calculating the yaw and pitch angles of the pallet based on the plane equation of the front surface of the pallet. In addition, based on the plane equation of the front surface of the pallet, the two fork holes provided in the pallet are detected, and the roll angle of the pallet is calculated based on the positional relationship between the two fork holes. Angles can be obtained easily and reliably.

The position/orientation estimation apparatus may further include a determination unit that determines whether the dimensions of the front surface of the pallet whose position and orientation have been estimated by the estimation calculation unit match predetermined specified values.

In such a configuration, the pallet whose position and orientation are estimated is appropriate as a pallet to be handled by determining whether the dimensions of the front surface of the pallet whose position and orientation are estimated match the specified values. I know whether

The position and orientation estimation apparatus further includes a filter unit that performs filtering processing on the detection data extracted by the data extraction unit. You may calculate the plane equation of the front surface of .

In such a configuration, a plane equation of the front surface of the pallet in which noise detection data is removed is obtained by filtering the detection data of the laser distance detection unit. Therefore, the position and attitude of the pallet to be handled can be estimated with higher accuracy.

ADVANTAGE OF THE INVENTION According to this invention, the position and attitude|position of the pallet for cargo handling with respect to a forklift can be estimated with high precision.

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 pallet to be handled; FIG. 1 is a schematic configuration diagram showing an automatic driving system including a position and orientation estimation device according to an embodiment of the present invention; FIG. 3 is a flowchart showing details of an estimation/control processing procedure executed by the controller shown in FIG. 2; It is a figure which shows roughly an example of laser measurement point data. FIG. 5 is a diagram showing a state in which a frame line surrounding a pallet is specified in the laser measurement point data shown in FIG. 4; It is a figure which shows roughly an example of the laser measurement point data before and behind filtering processing by the filter part shown by FIG.

BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

FIG. 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 pallet to be handled. In FIG. 1, a forklift 1 includes a vehicle body 2 and a cargo handling device 3 arranged in front of the vehicle body 2 for cargo handling. The cargo handling device 3 has a mast 4 attached to the front end of the vehicle body 2 and a pair of forks 6 attached to the mast 4 so as to be able to move up and down to lift a pallet 5 .

The pallet 5 is a loading platform for loading cargo. The pallet 5 is, for example, a flat pallet. The pallet 5 has a square shape in plan view. The pallet 5 has a front surface 5a, a rear surface 5b facing the front surface 5a, and two side surfaces 5c orthogonal to the front surface 5a and the rear surface 5b. The front surface 5 a faces the forklift 1 when the pallet 5 is lifted by the forks 6 . The pallet 5 is provided with two fork holes 7 into which a pair of forks 6 are inserted. The fork holes 7 extend from the front surface 5a of the pallet 5 to the rear surface 5b. The fork hole 7 has a rectangular shape when viewed from the front (see FIG. 6).

FIG. 2 is a schematic configuration diagram showing an automatic driving system provided with a position and orientation estimation device according to one embodiment of the present invention. In FIG. 2 , an automatic driving system 10 is a system for automatically driving the forklift 1 . An automatic driving system 10 is mounted on a forklift 1 . The automatic driving system 10 includes a laser sensor 11 , a controller 12 , a drive section 13 and a notification section 14 .

The laser sensor 11 is a laser distance detection unit that detects the distance to the pallet 5 to be handled by irradiating a laser toward the pallet 5 to be handled and receiving the reflected light of the laser. The laser sensor 11 irradiates a 2D (two-dimensional) or 3D (three-dimensional) laser with a high point density. As the laser sensor 11, for example, a laser range finder or the like is used.

The controller 12 includes a CPU, RAM, ROM, input/output interfaces, and the like. The controller 12 includes a pallet recognition unit 15, a data extraction unit 16, a filter unit 17, a plane equation calculation unit 18, a fork hole detection unit 19, an estimation calculation unit 20, and a final determination unit 21 (determination unit). , and a control unit 22 .

Here, the laser sensor 11, the pallet recognition unit 15 of the controller 12, the data extraction unit 16, the filter unit 17, the plane equation calculation unit 18, the fork hole detection unit 19, the estimation calculation unit 20, and the final determination unit 21 of the present embodiment constitutes the position and orientation estimation device 24 of. The position and orientation estimation device 24 is a device that estimates the position and orientation of the pallet 5 to be handled with respect to the forklift 1 . A pallet 5 to be handled is a pallet for which cargo handling is to be started by the cargo handling device 3 and is positioned in front of the forklift 1 .

The pallet recognition unit 15 recognizes the pallet 5 based on measurement point data (detection data) of the laser sensor 11 . The data extraction unit 16 extracts measurement point data corresponding to the pallet 5 recognized by the pallet recognition unit 15 from the measurement point data of the laser sensor 11 . The filter unit 17 filters the measurement point data of the laser sensor 11 extracted by the data extraction unit 16 .

The plane equation calculation unit 18 calculates the plane equation of the front surface 5 a of the pallet 5 based on the measurement point data of the laser sensor 11 filtered by the filter unit 17 . The fork hole detector 19 detects two fork holes 7 provided in the pallet 5 based on the plane equation of the front surface 5 a of the pallet 5 calculated by the plane equation calculator 18 .

The estimation calculation unit 20 estimates the position and orientation of the pallet 5 using the plane equation of the front surface 5 a of the pallet 5 . At this time, the estimation calculation unit 20 calculates the yaw angle, pitch angle, and roll angle of the pallet 5 as the attitude of the pallet 5 . The estimation calculation unit 20 calculates the yaw angle and pitch angle of the pallet 5 based on the plane equation of the front surface 5a of the pallet 5, and based on the positional relationship between the two fork holes 7 detected by the fork hole detection unit 19. to calculate the roll angle of the pallet 5.

The final determination unit 21 finally determines whether the dimensions of the front surface 5a of the pallet 5 whose position and orientation have been estimated by the estimation calculation unit 20 match predetermined specified values.

When the final determination unit 21 determines that the dimensions of the front surface 5a of the pallet 5 match the specified values, the control unit 22, based on the position and orientation of the pallet 5 estimated by the estimation calculation unit 20, The drive unit 13 is controlled so as to move the forklift 1 to a position near the front of the pallet 5 to be handled. Although not shown, the drive unit 13 has, for example, a travel motor that rotates the drive wheels and a steering motor that steers the steered wheels.

Further, when the final determination unit 21 determines that the dimension of the front surface 5a of the pallet 5 does not match the specified value, the control unit 22 notifies the notification unit 14 of NG. The notification unit 14 is, for example, a display or an alarm.

FIG. 3 is a flow chart showing details of the estimation/control processing procedure executed by the controller 12 . Note that this processing is executed, for example, when an instruction to start the automatic operation of the forklift 1 is given.

In FIG. 3, the controller 12 acquires measurement point data of the laser sensor 11 (hereinafter referred to as laser measurement point data) (step S101). FIG. 4 is a diagram schematically showing an example of laser measurement point data. FIG. 4 shows the laser measurement point data _DL1 for a state in which the pallets 5 are stacked in three stages on the mounting table 26 and a plurality of objects 27 other than the pallets 5 are arranged. Note that the pallets 5 on the laser measurement point data _L1 are assigned the same reference numerals as the actual pallets 5 for convenience.

Subsequently, the controller 12 recognizes the pallet 5 by a recognition method using deep learning based on the laser measurement point data (step S102).

Deep learning is one of the elemental technologies of artificial intelligence. Deep learning is learning using a deep neural network that allows a machine to automatically extract features from data without human intervention if there is a sufficient amount of data. Deep learning increases the amount of information transmission and processing by creating multiple intermediate layers between the input layer and the output layer, making it possible to increase the accuracy and versatility of feature values and improve prediction accuracy.

Specifically, the controller 12 designates a frame line W surrounding the pallet 5 in the laser measurement point data _DL1 , as shown in FIG. A frame line W is a rectangular frame called a bounding box in object detection using deep learning. Then, the controller 12 recognizes the pallet 5 by comparing the laser measurement point data _DL1 , in which the frame line W is designated, with previously created learning data.

Subsequently, the controller 12 extracts laser measurement point data corresponding to the pallet 5 recognized in step S102 from among the laser measurement point data acquired in step S101 (step S103). As a result, as shown in FIG. 6A, laser measurement point data _DL2 including the laser measurement points corresponding to the recognized pallet 5 among the laser measurement point data _DL1 is obtained.

Subsequently, the controller 12 filters the laser measurement point data extracted in step S103 (step S104). At this time, laser measurement points whose point density is lower than a predetermined amount are removed. As a result, as shown in FIG. 6B, the laser measurement point data _DL3 is obtained from which the laser measurement points corresponding to the noise other than the pallet 5 are removed.

Subsequently, the controller 12 calculates the plane equation of the front surface 5a of the pallet 5 based on the laser measurement point data obtained in step S104 (step S105). The controller 12 uses a robust estimation method such as RANSAC (Random Sample Consensus), for example, to remove the laser measurement points other than the laser measurement points corresponding to the plane in the laser measurement point data as outliers. Find the plane equation of Robust estimation is a technique aimed at suppressing the influence of outliers included in measured values (here, laser measurement points). The plane equation of the front surface 5a of the pallet 5 may be calculated using the method of least squares or the like instead of the robust estimation method.

Subsequently, the controller 12 detects two fork holes 7 provided in the pallet 5 based on the plane equation of the front surface 5a of the pallet 5 calculated in step S105 (step S106). At this time, in the laser measurement point data, for example, an area where the point density of the laser measurement points is a predetermined value or more is detected as the front surface 5a of the pallet 5, and two areas where the point density of the laser measurement points is lower than the predetermined value are forks. It is detected as a hole 7.

Subsequently, the controller 12 calculates the position of the pallet 5 and the yaw and pitch angles of the pallet 5 based on the plane equation of the front surface 5a of the pallet 5 (step S107). The yaw angle of the pallet 5 is the angle of rotation about the vertical direction (height direction) of the pallet 5 . The pitch angle of the pallet 5 is the angle of rotation about the left-right direction (width direction) of the pallet 5 .

Subsequently, the controller 12 calculates the roll angle of the pallet 5 based on the positional relationship between the two fork holes 7 detected in step S106 (step S108). The roll angle of the pallet 5 is a rotation angle about the front-rear direction (depth direction) of the pallet 5 . Specifically, the controller 12 calculates the center positions of the two fork holes 7 and calculates the roll angle of the pallet 5 from the relationship between the center positions of the two fork holes 7 . The calculation for estimating the position of the pallet 5 may also be performed based on the positional relationship between the two fork holes 7 .

Subsequently, the controller 12 determines whether the dimensions of each part of the front surface 5a of the pallet 5 whose positions and orientations have been estimated in steps S107 and S108 match predetermined specified values (step S109). The dimensions of each part of the front surface 5a of the pallet 5 include the width and height of the pallet 5, the dimensions of the two fork holes 7, the center-to-center distance between the two fork holes 7, and the like.

When the controller 12 determines that the dimensions of each part of the front surface 5 a of the pallet 5 match the specified values, the controller 12 moves the forklift 1 to the vicinity of the pallet 5 to be handled based on the position and attitude of the pallet 5 . The drive unit 13 is controlled to move to the position (step S110). When the controller 12 determines that the dimensions of each part of the front surface 5a of the pallet 5 do not match the specified values, the controller 12 notifies the notification unit 14 of NG (step S111).

Here, the palette recognition unit 15 executes steps S101 and S102. The data extraction unit 16 executes steps S101 and S103. The filter unit 17 executes step S104. The plane equation calculation unit 18 executes step S105. The fork hole detector 19 executes step S106. The estimation calculation unit 20 executes steps S107 and S108. The final determination unit 21 executes step S109. The control unit 22 executes steps S110 and S111.

As described above, in the present embodiment, the laser sensor 11 irradiates the pallet 5 with a laser beam and receives the reflected light of the laser beam, thereby detecting the distance to the pallet 5 . Then, the pallet 5 is recognized based on the measurement point data (detection data) of the laser sensor 11, and the measurement point data corresponding to the pallet 5 is extracted from the measurement point data of the laser sensor 11. FIG. Then, the plane equation of the front surface 5a of the pallet 5 is calculated based on the extracted measurement point data, and the position and orientation of the pallet 5 are estimated using the plane equation of the front surface 5a of the pallet 5. At this time, the yaw angle, pitch angle and roll angle of the pallet 5 are calculated as the attitude of the pallet 5 . As a result, the accuracy of estimating the orientation of the pallet 5 is improved. In this manner, not only the position of the pallet 5 to be handled, but also the attitude of the pallet 5 to be handled can be estimated with high accuracy. Further, even when the pallet 5 and the forklift 1 are far apart, the position and posture of the pallet 5 to be handled can be estimated with high accuracy without attaching a marker to the front surface 5a of the pallet 5. - 特許庁

By the way, there is also a method of capturing an image of the palette 5 with a camera and recognizing the palette 5 based on the captured image data of the camera. However, such a method requires two types of sensors: a laser sensor 11 and a camera. In addition, since the camera is vulnerable to direct sunlight, there is a possibility that the recognition accuracy of the pallet 5 will be lowered when the weather is fine.

On the other hand, since the present embodiment does not use a camera, the position/orientation estimation device 24 can be realized at low cost. In addition, since it is possible to recognize the pallet 5 which is less affected by sunlight, the recognition accuracy of the pallet 5 can be improved in fine weather as compared with the case of using a camera.

Further, in this embodiment, by calculating the yaw angle and pitch angle of the pallet 5 based on the plane equation of the front surface 5a of the pallet 5, the yaw angle and pitch angle of the pallet 5 can be obtained easily and reliably. can. Also, based on the plane equation of the front surface 5a of the pallet 5, the two fork holes 7 are detected, and based on the positional relationship between the two fork holes 7, the roll angle of the pallet 5 is calculated. A roll angle can be obtained easily and reliably.

Further, in this embodiment, by determining whether the dimensions of the front surface 5a of the pallet 5 whose position and orientation are estimated match the specified values, the pallet 5 whose position and orientation are estimated is the pallet to be handled. It can be seen whether it is appropriate as 5.

Further, in this embodiment, by filtering the measurement point data of the laser sensor 11, the plane equation of the front surface 5a of the pallet 5 is obtained in a state where the measurement point data, which becomes noise, is removed. Therefore, the position and orientation of the pallet 5 to be handled can be estimated with higher accuracy.

In addition, this invention is not limited to the said embodiment. For example, in the above-described embodiment, the pallet 5 is recognized by a recognition technique using deep learning based on the measurement point data of the laser sensor 11. However, the form is not particularly limited, and template matching, pattern matching, or the like can be used. may be used to recognize the pallet 5.

DESCRIPTION OF SYMBOLS 1... Forklift 5... Pallet 5a... Front surface 6... Fork 7... Fork hole 11... Laser sensor (laser distance detection part) 15... Pallet recognition part 16... Data extraction part 17... Filter part 18 19 fork hole detection unit 20 estimation calculation unit 21 final determination unit (determination unit) 24 position/orientation estimation device.

Claims (4)

A position and orientation estimation device for estimating the position and orientation of a pallet to be handled with respect to a forklift having a pair of forks,
a laser distance detection unit that detects the distance to the pallet by irradiating the pallet with a laser and receiving the reflected light of the laser;
a pallet recognition unit that recognizes the pallet based on the detection data of the laser distance detection unit;
a data extraction unit for extracting detection data corresponding to the pallet recognized by the pallet recognition unit from the detection data of the laser distance detection unit;
a plane equation calculation unit that calculates a plane equation of the front surface of the pallet based on the detection data extracted by the data extraction unit;
an estimation calculation unit that estimates the position and orientation of the pallet using the plane equation of the front surface of the pallet calculated by the plane equation calculation unit;
The position/attitude estimation device, wherein the estimation calculation unit calculates a yaw angle, a pitch angle, and a roll angle of the pallet as the attitude of the pallet. Further comprising a fork hole detection unit that detects two fork holes provided in the pallet into which the pair of forks are inserted based on the plane equation of the front surface of the pallet,
The estimation calculation unit calculates the yaw angle and pitch angle of the pallet based on the plane equation of the front surface of the pallet, and based on the positional relationship between the two fork holes detected by the fork hole detection unit , and calculating the roll angle of the pallet. 3. The position and orientation estimation apparatus according to claim 1, further comprising a determination unit that determines whether the dimensions of the front surface of the pallet whose position and orientation have been estimated by the estimation calculation unit match predetermined specified values. . Further comprising a filter unit for filtering the detection data extracted by the data extraction unit,
The position and orientation estimation device according to any one of claims 1 to 3, wherein the plane equation calculation unit calculates the plane equation of the front surface of the pallet based on the detection data subjected to the filtering process by the filter unit. .

Images