3dTOF camera rapid calibration method and system for pallet identification

Abstract

The present invention discloses a 3D TOF camera rapid calibration method for pallet recognition, comprising the following steps: starting the 3D TOF camera to start scanning, stopping scanning after the number of scanned frames exceeds a set number threshold, and storing all collected depth maps in a collection. depth_list Medium; Set-based depth_list All depth maps in the _D_ are used to calculate the mean depth map, and the mean depth map is binarized to form a binary map binary ; In the binary image binary Extract the forklift gear centerline; determine the forklift center based on the forklift gear centerline, and calculate the forklift center's pose in the camera coordinate system to complete the 3D TOF camera calibration. This solution eliminates the need to extract the left and right planes of the forklift gear during calibration, thus avoiding the non-parallel error between the left and right gear planes caused by the manufacturing process. During calibration, the intersection of the gear centerline and the bottom edge of the image is calculated to determine the forklift coordinate system center. Compared to determining the forklift coordinate system center by fitting the forklift gear point cloud, this avoids the error caused by unstable point cloud fitting centers.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot positioning, and more specifically, the present invention relates to a 3dTOF camera rapid calibration method and system for pallet identification. Background Art

[0002] As mobile robots become increasingly involved in intelligent manufacturing, automated forklifts are also widely used in production processes such as material insertion and handling. Positioning and navigation technology is a prerequisite for automated forklifts to perform all tasks. Automated forklifts typically use 2D LiDAR, 3D LiDAR, or visual sensors for positioning. However, these solutions can only calculate the position of the laser forklift within a global map and cannot guide the automated forklift to insert irregularly placed pallets. To address this issue, a pallet recognition system based on a 3dTOF camera is usually installed above the automated forklift's inserting gear. The pallet recognition system uses an algorithm to process the point cloud data collected by the 3dTOF camera, calculate the position of the pallet in the 3dTOF camera, and assist the laser forklift in inserting the pallet. Before using the pallet recognition system, the relative position relationship between the 3dTOF camera and the automated forklift must be calibrated in advance.

[0003] Application publication number: CN 113284197 A, application publication date: 2021.08.20, patent name: AGV TOF camera external parameter calibration method and device, electronic equipment; it uses 3dTOF point cloud data for plane fitting to obtain the ground plane and the left and right forklift gear planes, calculates the attitude angle of the forklift relative to the 3dTOF camera through the left and right forklift plane normal vectors, and then fits the left and right gear center points through the point cloud on the left and right gears, calculates the Z-axis and X-axis translation of the gear center point relative to the 3dTOF camera, and finally extracts the inner edge line of the forklift gear through the ground point cloud, thereby calculating the Y-axis translation of the forklift relative to the 3dTOF camera. However, this method has the following problems:

[0004] 1) Due to manufacturing errors, the left and right gear shaping planes of the forklift may not be flat and may not be in the same plane. The attitude angle calculation error using the normal vector after plane fitting is large;

[0005] 2) The X-axis offset of the TOF camera relative to the forklift is calculated using the center point of the forklift gear plane point cloud. The center point fitted from the gear plane point cloud is unstable and has a large error, so the calculated X-axis offset is inaccurate.

[0006] 3) The accuracy of plane fitting using a single point cloud measurement with a 3dTOF camera is low, and is not as good as the accuracy of plane fitting after averaging multiple measurements. Summary of the Invention

[0007] The present invention provides a 3dTOF camera rapid calibration method for pallet identification, aiming to improve the above-mentioned problem.

[0008] The present invention is implemented as follows: a 3dTOF camera rapid calibration method for pallet recognition, the method comprising the following steps:

[0009] S1. Start the 3D TOF camera to start scanning. When the number of scanned frames exceeds the set threshold, stop scanning and store all collected depth maps in the set depth_list.

[0010] S2. Calculate the mean depth map based on all depth maps in the set depth_list, and binarize the mean depth map to form a binary map binary;

[0011] S3, extracting the center line of the forklift gear in the binary image;

[0012] S4. Determine the forklift center based on the forklift gear centerline, and calculate the position of the forklift center in the camera coordinate system to complete the 3dTOF camera calibration.

[0013] Furthermore, the method for constructing the mean depth map is as follows:

[0014] Take all the depth maps from the collection depth_list respectively p The depth value depth at the position (r,c) is used to calculate the mean depth map. p The depth mean and standard deviation at (r, c) determine the depth standard deviation δ at the current position p(r, c) rc Is it greater than the set maximum depth standard deviation max_δ rc If the detection result is yes, the depth at the current position point p(r,c) is set to infinity in the mean depth map. If the detection result is no, the depth at the current position point p(r,c) is set to the average depth value in the mean depth map. All positions in the depth image are traversed sequentially to form a mean depth map.

[0015] Furthermore, the construction method of the binary graph binary is as follows:

[0016] A binary image with the same length and width as the depth mean image is created. The pixel value of each pixel in the binary image is set to 0. The mean depth map is traversed row by row. If the depth difference between the current pixel and one of the adjacent pixels in the same row is greater than the set depth difference, and the depth value of the current pixel is less than the corresponding adjacent pixel, the pixel value at the same position of the pixel in the binary image is set to 255. After traversing all pixels in the binary image, the final binary image is output.

[0017] Furthermore, the extraction process of the forklift gear centerline is as follows:

[0018] (a) Extracting straight lines from binary images using Hough transform;

[0019] (b) Traverse all the lines. If two lines are parallel, take a point f on one of the lines and calculate the foot of the perpendicular g of the point on the other line.

[0020] (c) Calculate the actual distance between points f and g based on the depths of the two points on the mean depth graph. If the actual distance between the two parallel lines is equal to the width of the forklift gear, then the two lines are the two sides of a single gear. Use the equations of these two lines to calculate the equation of the gear centerline.

[0021] Furthermore, the method for obtaining the position of the forklift center in the camera coordinate system is as follows:

[0022] Calculate the two intersection points c1 and c2 of the two gear shaping center lines l1 and l2 with the bottom edge of the binary graph, and find the perpendicular points p1 and p2 to the other gear shaping center line through the two intersection points c1 and c2, and keep the perpendicular point p1 with the larger y coordinate. i and the intersection point c corresponding to the foot of the perpendicular i ;

[0023] Determine the end points e1 and e2 of the gear shaping, and find the perpendicular points q1 and q2 through the end points e1 and e2 to the other gear shaping center line, and keep the perpendicular point q with the smaller y coordinate. i And the intersection point e corresponding to the foot point i ;

[0024] According to the mean depth map c i 、p i ,q i 、e i The depth values ​​are used to calculate the coordinates of the four points C, P, Q, and E in the camera coordinate system. The midpoint O1 of C and P and the midpoint O2 of Q and E are then calculated. O1 (X1, Y1, Z1) is the position of the forklift center in the camera coordinate system.

[0025] Calculate the vector V from the midpoint O1 to the midpoint O2 12 (X 12 ,Y 12 ,Z 12 ), calculate the vector V 12 The pose (θ x ,θ y ,θ z ), the forklift’s position in the camera coordinate system is pose(X1, Y1, Z1, θ x,θ y ,θ z ).

[0026] Furthermore, the method for determining the end points e1 and e2 is as follows:

[0027] The two intersection points c1 and c2 are taken as the starting points, and the pixels are traversed along their respective gear center lines l1 and l2 in the direction of increasing y coordinates on the binary image binary. When the pixel value is detected to be 0, the point is recorded as the end point of the corresponding gear center line, and the end points e1 and e2 of the gear center lines l1 and l2 are obtained respectively.

[0028] Furthermore, the vector V 12 (X 12 ,Y 12 ,Z 12 ) in the camera coordinate system (θ x ,θ y ,θ z ) The calculation formula is as follows:

[0029] θ x =atan(Z 12 / Y 12 );

[0030] θ y =atan(X 12 / Z 12 );

[0031] θ z =atan(Y 12 / X 12 ).

[0032] Furthermore, the depth mean and standard deviation of the mean depth map at p(r,c) are calculated as follows:

[0033]

[0034]

[0035] in, Represents the average depth value of n depth maps at position p(r,c), represents the depth value of the i-th depth map at position p(r,c), δ rc is the standard deviation of the depth of n depth maps at position p(r,c).

[0036] Furthermore, the system includes:

[0037] The 3dTOF camera is located above the forklift gear. The front half of the forklift gear is in the field of view of the 3dTOF camera. The 3dTOF camera is connected to the processor.

[0038] The 3D TOF camera is started to scan and the scanned frames are sent to the processor. The processor determines the position of the forklift center in the camera coordinate system based on the above-mentioned 3D TOF camera fast calibration method for pallet recognition.

[0039] The present invention does not rely on additional equipment and environment, and the calibration process can be completed by the forklift itself in a natural environment; the calibration does not require the laser forklift to perform specific actions, and the calibration process is simple; during the calibration process, the forklift does not need to output additional information such as posture, and the requirements for the forklift itself are low.

[0040] During calibration, this solution does not require the extraction of the left and right planes of the forklift gear, thus avoiding the non-parallel error of the left and right gear planes caused by the manufacturing process of the forklift gear. During calibration, the intersection of the gear centerline and the bottom edge of the image is calculated to determine the center of the forklift coordinate system. Compared with determining the center of the forklift coordinate system by fitting the forklift gear point cloud, this avoids the error of unstable point cloud fitting center. During calibration, the average depth of multiple data acquisitions is used to calculate, thus avoiding the depth error of single data acquisition and achieving higher calibration accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 A schematic diagram of the structure of a 3dTOF camera rapid calibration system for pallet identification provided by an embodiment of the present invention;

[0042] Figure 2 A flowchart of a 3dTOF camera rapid calibration method for pallet recognition provided by an embodiment of the present invention;

[0043] Figure 3 A schematic diagram of determining the forklift center provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0044] The specific implementation methods of the present invention will be further explained in detail below by describing the embodiments with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0045] Figure 1 The schematic diagram of the structure of the 3dTOF camera rapid calibration system for pallet identification provided by an embodiment of the present invention shows only the parts related to the embodiment of the present invention for ease of explanation.

[0046] The 3dTOF camera is located above the forklift gear. The front half of the forklift gear is in the field of view of the 3dTOF camera. The 3dTOF camera is connected to the processor.

[0047] The midpoint of the line connecting the midpoints of the two side tines is defined as the positioning origin of the forklift, and the forklift is located on the leveled ground.

[0048] The 3dTOF camera is raised synchronously with the lifting of the gear. When the gear is raised to the set height from the ground, the 3dTOF camera is controlled to scan. The internal parameter (f x ,f y ) and the length and width of the image captured by the camera are both known and fixed values, (f x ,f y ) is the focal length of the 3dTOF camera.

[0049] Figure 2 This is a flow chart of a 3dTOF camera rapid calibration method for pallet recognition provided by an embodiment of the present invention. The method specifically includes the following steps:

[0050] (1) Data collection: Start the TOF camera to scan, stop scanning after collecting 50 frames of data, and store all collected depth maps in a collection;

[0051] (2) Calculate the mean depth map: Create a new mean depth map with a length and width of (length, width). Calculate the depth mean and standard deviation of each pixel based on the 50 depth images in depth_list. Given a point p(r, c) on the depth map, where r and c are the row and column coordinates of the point in the image, take the depth value depth at the position of p(r, c) from each of the 50 depth maps, and calculate the depth mean and standard deviation of the mean depth map at p(r, c). The calculation formula is as follows:

[0052]

[0053]

[0054] in, Represents the average depth value of n depth maps at position p(r,c), represents the depth value of the i-th depth map at position p(r,c), δ rc is the standard deviation of the depth of n depth maps at position p(r,c).

[0055] Determine the depth standard deviation δ of the current position point p(r,c) rc Is it greater than the set maximum depth standard deviation max_δ rc If the detection result is yes, the depth at the current position point p(r,c) is set to infinity in the mean depth map. If the detection result is no, the depth at the current position point p(r,c) is set to the average depth value in the mean depth map. Traverse all the position points in the depth image in turn to form a mean depth map with a length and width of (length, width).

[0056] (3) Binarize the mean depth map: Create a new binary map with the same length and width as the mean depth map, set the pixel value of each pixel in the binary map to 0, and traverse the mean depth map row by row. If the depth difference between the current pixel and one of the adjacent pixels in the same row (the previous pixel or the next pixel) is greater than 0.5 meters, and the depth value of the current pixel is less than the corresponding adjacent pixel, then set the pixel value of the same position of the pixel in the binary map to 255. After traversing all pixels, the final binary map is obtained.

[0057] (4) Extract the center line of the forklift gear from the binary image. The extraction process is as follows:

[0058] (a) Extracting straight lines from binary images using Hough transform;

[0059] (b) Traverse all the lines. If two lines are parallel, take a point f on one of the lines and calculate the foot of the perpendicular g of the point on the other line.

[0060] (c) Calculate the coordinates of the two pixel points f and g in the camera coordinate system based on the depths of the two points f and g on the mean depth map, and then calculate the actual distance between the two points f and g. If the actual distance between the two parallel lines is equal to the width of the forklift gear, then the two lines are the two sides of a single gear. Use the linear equations of these two sides to calculate the linear equation of the gear centerline, and obtain the linear equations of the left and right gear centerlines:

[0061]

[0062] (5) Calculate the position of the forklift in the TOF camera:

[0063] Calculate the two intersection points c1 and c2 of the left and right gear center lines l1 and l2 with the bottom edge of the binary graph, that is, the line y = 0, and use the two intersection points c1 and c2 to find the perpendicular points p1 and p2 to the other gear center line, and keep the perpendicular point p with the larger y coordinate. i and the intersection point c corresponding to the foot of the perpendicular i ;

[0064] The two intersection points c1 and c2 are taken as the starting points. The pixels are traversed along the respective gear center lines l1 and l2 on the binary image in the direction of increasing y coordinates. When the pixel value is detected to be 0, the point is recorded as the end point of the corresponding gear center line. The end points e1 and e2 of the gear center lines l1 and l2 are obtained respectively. The perpendicular points q1 and q2 of the other gear center line are found through the end points e1 and e2 respectively. The perpendicular point q1 with the smaller y coordinate is retained. i And the intersection point e corresponding to the foot point i ;

[0065] According to the four points c on the mean depth map i 、p i ,q i 、e i The depth value of c is calculated using step 6) i 、p i ,q i 、e i The coordinates of the four points C, P, Q, and E in the camera coordinate system are calculated, and then the midpoint O1 of C and P and the midpoint O2 of Q and E are calculated respectively. O1 (X1, Y1, Z1) is the position of the forklift center in the camera coordinate system;

[0066] Calculate the vector V from the midpoint O1 to the midpoint O2 12 (X 12 ,Y 12 ,Z 12 )=O2-O1, calculate the vector V according to the following formula 12 The pose in the camera coordinate system (θ x ,θ y ,θ z ):

[0067] θ x =atan(Z 12 / Y 12 );

[0068] θ y =atan(X 12 / Z 12 );

[0069] θ z =atan(Y 12 / X 12 );

[0070] From the above calculations, we can get the pose of the forklift in the camera coordinate system as pose(X1, Y1, Z1, θ x ,θ y ,θ z ).

[0071] (6) Convert the 2D pixel coordinates to the 3D point cloud coordinates in the camera coordinate system based on the depth value;

[0072] Given (x′, y′) as the pixel coordinates of a point and the pixel depth depth of the point in the mean depth map, the 3D point cloud coordinates (x, y, z) of the pixel can be calculated as follows:

[0073]

[0074] The present invention has been described exemplarily. Obviously, the specific implementation of the present invention is not limited to the above-mentioned method. As long as various non-substantial improvements are made using the method concept and technical solution of the present invention, or the concept and technical solution of the present invention are directly applied to other occasions without improvement, they are all within the scope of protection of the present invention.

Claims

1. A 3dTOF camera rapid calibration method for pallet recognition, characterized in that: The method comprises the following steps: S1. Start the 3D TOF camera to start scanning. When the number of scanned frames exceeds the set threshold, stop scanning and store all collected depth maps in the set depth_list. S2. Calculate the mean depth map based on all depth maps in the set depth_list, and binarize the mean depth map to form a binary map binary; S3, extracting the center line of the forklift gear in the binary image; S4. Determine the forklift center based on the forklift gear centerline and calculate the position of the forklift center in the camera coordinate system to complete the calibration of the 3dTOF camera. The method for obtaining the position of the forklift center in the camera coordinate system is as follows: Calculate the two intersection points c1 and c2 of the two gear shaping center lines l1 and l2 with the bottom edge of the binary graph, and find the perpendicular points p1 and p2 to the other gear shaping center line through the two intersection points c1 and c2, and keep the perpendicular point p1 with the larger y coordinate. i and the intersection point c corresponding to the foot of the perpendicular i ; Determine the end points e1 and e2 of the gear shaping, and find the perpendicular points q1 and q2 through the end points e1 and e2 to the other gear shaping center line, and keep the perpendicular point q with the smaller y coordinate. i And the intersection point e corresponding to the foot point i ; According to the mean depth map c i 、p i ,q i 、e i The depth values ​​are used to calculate the coordinates of the four points C, P, Q, and E in the camera coordinate system. The midpoint O1 of C and P and the midpoint O2 of Q and E are then calculated. O1 (X1, Y1, Z1) is the position of the forklift center in the camera coordinate system. Calculate the vector V from the midpoint O1 to the midpoint O2 12 (X 12 ,Y 12 ,Z 12 ), calculate the vector V 12 The pose (θ x ,θ y ,θ z ), the forklift’s position in the camera coordinate system is pose(X1, Y1, Z1, θ x ,θ y ,θ z ); The method for determining the end points e1 and e2 is as follows: The two intersection points c1 and c2 are taken as the starting points, and the pixels are traversed along the respective gear shaping center lines l1 and l2 in the direction of increasing y coordinates on the binary image. When the pixel value is detected to be 0, the point is recorded as the end point of the corresponding gear shaping center line, and the end points e1 and e2 of the gear shaping center lines l1 and l2 are obtained respectively; Vector V 12 (X 12 ,Y 12 ,Z 12 ) in the camera coordinate system (θ x ,θ y ,θ z ) The calculation formula is as follows: θ x =arctan(Z 12 / Y 12 ): θ y =arctan(X 12 / Z 12 ): θ z =arctan(Y 12 / X 12 )。 2. The 3dTOF camera rapid calibration method for pallet recognition according to claim 1, characterized in that: The method for constructing the mean depth map is as follows: Take the depth value depth at the position p(r,c) from all depth maps in the set depth_list, calculate the depth mean and standard deviation of the mean depth map at p(r,c), and determine the depth standard deviation δ at the current position p(r,c) rc Is it greater than the set maximum depth standard deviation max_δ rc If the detection result is yes, the depth at the current position point p(r,c) is set to infinity in the mean depth map. If the detection result is no, the depth at the current position point p(r,c) is set to the average depth value in the mean depth map. All positions in the depth image are traversed sequentially to form a mean depth map.

3. The 3dTOF camera rapid calibration method for pallet recognition according to claim 1, characterized in that: The construction method of binary graph binary is as follows: A binary image with the same length and width as the mean depth image is used. The pixel value of each pixel in the binary image is set to 0. The mean depth map is traversed row by row. If the depth difference between the current pixel and one of the adjacent pixels in the same row is greater than the set depth difference, and the depth value of the current pixel is less than the corresponding adjacent pixel, the pixel value at the same position of the pixel in the binary image is set to 255. After traversing all pixels in the binary image, the final binary image is output.

4. The 3dTOF camera rapid calibration method for pallet recognition according to claim 1, characterized in that: The extraction process of the forklift gear centerline is as follows: (a) Extracting straight lines from binary images using Hough transform; (b) Traverse all the lines. If two lines are parallel, take a point f on one of the lines and calculate the foot of the perpendicular g of the point on the other line. (c) Calculate the actual distance between points f and g based on the depths of the two points on the mean depth graph. If the actual distance between the two parallel lines is equal to the width of the forklift gear, then the two lines are the two sides of a single gear. Use the equations of these two lines to calculate the equation of the gear centerline.

5. The 3dTOF camera rapid calibration method for pallet recognition according to claim 2, characterized in that: The depth mean and standard deviation of the mean depth map at p(r,c) are calculated as follows: in, Represents the average depth value of n depth maps at position p(r,c), represents the depth value of the i-th depth map at position p(r,c), δ rc is the standard deviation of the depth of n depth maps at position p(r,c).

6. A 3dTOF camera rapid calibration system for pallet identification, characterized in that: The system comprises: The 3dTOF camera is located above the forklift gear. The front half of the forklift gear is in the field of view of the 3dTOF camera. The 3dTOF camera is connected to the processor. The 3D TOF camera is started to scan and the scanned frame is sent to the processor. The processor determines the position of the forklift center in the camera coordinate system based on the 3D TOF camera rapid calibration method for pallet recognition described in any one of claims 1 to 5.

Images