A 3D point cloud analysis and guidance method for robotic arm processing

Abstract

The present invention belongs to the field of artificial intelligence, and relates to a three-dimensional point cloud analysis and guidance method applied to robot arm processing. The method first calibrates the three-dimensional vision system and the robot arm and converts the coordinate system, performs a fast three-dimensional scan of the workpiece through a binocular three-dimensional reconstruction device, obtains a depth map after filtering the obtained three-dimensional point cloud, divides the depth map into blocks according to the diameter of the robot arm tool head and the running step length, determines the block target point and normal vector according to the fitting plane and processing depth of the points in each block, and then calculates the block quaternion, and inputs the coordinate point set and the quaternion set information in the process into the robot arm control end through Ethernet communication, thereby realizing autonomous guidance of the robot arm. Compared with the traditional robot arm teaching programming method, the three-dimensional point cloud analysis and guidance method designed by the present invention can realize personalized and intelligent processing of different workpieces without teaching programming, improves the reliability and accuracy of path planning and guidance, improves production efficiency, reduces production costs, and promotes intelligent processing and manufacturing.

Description

Technical Field

[0001] The present invention designs a three-dimensional point cloud analysis and guidance method for robotic arm processing. More specifically, the present invention relates to a method for rapid three-dimensional measurement of a workpiece with a curved surface to be processed, obtaining processing points and processing postures based on the three-dimensional point cloud, and guiding the robotic arm processing path, thereby realizing autonomous guidance of the robotic arm for processing complex curved surface workpieces. Background Art

[0002] The future development trend of the manufacturing industry is that robotic arms will replace humans in workpiece processing operations. However, due to the diversity of workpieces and interference from factors such as the on-site environment, the current use of robotic arms for workpiece processing is mainly based on single workpieces, and the teach pendant needs to be operated to manually guide the movement of the robotic arm before it is put into use. In the case of high precision requirements, a large amount of surveying and mapping of finished workpiece samples is required in advance, and then the motion path is manually planned according to the surveying and mapping data and written into the robotic arm control end through a special language. After the type of workpiece to be processed changes, the robotic arm needs to be re-operated, and the process is relatively cumbersome.

[0003] In order to reduce the involvement of personnel and increase the applicability of scenarios, the present invention attempts to use machine vision to reconstruct the three-dimensional point cloud of the workpiece to be processed, and calculates the parameters required for the movement of the robotic arm based on the three-dimensional point cloud data, thereby designing a three-dimensional point cloud analysis and guidance method for robotic arm processing to achieve intelligent and unmanned guidance of the robotic arm. Summary of the invention

[0004] The present invention designs a three-dimensional point cloud analysis and guidance method for robotic arm processing. The method can be applied to path planning and posture adjustment during the process of robotic arm processing workpieces, which makes up for the problems that existing robotic arm processing of workpieces requires manual teaching programming, is applicable to a single workpiece model and has low processing efficiency.

[0005] A computer for image acquisition and data processing;

[0006] The camera used to collect images has an image resolution of W×H and two cameras;

[0007] 1 projector for projecting sinusoidal fringes;

[0008] A robotic arm for workpiece processing;

[0009] 1. A three-dimensional point cloud analysis and guidance method for robot arm processing, characterized in that it includes the following steps:

[0010] Step 1: convert the robot base coordinate system to the robot tool coordinate system, calculate the initial vector q0 according to the quaternion of the robot in the initial position after the conversion, start the binocular 3D reconstruction device for generating 3D point cloud data of the workpiece to be processed, select sinusoidal fringe grating projection with frequencies of λ1′=16 and λ2′=18, determine the lateral constraint of the point cloud through phase unwrapping and deconvolution, calibrate the internal and external parameters of the binocular 3D reconstruction device by binocular positioning method, so as to determine the longitudinal constraint, and generate a 3D point cloud containing the workpiece to be processed by calculating the lateral constraint and the longitudinal constraint, and the number of points in the 3D point cloud is N0;

[0011] Step 2: Let the point in the 3D point cloud described in step 1 be denoted as p i =(x1,y i , z i ), i = 1, 2, 3, ..., N0, respectively calculate the mean values ​​of the three axial coordinates of the three-dimensional point cloud μ = (μ x , μ y , μ z ) and variance σ=(σ x , σ y , σ z ), under normal distribution, traverse the point cloud data of the three-dimensional point cloud and calculate the probability r of each point appearing in the correct area i (x), the calculation method is shown in formula (1):

[0012]

[0013] According to the calculated probability rx(x), determine the corresponding point p i Is it an outlier? When any point p i The corresponding probability r i (x) is less than 97%, it is considered as an outlier and removed;

[0014] Step 3: After processing in step 2, the number of points in the three-dimensional point cloud is N1, and the threshold d is set std and the number of cycles K, randomly selecting three points in the three-dimensional point cloud, and calculating the plane A1x+B1y+C1z+D1=0 determined by the three points;

[0015] Step 4: Calculate the Euclidean distance d from each point in the 3D point cloud to the plane A1x+B1y+C1z+D1=0 i , count all the values ​​that meet the threshold d std The number of points N 1i , update the plane parameters A1, B1, C1, D1 of the plane;

[0016] Step 5: Repeat step 4 K times to find the N 1i The largest plane is taken as the optimal plane A c x+B c y+C c z+D c = 0, calculate the distance from each point in the three-dimensional point cloud to the optimal plane A c x+B c y+C c z+D c = The distance between 0 and i , i = 1, 2, 3, ..., N1, remove all the points in the three-dimensional point cloud that satisfy dis i Points <0.5mm;

[0017] Step 6: Project the three-dimensional point cloud onto the xy plane, and define p(x, y, z) as a point in the three-dimensional point cloud. The relationship between the gray value I of the projection point p′ of p(x, y, z) on the two-dimensional image and the point coordinates (x′, y′) and x, y, z is shown in formula (2):

[0018]

[0019] Among them, z max is the maximum z value in the three-dimensional point cloud, z min is the minimum z value in the three-dimensional point cloud, and all points in the three-dimensional point cloud are processed according to formula (2) to obtain a depth map containing depth and position information;

[0020] Step 7: Set the machining width of the robot tool head to d, the running step length of the robot to S, divide the depth map into M0 blocks with a width of d and a length of S, when a block contains a part of the three-dimensional point cloud, the block is regarded as a non-empty block, when a block does not contain any part of the three-dimensional point cloud, it is regarded as a blank block, the blank block between the non-empty block and the non-empty block is regarded as a valid block, the total number of the valid blocks is M1, and the target point of the valid block is p j1 , the normal vector is q j1 , j=1, 2, 3,..., M1;

[0021] Step 8: For the non-empty blocks described in step 7, the number of points is N 2j , point is denoted as p l =(x l ,y l , z l ), l = 1, 2, ..., N 2j The expression of the fitting plane obtained by plane fitting for all points in the block is z = a j0 x+a j1y+a j2 ,in:

[0022]

[0023] Set the processing depth to d0, q j1 is the plane normal vector of the fitting plane, p j1 The normal vector q is the center point of the fitting plane in the block. j1 Move the point at d0 downward;

[0024] Step 9: When there are M2 blank blocks between two non-empty blocks in the same row, take the p of the three non-empty blocks before and after the M2 blank blocks. j1 The projection points on the xz plane form a point set P1, and the cubic fitting polynomial Q(x) = a0+a1x+a2x is calculated for P1. 2 +a3x 3 , take p for the M2 blank blocks respectively j1 =(x j1 , yj 1, Q(x j1 )), where x j1 is the x-axis coordinate value of the center point of the blank area on the xz plane, j1 is the y-axis coordinate value of the center point of the blank block on the xz plane;

[0025] Step 10: For non-empty blocks, according to the initial vector q0 described in step 1 and the normal vector q described in step 7 j1 , we can determine q0, q j1 The rotation angle θ between the rotation axis C and the rotation axis C can be used to obtain the point p j1 , the normal vectors of the non-empty blocks at the left and right ends of the M2 blank blocks obtained in step 8 are recorded as q2 and q3 respectively, and the angle γ between q2 and q3 is calculated. The angle p of the k-1th block between the non-empty blocks at the left and right ends of the M2 blank blocks is obtained by calculation. j1 Run to the kth block p j1 Time required k , k = 1, 2, 3, ..., M2, the 0th block is the non-empty block at the left end of the M2 blank blocks, and the quaternion s of the M2 blank blocks is calculated according to formula (3): k :

[0026]

[0027] Step 11: Repeat steps 8, 9, and 10 until all the valid blocks corresponding to the p j1 and quaternions;

[0028] Step 12: Sort the valid blocks row by row, and sort the blocks in the odd rows according to the corresponding p j1 The x-axis coordinate values ​​of the even-numbered rows are sorted in ascending order, and the blocks in the even-numbered rows are sorted according to the corresponding p j1 The x-axis coordinate values ​​of the blocks are sorted in descending order. After the sorting is completed, the p corresponding to the valid blocks is obtained. j1 and a coordinate point set and a quaternion set composed of quaternions, and sending the coordinate point set and the quaternion set to the robot arm control end through Ethernet to guide the robot arm to perform operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 : Flowchart of autonomous guidance method;

[0030] Figure 2 : Physical picture of the workpiece area to be processed;

[0031] Figure 3 : 3D point cloud and fitted plane diagram of the workpiece area to be processed;

[0032] (a) The overall 3D point cloud of the area to be processed;

[0033] (b) Point cloud within a certain area of ​​the processing area;

[0034] (c) 3D plane fitted by the least squares method of the point cloud within the block;

[0035] (d) Point cloud location distribution map within the block;

[0036] Figure 4 : The robot arm running path planned in the robot arm tool coordinate system;

[0037] Figure 5 : Normal vector of the path point in the robot tool coordinate system;

[0038] Figure 6 : The actual scene of the robotic arm processing workpiece. DETAILED DESCRIPTION

[0039] Firstly, the 3D vision system and the robot arm are calibrated and the coordinate system is converted. The workpiece is quickly scanned in 3D by a binocular 3D reconstruction device. The obtained 3D point cloud is filtered and processed to obtain a depth map. The depth map is divided into blocks according to the diameter of the robot arm tool head and the running step length. The block target point and normal vector are determined according to the fitting plane and processing depth of the points in each block, and then the block quaternion is calculated. The coordinate point set and the quaternion set information in the process are input into the robot arm control end through Ethernet communication, so as to realize the autonomous guidance of the robot arm. The method is characterized in that it includes the following steps:

[0040] Step 1: convert the robot base coordinate system to the robot tool coordinate system, calculate the initial vector q0 according to the quaternion of the robot in the initial position after the conversion, start the binocular 3D reconstruction device for generating 3D point cloud data of the workpiece to be processed, select sinusoidal fringe grating projection with frequencies of λ1′=16 and λ2′=18, determine the lateral constraint of the point cloud through phase unwrapping and deconvolution, calibrate the internal and external parameters of the binocular 3D reconstruction device by binocular positioning method, so as to determine the longitudinal constraint, and generate a 3D point cloud containing the workpiece to be processed by calculating the lateral constraint and the longitudinal constraint, and the number of points in the 3D point cloud is N0;

[0041] Step 2: Let the point in the 3D point cloud described in step 1 be denoted as p i =(x i ,y i , z i ), i = 1, 2, 3, ..., N0, respectively calculate the mean values ​​of the three axial coordinates of the three-dimensional point cloud μ = (μ x , μ y , μ z ) and variance σ=(σ x , σ y , σ z ), under normal distribution, traverse the point cloud data of the three-dimensional point cloud and calculate the probability r of each point appearing in the correct area i (x), the calculation method is shown in formula (1):

[0042]

[0043] According to the calculated probability r i (x) to determine the corresponding point p i Is it an outlier? When any point p i The corresponding probability r i (x) is less than 97%, it is considered as an outlier and removed;

[0044] Step 3: After processing in step 2, the number of points in the three-dimensional point cloud is N1, and the threshold d is set std and the number of cycles K, randomly selecting three points in the three-dimensional point cloud, and calculating the plane A1x+B1y+C1z+D1=0 determined by the three points;

[0045] Step 4: Calculate the Euclidean distance d from each point in the 3D point cloud to the plane A1x+B1y+C1z+D1=0 i , count all the values ​​that meet the threshold d std The number of points N 1i , update the plane parameters A1, B1, C1, D1 of the plane;

[0046] Step 5: Repeat step 4 K times to find the N 1i The largest plane is taken as the optimal plane A c x+B c y+C c z+D c = 0, calculate the distance from each point in the three-dimensional point cloud to the optimal plane A c x+B c y+C c z+D c = The distance between 0 and i , i = 1, 2, 3, ..., N1, remove all the points in the three-dimensional point cloud that satisfy dis i Points <0.5mm;

[0047] Step 6: Project the three-dimensional point cloud onto the xy plane, and define p(x, y, z) as a point in the three-dimensional point cloud. The relationship between the gray value I of the projection point p′ of p(x, y, z) on the two-dimensional image and the point coordinates (x′, y′) and x, y, z is shown in formula (2):

[0048]

[0049] Among them, z max is the maximum z value in the three-dimensional point cloud, z min is the minimum z value in the three-dimensional point cloud, and all points in the three-dimensional point cloud are processed according to formula (2) to obtain a depth map containing depth and position information, and the resolution of the depth map is W×H;

[0050] Step 7: Set the machining width of the robot tool head to d, the robot operation step length to S, divide the depth map into R rows and T columns, a total of M0 blocks with a width of d and a length of S, M0 = R × T, where R is determined by H and d, and T is determined by W and S. When a block contains a part of the three-dimensional point cloud, the block is regarded as a non-empty block. When a block does not contain any part of the three-dimensional point cloud, it is regarded as a blank block. The blank blocks between the non-empty blocks and the non-empty blocks are regarded as valid blocks. The total number of valid blocks is M1, and the target point of the valid block is p. j1 , the normal vector is q j1 , j=1, 2, 3,..., M1;

[0051] Step 8: For the non-empty blocks described in step 7, the number of points is N 2j , point is denoted as p l =(x l ,y l , z l ), l = 1, 2, ..., N 2jThe expression of the fitting plane obtained by plane fitting for all points in the block is z = a j0 x+a j1 y+a j2 ,

[0052] Defined by the least squares method:

[0053]

[0054] The solution is:

[0055]

[0056] Set the processing depth to d0, q j1 is the plane normal vector of the fitting plane (a j0 , a j1 , -1), p j1 The fitting plane is located at the center of the block along the normal vector q j1 Move the point at d0 downward;

[0057] Step 9: When there are M2 blank blocks between two non-empty blocks in the same row, take the p of the three non-empty blocks before and after the M2 blank blocks. j1 The projection points on the xz plane form a point set P1, and the cubic fitting polynomial Q(x) = a0+a1x+a2x is calculated for P1. 2 +a3x 3 , take p for the M2 blank blocks respectively j1 =(x j1 ,y j1 , Q(x j1 )), where x j1 is the x-axis coordinate value of the center point of the blank area on the xz plane, j1 is the y-axis coordinate value of the center point of the blank block on the xz plane;

[0058] Step 10: For non-empty blocks, according to the initial vector q0 described in step 1 and the normal vector q described in step 7 j1 , we can determine q0, q j1 The rotation angle θ between the rotation axis C and the rotation axis C can be used to obtain the point p j1 The quaternion is obtained by recording the normal vectors of the non-empty blocks at the left and right ends of the M2 blank blocks obtained in step 8 as q2 and q3 respectively, and the angle γ between q2 and q3 is calculated. The end of the robot tool is calculated to be between the non-empty blocks at the left and right ends of the M2 blank blocks, from the target point p of the k-1th block. j1 Run to the target point p of the kth block j1 Time required k, k = 1, 2, 3, ..., M2, the 0th block is the non-empty block at the left end of the M2 blank blocks, and the quaternion s of the M2 blank blocks is calculated according to formula (3): k :

[0059]

[0060] Step 11: Repeat steps 8, 9, and 10 until all the valid blocks corresponding to the p j1 and quaternions;

[0061] Step 12: Sort the valid blocks row by row, and sort the blocks in the odd rows according to the corresponding p j1 The x-axis coordinate values ​​of the even-numbered rows are sorted in ascending order, and the blocks in the even-numbered rows are sorted according to the corresponding p j1 The x-axis coordinate values ​​of the blocks are sorted in descending order. After the sorting is completed, the p corresponding to the valid blocks is obtained. j1 and a coordinate point set and a quaternion set composed of quaternions, and sending the coordinate point set and the quaternion set to the robot arm control end through Ethernet to guide the robot arm to perform operations.

[0062] The beneficial effects of the present invention are: realizing the intelligentization of the processing process, solving the problems that the robot arm needs to manually plan the path before processing the workpiece, manually teach and guide the robot arm to process, and the processing process has a single application scope and low adjustment efficiency.

[0063] To sum up, the advantages of the three-dimensional point cloud analysis and guidance method designed by the present invention for robotic arm processing are: it realizes autonomous path planning and posture adjustment of the robotic arm during workpiece processing. Compared with the traditional robotic arm teaching programming method, personalized and intelligent processing of different workpieces can be achieved without teaching programming, which improves the reliability and accuracy of path planning and guidance, improves production efficiency, reduces production costs, and promotes intelligent processing and manufacturing.

[0064] The above is a schematic description of the present invention and its implementation mode, which is not limited. The drawings are only one of the implementation modes of the present invention. Therefore, if a person skilled in the art is inspired by it and adopts other forms of similar components or other forms of component layout without departing from the purpose of the invention, and designs a technical solution and embodiment similar to the technical solution without creativity, they should all fall within the protection scope of the present invention.

Claims

1. A three-dimensional point cloud analysis and guidance method for robot arm processing, characterized in that: The following steps are involved: Step 1: convert the robot base coordinate system to the robot tool coordinate system, calculate the initial vector q0 according to the quaternion of the robot in the initial position after the conversion, start the binocular 3D reconstruction device for generating 3D point cloud data of the workpiece to be processed, select sinusoidal fringe grating projection with frequencies of λ1′=16 and λ2′=18, determine the lateral constraint of the point cloud through phase unwrapping and deconvolution, calibrate the internal and external parameters of the binocular 3D reconstruction device by binocular positioning method, so as to determine the longitudinal constraint, and generate a 3D point cloud containing the workpiece to be processed by calculating the lateral constraint and the longitudinal constraint, and the number of points in the 3D point cloud is N0; Step 2: Let the point in the 3D point cloud described in step 1 be denoted as p i =(x i ,y i , z i ), i = 1, 2, 3, ..., N0, respectively calculate the mean values ​​of the three axial coordinates of the three-dimensional point cloud μ = (μ x , μ y , μ z ) and variance σ=(σ x , σ y , σ z ), under normal distribution, traverse the point cloud data of the three-dimensional point cloud and calculate the probability r of each point appearing in the correct area i (x), the calculation method is shown in formula (1): According to the calculated probability r i (x) to determine the corresponding point p i Is it an outlier? When any point p i The corresponding probability r i (x) is less than 97%, it is considered as an outlier and removed; Step 3: After processing in step 2, the number of points in the three-dimensional point cloud is N1, and the threshold d is set std and the number of cycles K, randomly selecting three points in the three-dimensional point cloud, and calculating the plane A1x+B1y+C1z+D1=0 determined by the three points; Step 4: Calculate the Euclidean distance d from each point in the 3D point cloud to the plane A1x+B1y+C1z+D1=0 i , count all the values ​​that meet the threshold d std The number of points N 1i , update the plane parameters A1, B1, C1, D1 of the plane; Step 5: Repeat step 4 K times to find the N 1i The largest plane is taken as the optimal plane A c x+B c y+C c z+D c = 0, calculate the distance from each point in the three-dimensional point cloud to the optimal plane A c x+B c y+C c z+D c = The distance between 0 and i , i = 1, 2, 3, ..., N1, remove all the points in the three-dimensional point cloud that satisfy dis i Points <0.5mm; Step 6: Project the three-dimensional point cloud onto the xy plane, and define p(x, y, z) as a point in the three-dimensional point cloud. The relationship between the gray value I of the projection point p′ of p(x, y, z) on the two-dimensional image and the point coordinates (x′, y′) and x, y, z is shown in formula (2): Among them, z max is the maximum z value in the three-dimensional point cloud, z min is the minimum z value in the three-dimensional point cloud, and all points in the three-dimensional point cloud are processed according to formula (2) to obtain a depth map containing depth and position information; Step 7: Set the machining width of the robot tool head to d, the running step length of the robot to S, divide the depth map into M0 blocks with a width of d and a length of S, when a block contains a part of the three-dimensional point cloud, the block is regarded as a non-empty block, when a block does not contain any part of the three-dimensional point cloud, it is regarded as a blank block, the blank block between the non-empty block and the non-empty block is regarded as a valid block, the total number of the valid blocks is M1, and the target point of the valid block is p j1 , the normal vector is q j1 , j=1, 2, 3,..., M1; Step 8: For the non-empty blocks described in step 7, the number of points is N 2j , point is denoted as p l =(x l ,y l , z l ), l = 1, 2, ..., N 2j The expression of the fitting plane obtained by plane fitting for all points in the block is z = a j0 x+a j1 y+a j2 ,in: Set the processing depth to d0, q j1 is the plane normal vector of the fitting plane, p j1 The normal vector q is the center point of the fitting plane in the block. j1 Move the point at d0 downward; Step 9: When there are M2 blank blocks between two non-empty blocks in the same row, take the p of the three non-empty blocks before and after the M2 blank blocks. j1 The projection points on the xz plane form a point set P1, and the cubic fitting polynomial Q(x) = a0+a1x+a2x is calculated for P1. 2 +a3x 3 , take p for the M2 blank blocks respectively j1 =(x j1 ,y j1 , Q(x j1 )), where x j1 is the x-axis coordinate value of the center point of the blank area on the xz plane, j1 is the y-axis coordinate value of the center point of the blank block on the xz plane; Step 10: For non-empty blocks, according to the initial vector q0 described in step 1 and the normal vector q described in step 7 j1 , we can determine q0, q j1 The rotation angle θ between the rotation axis C and the rotation axis C can be used to obtain the point p j1 , the normal vectors of the non-empty blocks at the left and right ends of the M2 blank blocks obtained in step 8 are recorded as q2 and q3 respectively, and the angle γ between q2 and q3 is calculated. The angle p of the k-1th block between the non-empty blocks at the left and right ends of the M2 blank blocks is obtained by calculation. j1 Run to the kth block p j1 Time required k , k = 1, 2, 3, ..., M2, the 0th block is the non-empty block at the left end of the M2 blank blocks, and the quaternion s of the M2 blank blocks is calculated according to formula (3): k : Step 11: Repeat steps 8, 9, and 10 until all the valid blocks corresponding to the p j1 and quaternions; Step 12: Sort the valid blocks row by row, and sort the blocks in the odd rows according to the corresponding p j1 The x-axis coordinate values ​​of the even-numbered rows are sorted in ascending order, and the blocks in the even-numbered rows are sorted according to the corresponding p j1 The x-axis coordinate values ​​of the blocks are sorted in descending order. After the sorting is completed, the p corresponding to the valid blocks is obtained. j1 and a coordinate point set and a quaternion set composed of quaternions, and sending the coordinate point set and the quaternion set to the robot arm control end through Ethernet to guide the robot arm to perform operations.

Images