JP2023076925A - Position/attitude estimation device - Google Patents

Abstract

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

Description

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/orientation estimation apparatus described in Patent Document 1 includes a monocular camera that detects a palette, and a controller connected to the monocular camera. The controller detects the pallet based on the image data captured by the monocular camera, and creates framed captured image data in which a frame surrounding the front surface of the pallet and the two pallet holes is specified in the captured image data. and an estimation calculation unit for estimating the position and orientation of the pallet using framed captured image data.

Japanese Patent Application Laid-Open No. 2021-24718

By the way, as one of the pallets used for cargo handling work by a forklift, there is a post pallet that can be stacked in a state where cargo is placed. For such a post pallet, it is also necessary to estimate the position and orientation with respect to the forklift with high accuracy.

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

One aspect of the present invention is a position and orientation for estimating the position and orientation of a post pallet that is mounted on a forklift and has a pallet on which a load is placed and four pillars erected at the front, rear, left, and right corners of the pallet. a laser distance detection unit that detects the distance to the post pallet and acquires point cloud data by irradiating a laser toward the front surface of the post pallet and receiving the reflected light of the laser; Based on the point cloud data acquired by the distance detection unit, the plane detection unit detects the plane corresponding to the front surface of the pallet and the pillar, and the front surface of the pallet and the pillar detected by the plane detection unit. an estimation calculation unit that performs estimation calculation of the position and posture of the post pallet with respect to the forklift using a plane, and the estimation calculation unit corresponds to the pallet unit based on the plane corresponding to the front surface of the pallet unit and the pillar unit. A horizontal line and a vertical line corresponding to the pillar are extracted, and the position and posture of the post pallet with respect to the forklift are calculated based on the horizontal line and the vertical line.

In such a position/orientation estimation apparatus, the point cloud data corresponding to the post pallet is obtained by irradiating the front surface of the post pallet with laser by the laser distance detection unit. Then, based on the point cloud data corresponding to the post pallet, a plane corresponding to the front surface of the pallet portion and the pillar portion of the post pallet is detected. Then, horizontal lines corresponding to the pallet portion and vertical lines corresponding to the pillar portion are extracted based on the plane corresponding to the front surface of the pallet portion and the pillar portion. Then, the position and orientation of the post pallet with respect to the forklift are calculated based on the horizontal and vertical lines. At this time, since the pillars are elongated in the vertical direction, it is easy to achieve accuracy in the vertical position and angle of the post pallet. As a result, the position and orientation of the post pallet with respect to the forklift can be estimated with high accuracy.

The position and orientation estimation device includes an imaging unit that acquires image data by imaging the front surface of the post pallet, a post pallet recognition unit that recognizes the post pallet based on the image data acquired by the imaging unit, and a laser distance detection unit. and a point cloud processing unit for extracting point cloud data corresponding to the post pallet recognized by the post pallet recognition unit from among the point cloud data acquired by the plane detection unit. A plane corresponding to the front surface of the pallet portion and the column portion may be detected based on the point cloud data.

In such a configuration, the post-pallet is recognized based on the image data acquired by the imaging unit, and the point cloud data corresponding to the recognized post-pallet is extracted from the point cloud data acquired by the laser distance detection unit. be. Image data has a higher resolution than point cloud data. As a result, post-pallet recognition accuracy is enhanced. Therefore, the position and orientation of the post pallet with respect to the forklift can be estimated with higher accuracy.

The estimation calculation unit calculates the posture of the post pallet with respect to the forklift by calculating the posture of the pillar based on the inclination and size of the vertical line, obtains the intersection of the horizontal line and the vertical line, and calculates the position of the intersection based on the position of the intersection. The position of the post pallet with respect to the forklift may be calculated by calculating the position of the pallet unit with the forklift.

In such a configuration, the posture of the pillar is calculated based on the inclination and size of the vertical line corresponding to the pillar, and the position of the intersection of the horizontal line corresponding to the pallet and the vertical line corresponding to the pillar is calculated. Based on this, the position of the pallet unit is calculated. Therefore, the position and orientation of the post pallet with respect to the forklift can be easily calculated.

The plane detection unit obtains second point cloud data from which the point cloud is removed from the first point cloud data corresponding to the post pallet, and adds a plane to a thickness greater than the specified thickness for the second point cloud data. Approximation is performed to obtain the first plane, then the third point cloud data included in the first plane is extracted from the first point cloud data, plane approximation is performed on the third point cloud data to a specified thickness, and the second plane is obtained. By determining the plane, the plane corresponding to the front surface of the pallet and the column may be detected.

In such a configuration, by performing plane approximation on the second point group data from which the point group has been removed to a thickness larger than the specified thickness to obtain the first plane, when plane approximation is repeatedly performed, the first plane Variations in two-point cloud data can be dealt with. Then, the third point cloud data included in the first plane is extracted from the first point cloud data, and the second plane is obtained by performing plane approximation on the third point cloud data to a specified thickness to obtain the pallet part and the The plane corresponding to the front surface of the pillar section is less likely to be chipped, and unnecessary point groups not included in the plane corresponding to the front surface of the pallet section and the pillar section are deleted. In addition, deviation of the position and angle of the plane corresponding to the front surface of the pallet portion and the column portion is suppressed. As described above, the position and orientation of the post pallet with respect to the forklift can be estimated with higher accuracy.

The estimation calculation unit performs straight line approximation on the plane point cloud data included in the plane corresponding to the front surface of the pallet part and the column part to obtain a straight line, and then calculates the point cloud data on the straight line among the plane point cloud data. By taking out, horizontal lines and vertical lines may be extracted.

In such a configuration, after linear approximation is performed on the plane point cloud data included in the plane corresponding to the front surface of the pallet part and the column part to obtain a straight line, the point cloud on the straight line among the plane point cloud data By extracting the data, the horizontal lines corresponding to the pallet portions and the vertical lines corresponding to the column portions are less likely to be missing, and unnecessary point groups not included in the horizontal lines and vertical lines are deleted. Therefore, the position and orientation of the post pallet with respect to the forklift can be estimated with higher accuracy.

The estimation calculation unit rotates the plane point cloud data so that the plane point cloud data included in the plane corresponding to the front surface of the pallet part and the pillar virtually faces the laser distance detection part, thereby obtaining the plane point cloud data. After converting the coordinate system of , calculate the position and orientation of the post pallet with respect to the forklift based on the horizontal and vertical lines, and then reverse rotate the post pallet to return the position and orientation to the original state. The final post pallet position and orientation relative to the forklift may be determined.

In such a configuration, by rotating the plane point cloud data included in the plane corresponding to the front surface of the pallet part and the column part so that the plane point cloud data virtually faces the laser distance detection part, the post pallet The crossing point of the horizontal line and the vertical line is obtained two-dimensionally, ignoring the front-rear direction. Therefore, the intersections of horizontal lines and vertical lines can be extracted with high accuracy. Therefore, the position and orientation of the post pallet with respect to the forklift can be estimated with higher accuracy.

After calculating the position and orientation of the post pallet with respect to the forklift, the estimation calculation unit finally determines whether the plane detected by the plane detection unit corresponds to the front surface of the post pallet based on the inclination and dimensions of the horizontal and vertical lines. When it is determined that the plane detected by the plane detection unit does not correspond to the front surface of the post pallet, the point cloud data included in the plane detected by the plane detection unit is deleted, and the plane detection unit Alternatively, the detection process of the plane corresponding to the front surface of the pallet portion and the pillar portion may be executed again.

With such a configuration, when it is determined that the detected plane does not correspond to the front surface of the post-pallet, the point cloud data included in the plane is deleted and the plane detection processing is executed again. and an exact plane corresponding to the front face of the column is obtained. Therefore, the position and orientation of the post pallet with respect to the forklift can be estimated with higher accuracy.

ADVANTAGE OF THE INVENTION According to this invention, the position and attitude|position of the post pallet 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 post pallet; FIG. 2 is a front view of the post pallet shown in FIG. 1; FIG. 1 is a schematic configuration diagram showing a traveling control device equipped with a position and orientation estimation device according to an embodiment of the present invention; FIG. It is a figure which shows the point-group data acquired by the laser sensor, and the point-group data extracted by the point-group process part. FIG. 4 is a flowchart showing a procedure of plane detection processing executed by the plane detection unit shown in FIG. 3; FIG. FIG. 4 is a conceptual diagram showing a simplified plane detection process for performing normal plane approximation on point cloud data. 4 is a conceptual diagram showing a simplified plane detection process executed by the plane detection unit shown in FIG. 3; FIG. FIG. 4 is a flow chart showing a procedure of estimation calculation processing executed by an estimation calculation unit shown in FIG. 3; FIG. FIG. 4 is a conceptual diagram showing how coordinate transformation of planar point cloud data is performed; It is a figure which shows the horizontal line corresponded to a pallet part, the vertical line corresponded to a column part, and the intersection of a horizontal line and a vertical line. FIG. 9 is a flow chart showing the details of a straight line extraction process procedure in step S122 shown in FIG. 8; FIG. FIG. 4 is a conceptual diagram showing a simplified process of performing linear approximation on plane point cloud data; FIG. 4 is a conceptual diagram showing the roll angle, yaw angle and pitch angle of a post pallet;

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 post pallet. 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 left and right forks 6 attached to the mast 4 so as to be able to move up and down and lift a post pallet 5 .

As shown in FIG. 2, the post pallet 5 includes a square pallet portion 7 in a plan view and four pillars 8 erected at the four corners of the pallet portion 7 on the front, back, left and right. have. The longitudinal direction of the post pallet 5 corresponds to the X direction, the lateral direction of the post pallet 5 corresponds to the Y direction, and the vertical direction of the post pallet 5 corresponds to the Z direction.

The pallet part 7 is a loading platform on which the cargo M is placed. The pallet part 7 has a front surface 7a facing the forklift 1 when the post pallet 5 is lifted by the fork 6. - 特許庁The pallet part 7 is provided with two fork holes 9 into which a pair of forks 6 are inserted. The fork holes 9 extend from the front surface 7a of the pallet portion 7 to the rear surface 7b. The shape of the fork hole 9 is rectangular when viewed from the front.

The columnar portion 8 has a square columnar shape. The column portion 8 is connected to a corner portion of the pallet portion 7 so as to be foldable. The column portion 8 has a front surface 8a. The front surface 8a is a surface on the same side as the front surface 7a of the pallet part 7. As shown in FIG.

Such a post pallet 5 can be stacked with the load M placed on the pallet portion 7 when the pillars 8 stand against the pallet portion 7 . In this case, the pallet portion 7 of the upper post pallet 5 is placed on the upper surface of each column portion 8 of the lower post pallet 5 .

FIG. 3 is a schematic configuration diagram showing a cruise control device including a position and orientation estimation device according to an embodiment of the present invention. In FIG. 3, the travel control device 10 is a device that automatically travels the forklift 1 to a pick-up position when the forklift 1 picks up the post pallet 5 loaded on the truck bed T (see FIG. 1). be. The unloading position is a position where each fork 6 of the forklift 1 is inserted into the fork hole 9 of the post pallet 5 .

A travel control device 10 is mounted on the forklift 1 . The travel control device 10 includes a camera 11 , a laser sensor 12 , a controller 13 and a driving section 14 .

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

The laser sensor 12 is a laser distance detector that detects the distance to the post pallet 5 by irradiating a laser toward the front surface of the post pallet 5 and receiving the reflected light of the laser to acquire point cloud data. . A point cloud is a collection of laser reflection points on an object including the post pallet 5 . The laser sensor 12 irradiates a 2D or 3D laser with a high point density. As the laser sensor 12, for example, a LIDAR, a laser range finder, or the like is used.

The controller 13 includes a CPU, RAM, ROM, input/output interface, and the like. The controller 13 inputs the image data acquired by the camera 11 and the point cloud data acquired by the laser sensor 12 , executes predetermined processing, and controls the driving section 14 . The driving unit 14 has, although not shown, a traveling motor that rotates the driving wheels of the forklift 1 and a steering motor that steers the steering wheels of the forklift 1, for example.

The controller 13 has a post-pallet recognition unit 15 , a point group processing unit 16 , a plane detection unit 17 , an estimation calculation unit 18 , a route generation unit 19 and a travel control unit 20 .

Here, the post-pallet recognition unit 15, the point group processing unit 16, the plane detection unit 17, and the estimation calculation unit 18 cooperate with the camera 11 and the laser sensor 12 to configure the position/orientation estimation device 21 of the present embodiment. ing. The position/posture estimation device 21 is a device that estimates the position and posture of the post pallet 5 with respect to the forklift 1 when the forklift 1 is positioned in front of the post pallet 5 .

The post-palette recognition unit 15 recognizes the post-palette 5 based on the image data acquired by the camera 11 . The post-palette recognition unit 15 recognizes the approximate position and size of the post-palette 5 by image processing technology using deep learning, for example.

The point cloud processing unit 16 extracts point cloud data corresponding to the post pallet 5 recognized by the post pallet recognition unit 15 from the point cloud data acquired by the laser sensor 12 . For example, when the laser sensor 12 acquires point cloud data Dp as shown in FIG. , the point cloud data Dpp corresponding to the post palette 5 is obtained. In the point cloud data Dpp, the range of the point cloud data is somewhat narrowed in the horizontal direction (Y direction) compared to the point cloud data Dp.

Based on the point cloud data corresponding to the post pallet 5 extracted by the point cloud processing unit 16, the plane detection unit 17 detects the front surface 7a of the pallet portion 7 and the front surface 8a of the column portion 8 (hereinafter simply referred to as the front surface 7a and 8a) are detected.

The plane detection unit 17 detects the planes corresponding to the front surfaces 7a and 8a by, for example, fitting the planes to the point cloud data and obtaining the coefficients of the plane equation. At this time, the plane detection unit 17 uses, for example, RANSAC (Random sample consensus) to randomly pick up some points of the point cloud data and fit the plane.

FIG. 5 is a flowchart showing the procedure of plane detection processing executed by the plane detection unit 17. As shown in FIG. In FIG. 5, the plane detection unit 17 first acquires point cloud data (hereinafter referred to as first point cloud data) corresponding to the post-pallet 5 extracted by the point cloud processing unit 16 (step S101).

Subsequently, the plane detection unit 17 detects second point cloud data (see FIG. 7A), which will be described later, in which unnecessary point groups on the plane have been removed by plane detection. 7(b)) is acquired (step S102). Note that the initial data of the second point cloud data is, for example, the same point cloud data as the first point cloud data.

Subsequently, the plane detection unit 17 performs plane approximation on the second point cloud data to a thickness larger than the specified thickness (see FIG. 7C), and obtains a first plane (step S103). The specified thickness is the dimension of the pallet portion 7 and the column portion 8 in the depth direction (front-rear direction).

Subsequently, the plane detection unit 17 extracts point cloud data (referred to as third point cloud data) included in the first plane from the first point cloud data (step S104). Subsequently, the plane detection unit 17 performs plane approximation on the third point cloud data to a specified thickness (see FIG. 7D), and obtains a second plane (step S105). That is, the plane detection unit 17 performs plane approximation on the third point cloud data more thinly than in step S103.

Subsequently, the plane detection unit 17 extracts point cloud data (referred to as fourth point cloud data) included in the second plane from the first point cloud data (step S106). Then, the plane detection unit 17 records the number of reflection points of the fourth point cloud data included in the second plane (step S107).

Subsequently, the plane detection unit 17 determines whether the plane detection process has been performed a predetermined number of times (for example, 100 times) (step S108). When the plane detection unit 17 determines that the plane detection process has not been performed the predetermined number of times, it sets the point cloud data from which unnecessary reflection points on the plane are deleted as the second point cloud data (step S112), The above steps S102 to S107 are executed again.

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

The plane detector 17 determines whether the selected second plane is a plane likely to be the front surface of the post pallet 5 based on the normal vector of the second plane (step S111). For example, it is possible to determine whether the second plane is a plane likely to be the front surface of the post pallet 5 based on the direction of the normal vector or the like.

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

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

FIG. 6 is a conceptual diagram showing a simplified plane detection process for performing normal plane approximation on point cloud data. Point cloud data Dp0 as shown in FIG. 6A is obtained by plane detection processing on the point cloud data corresponding to the post-pallet 5 extracted by the point cloud processing unit 16. FIG. Note that the point cloud data Dp0 consists of a plurality of reflection points P0.

When plane approximation as shown in FIG. 6B is performed on such point cloud data Dp0, the point cloud corresponding to the front surface of the post pallet 5 is obtained. However, due to the error of the laser sensor 12 and the variation of the point cloud due to the material of the post pallet 5, etc., not all the point cloud can be obtained, and the detected plane is missing (see K in the figure). . Circles P are reflection points extracted by plane approximation. Triangular marks Q are reflection points that are not extracted by plane approximation.

Further, when plane approximation as shown in FIG. 6C is performed on the point cloud data Dp0, it is determined that the detected plane is not the plane corresponding to the front surface of the post pallet 5. , all the reflection points P0 of the point cloud data Dp0 included in the plane are erased.

In this state, as shown in FIG. 6D, when plane approximation is performed by the next plane detection process, the left reflection point P0 of the point cloud data Dp0 is missing. Therefore, an accurate plane corresponding to the front surface of the post pallet 5 cannot be obtained. A white circle mark R is an erased reflection point.

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

Then, as shown in FIG. 7(c), the second point cloud data Dp2 is approximated to a thickness larger than the specified thickness to obtain a first plane F1. As a result, most of the point group of the second point group data Dp2 can be obtained.

Then, the third point cloud data Dp3 included in the first plane F1 is extracted from the first point cloud data Dp1. The third point cloud data Dp3 includes almost all of the point cloud on the front surface of the post pallet 5 . Therefore, the third point cloud data Dp3 forms a plane substantially corresponding to the front surfaces 7a and 8a. However, since the first plane F1 extends obliquely, the angular error of the post pallet 5 increases.

Thereafter, as shown in FIG. 7D, a second plane F2 is obtained by performing plane approximation on the third point cloud data Dp3 to a specified thickness. Since the second plane F2 extends at a correct angle, the correct orientation (yaw angle) of the front surface of the post pallet 5 can be obtained.

Returning to FIG. 3 , the estimation calculation unit 18 uses the plane corresponding to the front surface 7 a of the pallet unit 7 and the front surface 8 a of the column unit 8 detected by the plane detection unit 17 to determine the position and attitude of the post pallet 5 with respect to the forklift 1 . perform an estimation operation.

The estimation calculation unit 18 extracts a horizontal line corresponding to the pallet unit 7 and a vertical line corresponding to the column unit 8 based on the planes corresponding to the front surfaces 7a and 8a, and based on the horizontal and vertical lines, the forklift 1 The position and orientation of the post pallet 5 are calculated.

FIG. 8 is a flow chart showing the procedure of the estimation calculation process executed by the estimation calculation unit 18. As shown in FIG. In FIG. 8, the estimation calculation unit 18 first rotates the plane point cloud data included in the planes corresponding to the front surfaces 7a and 8a so that the plane point cloud data virtually faces the laser sensor 12 (step S121). That is, the XYZ coordinate system of the planar point cloud data is transformed so that the planar point cloud data virtually faces the laser sensor 12 .

Therefore, as shown in FIG. 9(a), even if the plane point cloud data Df is arranged to be inclined with respect to the irradiation direction of the laser from the laser sensor 12, as shown in FIG. 9(b) , the planar point group data Df virtually face the direction of laser irradiation from the laser sensor 12 directly in front.

Subsequently, the estimation calculation unit 18 extracts a horizontal line corresponding to the pallet portion 7 and a vertical line corresponding to the column portion 8 based on the planes corresponding to the front surfaces 7a and 8a (step S122). At this time, as shown in FIG. The corresponding two vertical lines Lb are extracted. The vertical line Lb is the center line of the plane corresponding to the front surface 8a of the pillar 8, for example. The center line is a line that extends in the vertical direction (Z direction) from the center in the horizontal direction (Y direction) on a plane corresponding to the front surface 8a of the column portion 8 .

The estimation calculation unit 18 extracts straight lines, which are horizontal and vertical lines, by randomly picking up several points of the plane point cloud data and fitting straight lines using, for example, RANSAC (described above).

FIG. 11 is a flow chart showing the details of the straight line extraction processing procedure in step S122. In FIG. 11, the estimation calculation unit 18 first obtains a straight line by performing straight line approximation on the plane point group data after rotation processing (step S151).

Subsequently, the estimation calculation unit 18 extracts the point cloud data on the straight line from the planar point cloud data after the rotation processing (step S152). Subsequently, the estimation calculation unit 18 records the number of reflection points of the point cloud data on the straight line (step S153).

Subsequently, the estimation calculation unit 18 determines whether or not the linear approximation process for the plane point cloud data has been performed a predetermined number of times (for example, 100 times) (step S154). When the estimation calculation unit 18 determines that the linear approximation process for the plane point cloud data has not been performed the predetermined number of times, the above steps S151 to S153 are executed again.

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

For example, when planar point cloud data Df1 as shown in FIG. 12(a) is obtained, if the planar point cloud data Df1 is strangely linearly approximated, as shown in FIGS. 12(b) and 12(c), , the point cloud may be missing. The plane point cloud data Df1 consists of a plurality of reflection points P1.

Therefore, as shown in FIGS. 12(c) and 12(d), after linear approximation is performed on the point cloud data Df2 in which the point group lacks, as shown in FIG. 12(e), Points on the straight line L are extracted from the plane point cloud data Df1. As a result, appropriate horizontal lines and vertical lines are extracted, so errors are less likely to occur in subsequent calculations of the position and orientation of the post-pallet 5 .

Returning to FIG. 8, the estimation calculation unit 18 calculates the intersection of the horizontal line and the vertical line after executing the above step S122 (step S123). At this time, as shown in FIG. 10, four intersections I of two horizontal lines La and two vertical lines Lb are obtained.

Subsequently, the estimation calculation unit 18 calculates the attitude of the post-pallet 5 by calculating the attitude of the column part 8 based on the inclination and the size of the two vertical lines (step S124). As the posture of the post pallet 5, the roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 are calculated. The roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 are calculated from vertical lines corresponding to the column portions 8, as shown in FIG. FIG. 13(a) is a front view of the column portion 8. FIG. 13(b) is a plan view of the column portion 8. FIG. FIG. 13(c) is a side view of the column portion 8. FIG.

The estimation calculation unit 18 preliminarily stores reference values such as the dimensions and inclination of the two vertical lines, and compares the dimensions and inclination of the two vertical lines with the reference values to determine the post pallet 5. A roll angle θa, a yaw angle θb, and a pitch angle θc are calculated. Since the pillars 8 are elongated in the vertical direction, the accuracy of the roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 can be easily obtained. However, if the column portion 8 is not completely fixed to the pallet portion 7, the values of the roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 tend to vary. Therefore, the accuracy of the roll angle θa, yaw angle θb, and pitch angle θc of the post pallet 5 can be improved by averaging the values of the two vertical lines.

Subsequently, the estimation calculation unit 18 calculates the position of the post-pallet 5 by calculating the position of the pallet unit 7 based on the positions of the four intersection points obtained in step S123 (step S125). Specifically, the estimation calculation unit 18 calculates the center position G (see FIG. 2) of the front surface 7a of the pallet unit 7. As shown in FIG. The center position G of the front surface 7a of the pallet unit 7 is represented by three-dimensional position coordinates (x0, y0, z0).

Subsequently, the estimation calculation unit 18 reversely rotates the position and orientation data of the post-pallet 5 obtained in steps S124 and S125 so as to return them to the original XYZ coordinate system (step S126). As a result, the final position and posture of the post pallet 5 with respect to the forklift 1 are obtained. At this time, the XYZ coordinate system after rotation as shown in FIG. 9(b) is transformed into the original XYZ coordinate system as shown in FIG. 9(a). Therefore, the coordinates (x0, y0, z0) of the center position G of the pallet part 7 in the XYZ coordinate system after rotation are the coordinates (x, y, z) of the center position G of the pallet part 7 in the original XYZ coordinate system. becomes.

Subsequently, the estimation calculation unit 18 determines that the plane detected by the plane detection unit 17 corresponds to the front surface of the post pallet 5 based on data such as the dimensions and inclinations of the horizontal and vertical lines and the intersection points of the horizontal and vertical lines. It is finally determined whether or not (step S127). When the estimation calculation unit 18 determines that the detected plane corresponds to the front surface of the post-pallet 5, it outputs data on the position and orientation of the post-pallet 5 to the route generation unit 19 (step S128).

When the estimation calculation unit 18 determines that the detected plane does not correspond to the front surface of the post pallet 5, it deletes the plane point cloud data included in the detected plane (step S129). Then, the estimation calculation unit 18 instructs the plane detection unit 17 to detect the plane again (step S130). Upon receiving the plane re-detection instruction, the plane detector 17 again detects planes corresponding to the front surface 7a of the pallet unit 7 and the front surface 8a of the column unit 8 .

Returning to FIG. 3 , the route generation unit 19 creates a traveling route to the pickup position (described above) where the forklift 1 picks up, based on the position and attitude data of the post pallet 5 obtained by the estimation calculation unit 18 . Generate.

The travel control unit 20 controls the drive unit 14 so that the forklift 1 automatically travels to the pickup position according to the travel route generated by the route generation unit 19 .

As described above, in the present embodiment, the laser sensor 12 irradiates the front surface of the post pallet 5 with laser light, so that the point cloud data corresponding to the post pallet 5 is obtained. Based on the point cloud data corresponding to the post pallet 5, planes corresponding to the front surface 7a of the pallet portion 7 and the front surface 8a of the column portion 8 of the post pallet 5 are detected. Then, horizontal lines corresponding to the pallet portion 7 and vertical lines corresponding to the column portions 8 are extracted based on the planes corresponding to the front surfaces 7a and 8a. Then, the position and posture of the post pallet 5 with respect to the forklift 1 are calculated based on the horizontal line and the vertical line. At this time, since the pillars 8 are elongated in the vertical direction, the position and angle of the post pallet 5 in the vertical direction are likely to be accurate. Thereby, the position and orientation of the post pallet 5 with respect to the forklift 1 are estimated with high accuracy.

Further, in this embodiment, the post-pallet 5 is recognized based on the image data acquired by the camera 11, and the point cloud data corresponding to the recognized post-pallet 5 among the point cloud data acquired by the laser sensor 12 is extracted. Image data has a higher resolution than point cloud data. Therefore, the post-pallet 5 can be recognized with higher accuracy. Therefore, the position and orientation of the post pallet 5 with respect to the forklift 1 can be estimated with even higher accuracy.

In addition, in the present embodiment, the attitude of the pillar 8 is calculated based on the inclination and size of the vertical line corresponding to the pillar 8, and the horizontal line corresponding to the pallet 7 and the vertical line corresponding to the pillar 8 are calculated. The position of the pallet unit 7 is calculated based on the position of the intersection of the . Therefore, the position and orientation of the post pallet 5 with respect to the forklift 1 can be easily calculated.

Further, in this embodiment, by performing plane approximation on the second point cloud data from which the point group has been removed to a thickness larger than the specified thickness to obtain the first plane, when plane approximation is repeatedly performed Variation in the second point cloud data can be dealt with. Then, the third point cloud data included in the first plane is extracted from the first point cloud data, and the second plane is obtained by performing plane approximation on the third point cloud data to a specified thickness. The plane corresponding to the front surface 7a of the column 8 and the front surface 8a of the column 8 is less likely to be chipped, and unnecessary point groups not included in the plane corresponding to the front surfaces 7a and 8a are deleted. In addition, fluctuations in the positions and angles of the planes corresponding to the front surfaces 7a and 8a are suppressed. As described above, the position and posture of the post pallet 5 with respect to the forklift 1 are estimated with higher accuracy.

Further, in the present embodiment, after linear approximation is performed on the plane point cloud data included in the plane corresponding to the front surface 7a of the pallet portion 7 and the front surface 8a of the column portion 8 to obtain a straight line, the plane point cloud data By extracting the point cloud data on the straight line, the horizontal line corresponding to the pallet part 7 and the vertical line corresponding to the column part 8 are less likely to be missing, and unnecessary point groups not included in the horizontal line and the vertical line are removed. Deleted. Therefore, the position and orientation of the post pallet 5 with respect to the forklift 1 can be estimated with higher accuracy.

Further, in this embodiment, the plane point cloud data included in the plane corresponding to the front surface 7a of the pallet part 7 and the front surface 8a of the column part 8 is rotated so that the plane point cloud data virtually faces the laser sensor 12. Thus, the crossing points of the horizontal lines and the vertical lines can be obtained two-dimensionally, ignoring the front-rear direction (X direction) of the post pallet 5 . Therefore, the intersections of horizontal lines and vertical lines can be extracted with high accuracy. Therefore, the position and orientation of the post pallet 5 with respect to the forklift 1 can be estimated with even higher accuracy.

Further, in this embodiment, when it is determined that the detected plane does not correspond to the front surface of the post pallet 5, the point cloud data included in the plane is deleted, and the plane detection process is executed again. An exact plane corresponding to the front face 7a of the pallet part 7 and the front face 8a of the column 8 is obtained. Therefore, the position and orientation of the post pallet 5 with respect to the forklift 1 can be estimated with higher accuracy.

Furthermore, in this embodiment, the fork holes 9 of the pallet part 7 can be found by the camera 11, for example, if there is dust in the fork holes 9 of the pallet part 7 or if the fork holes 9 of the pallet part 7 are not captured in a dark place. Even if it is difficult, the position and posture of the post pallet 5 with respect to the forklift 1 can be estimated. Further, even when the load M is placed on the pallet portion 7 of the post pallet 5, the position and posture of the post pallet 5 including the load M can be estimated.

In addition, this invention is not limited to the said embodiment. For example, in the above embodiment, the post-pallet 5 is recognized based on the image data acquired by the camera 11, and based on the point cloud data corresponding to the recognized post-pallet 5 among the point cloud data acquired by the laser sensor 12. Although the plane corresponding to the front surface 7a of the pallet portion 7 and the front surface 8a of the column portion 8 is detected, the present invention is not limited to such a form. For example, when the position of the post pallet 5 is determined, the planes corresponding to the front surfaces 7a and 8a are detected based only on the point cloud data acquired by the laser sensor 12 without using the camera 11. You may In this case, the configuration of the position/orientation estimation device 21 is simplified.

In the above embodiment, the pallet portion 7 of the post pallet 5 is provided with a pair of left and right fork holes 9, but the present invention is not particularly limited to this form, and the pallet portion is provided with fork holes. It is also applicable to non-post-pallets. In this case, one horizontal line corresponding to the pallet part and two vertical lines corresponding to the pillar part may be extracted based on the plane corresponding to the front surface of the pallet part and the pillar part.

Further, in the above embodiment, the position and orientation of the post pallet 5 placed on the truck bed T are estimated, but the present invention is not particularly limited to that form, and the post pallet 5 is placed on a road surface, a floor surface, or the like. It is also applicable to post pallets.

DESCRIPTION OF SYMBOLS 1... Forklift, 5... Post pallet, 7... Pallet part, 7a... Front surface, 8... Column part, 8a... Front surface, 11... Camera (imaging part), 12... Laser sensor (laser distance detection part), 15... Post pallet Recognition unit 16 Point cloud processing unit 17 Plane detection unit 18 Estimation calculation unit 21 Position and orientation estimation device Dp Point cloud data Dpp Point cloud data Dp1 First point cloud data Dp2 ... second point cloud data, Dp3 ... third point cloud data, Df ... plane point cloud data, Df1 ... plane point cloud data, Df2 ... point cloud data, F1 ... first plane, F2 ... second plane, L ... straight line , La...horizontal line, Lb...vertical line, I...intersection point.

Claims (7)

A position/orientation estimating device for estimating the position and orientation of a post pallet mounted on a forklift and having a pallet on which a load is placed and four pillars erected at the front, rear, left, and right corners of the pallet,
a laser distance detection unit that acquires point cloud data by detecting the distance to the post pallet by irradiating a laser toward the front surface of the post pallet and receiving the reflected light of the laser;
a plane detection unit that detects a plane corresponding to the front surface of the pallet unit and the pillar unit based on the point cloud data acquired by the laser distance detection unit;
an estimation calculation unit that performs an estimation calculation of the position and orientation of the post pallet with respect to the forklift using the plane corresponding to the front surface of the pallet and the pillar detected by the plane detection unit;
The estimation calculation unit extracts a horizontal line corresponding to the pallet portion and a vertical line corresponding to the pillar portion based on a plane corresponding to the front surface of the pallet portion and the pillar portion, and extracts the horizontal line and the vertical line. A position and orientation estimation device for calculating the position and orientation of the post pallet with respect to the forklift based on. an imaging unit that acquires image data by imaging the front surface of the post pallet;
a post-palette recognition unit that recognizes the post-palette based on the image data acquired by the imaging unit;
a point cloud processing unit that extracts point cloud data corresponding to the post pallet recognized by the post pallet recognition unit from among the point cloud data acquired by the laser distance detection unit,
2. The position and orientation estimation apparatus according to claim 1, wherein the plane detection unit detects a plane corresponding to the front surface of the pallet unit and the pillar unit based on the point cloud data extracted by the point cloud processing unit. The estimation calculation unit calculates the attitude of the post pallet with respect to the forklift by calculating the attitude of the pillar based on the inclination and the dimension of the vertical line, and calculates the intersection of the horizontal line and the vertical line. 3. The position and orientation estimation device according to claim 1, wherein the position of the post pallet relative to the forklift is calculated by calculating the position of the pallet unit based on the position of the intersection. The plane detection unit obtains second point cloud data from which the point cloud is removed from the first point cloud data corresponding to the post pallet, and obtains a thickness greater than a specified thickness for the second point cloud data. Then, a plane approximation is performed to obtain the first plane, and then the third point cloud data included in the first plane is extracted from the first point cloud data, and the specified thickness is obtained for the third point cloud data. 4. The position and orientation estimation device according to claim 1, wherein the plane corresponding to the front surface of the pallet section and the pillar section is detected by performing plane approximation to obtain a second plane. The estimation calculation unit obtains a straight line by performing straight line approximation on the plane point cloud data included in the plane corresponding to the front surface of the pallet part and the pillar part. The position and orientation estimation device according to any one of claims 1 to 4, wherein the horizontal line and the vertical line are extracted by extracting certain point cloud data. The estimation calculation unit rotates the plane point cloud data so that the plane point cloud data included in the plane corresponding to the front surfaces of the pallet and the column virtually faces the laser distance detection unit. , the position and orientation of the post pallet with respect to the forklift are calculated based on the horizontal line and the vertical line in a state in which the coordinate system of the plane point cloud data is transformed, and then the position and orientation of the post pallet are converted back to the original 6. The position and orientation estimation device according to any one of claims 1 to 5, wherein the final position and orientation of the post pallet with respect to the forklift are obtained by reverse rotation to return to the coordinate system. After calculating the position and orientation of the post pallet with respect to the forklift, the estimation calculation unit determines whether the plane detected by the plane detection unit When it is determined that the plane detected by the plane detection unit does not correspond to the front surface of the post pallet, the point group included in the plane detected by the plane detection unit 7. The position/orientation estimation device according to claim 1, wherein data is deleted, and the plane detection unit re-executes detection processing of a plane corresponding to the front surface of the pallet section and the column section.

Images