A method and apparatus for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction.

Abstract

This invention discloses a method and apparatus for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction. The implementation system of this invention comprises: a binocular camera (1), a multi-joint traction robotic arm (2), a sampling device (3), and a sampling device (4). The binocular camera captures images of the sampling area, and the sampling area is reconstructed using binocular stereo vision to obtain a point cloud. Based on the spatial information contained in the point cloud, the target pose of the sampling device at the sampling point is determined, which is then used for the robotic arm's traction. To overcome the problem of significant deviation in robotic arm traction, this invention compares the difference between the actual pose of the sampling device and the target pose, and uses 3D point cloud analysis to provide reference indicators for operator evaluation and adjustment. This invention can assist operators in quickly and accurately fine-tuning the sampling robotic arm during sampling tasks, thereby improving sampling efficiency and quality.

Description

Technical Field

[0001] This invention relates to the field of robotic arm sampling technology, specifically to a method and apparatus for fine-tuning the pose of a robotic arm used for table sampling. Background Technology

[0002] In robotic arm sampling tasks, binocular cameras are often used as one of the information acquisition methods to assist in the sampling process. After the images captured by the binocular camera are calibrated, the pixel correspondence can be matched relatively accurately to estimate the disparity, and depth information can be calculated from this. The two-dimensional image is then projected into three-dimensional space to obtain three-dimensional point cloud information.

[0003] The robotic arm for surface sampling has a sampling device fixed to its end effector. When performing surface sampling, the arm starts moving from its initial position and stops moving when it reaches the target sampling position, and then performs the surface sampling operation. Throughout the sampling process, the position and posture during surface sampling directly affect the sampling effect and quality.

[0004] Due to factors such as flexible deformation and uncertain posture, traction robotic arms often exhibit significant deviations when pulling sampling devices to the predetermined target position. This necessitates further fine-tuning through autonomous adjustment or manual remote operation, combined with other measurement methods. To address this issue, existing methods mostly only input the approximate target pose to guide the robotic arm to the vicinity of the target point, then rely on manual adjustments based on telemetry data and 2D image information to gradually bring the sampling device closer to an optimal sampling pose. This process involves limited reference information, with the completion of adjustments entirely dependent on the operator's judgment. Furthermore, the adjustment results cannot be directly fed back to the operator, resulting in significant adjustment difficulty, a cumbersome process, and high demands on the operator's technical experience. This makes it difficult to achieve rapid and accurate fine-tuning, leading to low overall adjustment efficiency and difficulty in guaranteeing adjustment quality, thus significantly impacting the efficiency and quality of sampling.

[0005] In sampling tasks such as sampling from the surface of extraterrestrial objects, the sampling time window is very limited, requiring high sampling efficiency and quality to ensure that the device collects a sufficient amount of samples of interest. Therefore, how to improve the overall sampling efficiency and sampling quality is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0006] In view of this, the present invention designs a fine-tuning strategy determination method for a sampling robotic arm, which can achieve rapid and accurate determination and fine-tuning of the sampling target pose. The method utilizes binocular stereo vision for 3D reconstruction, and analyzes the sampling state and pose deviation using 3D point cloud information of the sampling area. This provides operators with intuitive reference and decision-making, or assists the robotic arm in autonomous adjustment, thereby improving sampling efficiency, ensuring sampling quality, and facilitating the overall sampling process.

[0007] To achieve the above objectives, the present invention adopts the following technical solution:

[0008] A method for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction includes the following steps:

[0009] S1. The binocular camera captures images of the complete sampling area, and the captured images are corrected according to the camera's intrinsic and extrinsic parameters determined by the binocular camera calibration to align the epipolar lines.

[0010] S2. Perform stereo matching on the two calibrated images, search for corresponding points to estimate pixel correspondence, and thus obtain disparity information. Then, calculate the depth of each pixel based on the binocular camera extrinsic parameters determined by camera calibration to obtain the corresponding three-dimensional coordinates, and map the two-dimensional image to the global coordinate system three-dimensional space to obtain the point cloud information of the sampling area;

[0011] S3. Using the coordinates of the target sampling point as the center of the sphere, obtain a spherical enclosed region according to the set search radius, and use the points in the sampling region point cloud located within the enclosed region to fit the plane and obtain the slope angle of the local area near the target sampling point;

[0012] S4. Based on the coordinates of the sampling points and the slope angle information of the local area, and according to the set sampling depth, determine the coordinates of the robotic arm wrist center and the pitch angle of the sampling device, thereby determining the precise target pose matrix of the sampling device;

[0013] S5. The robotic arm pulls the sampling device to the target area according to the target pose matrix. By obtaining the actual pose of the current sampling device, the deviation between the current pose and the target pose is calculated and the robotic arm is controlled to make adjustments.

[0014] S6. Based on the current pose of the sampler after adjustment, use the point cloud of the sampling area and the point cloud of the standard sampling device to analyze the accessible terrain area of ​​the sampling device and the sampling depth, angle and other information, calculate and visualize the distance of each part of the sampling device from the terrain, and provide intuitive information feedback to the operator, who will then confirm and decide whether to continue the adjustment.

[0015] After the above steps are completed, the sampling device is in the ideal sampling position, achieving the fine-tuning effect of table sampling.

[0016] The specific content of S3 includes:

[0017] (1) Establish a KD search tree for the point cloud of the sampling area, traverse the distance from each point to the target sampling point, and retain the points whose distance is less than the set search radius R, that is, the point set within the spherically enclosed area of ​​the target sampling point. Specifically, use the KD tree to search for the nearest neighbor of the target sampling point in the point cloud of the sampling area. If its distance is less than R, it is added to the point set, and the search for the next nearest neighbor is performed. Repeat this process until the distance between the nearest neighbor and the target sampling point is greater than R. At this point, all point cloud points within the spherically enclosed area of ​​the target sampling point have been included in the point set.

[0018] (2) In the global coordinate system, establish a plane mathematical model of Ax + By + Cz + D = 0, and use the RANSAC method to find the plane parameters that maximize the number of points in the point set, thus determining the fitting plane. Specifically, randomly select several points from the point set, calculate the plane equation parameters to determine the plane, and iterate through the distances from all points in the point set to the plane, treating points with distances less than a certain threshold as interior points. After repeating this process a certain number of times, the plane with the most interior points is the better fitting plane for the point set.

[0019] (3) Obtain the fitting plane normal vector (A,B,C) through the planar model, and thus determine its angle with the global coordinate system Z-axis, i.e., the slope angle β:

[0020]

[0021] The normal vector and slope angle directly reflect the spatial tilt of the area near the target sampling point and can be used as reference values ​​for subsequent calculation of the target pose matrix.

[0022] The specific content of S4 includes:

[0023] (1) Set the sampling depth h according to the actual sampling requirements;

[0024] (2) In the global coordinate system, the angle between the arm extension direction and the X-axis of the global coordinate system, i.e., the azimuth angle, is calculated based on the coordinates (x0, y0, z0) of the target sampling point:

[0025]

[0026] Where F is the total lateral offset of the robotic arm joint, which is the fixed lateral distance between the wrist center of the robotic arm and the origin of the robotic arm base, determined by the assembly relationship of the robotic arm. The extension direction of the robotic arm is also the orientation of the sampling device connected to the end effector, and this azimuth angle is also the orientation of the sampling device;

[0027] (3) In the global coordinate system, based on the azimuth angle ψ and the slope angle β determined in step S3, and the fitting plane normal vector (A,B,C), calculate the component of the slope angle along the azimuth direction of the sampling device:

[0028]

[0029] The significance of this angle is that after the sampling device rotates by this angle in the pitch direction, it is approximately parallel to the ground along the direction it is facing. This ensures that the sampling device does not come into contact with the terrain during sampling. Therefore, α calculated in this step is the pitch angle of the sampling device in the target pose.

[0030] (4) Determine the coordinates (x, y, z) of the robotic arm wrist center in the global coordinate system using the azimuth angle, pitch angle, and target sampling point coordinates:

[0031] H = R0 - h

[0032]

[0033] x=Lcosψ+Fsinψ

[0034] y=Lsinψ-Fcosψ

[0035]

[0036] Where K is the distance from the center of the robotic arm wrist to the rotation axis of the sampling device, and R0 is the rotation radius of the sampling device, determined by the assembly relationship of the robotic arm. The coordinates of the robotic arm wrist obtained through this step can be used to make the line connecting the center point of the rotation axis of the sampling device and the target sampling point perpendicular to the ground, thereby ensuring that the maximum sampling depth is located at the target sampling point as much as possible and meets the set sampling depth h.

[0037] (5) With the wrist center of the robotic arm as the origin of the coordinate system, the direction perpendicular to the rotation axis of the sampling device is taken as the X-axis, and the direction perpendicular to the X-axis downwards in the rotation plane of the sampling device is taken as the Y-axis. The Z-axis is determined by the right-hand rule to establish the sampling device coordinate system. Based on the rotation relationship between the sampling device coordinate system and the global coordinate system, the coordinate transformation matrix between the two coordinate systems, i.e., the target pose matrix, is determined using the azimuth angle, pitch angle, and wrist center coordinates.

[0038] The specific content of S5 includes:

[0039] (1) Based on the target pose matrix obtained in step S4, the accurate rotation angles of each joint of the robot arm are obtained by inverse solution of the pose matrix according to the sampled robot arm structure and constraint properties.

[0040] (2) The solved joint rotation angle is transmitted to the control motor of the robotic arm, so that the robotic arm moves and pulls the sampling device connected to the end to reach the vicinity of the target position;

[0041] (3) Obtain the current pose information of the sampling device by binocular vision or other measurement methods, compare it with the target pose, and calculate the pose deviation. Convert the deviation into the adjustment amount of the joint angle of the robotic arm, and transmit it to the robotic arm control motor for adjustment.

[0042] The specific content of S6 includes:

[0043] (1) Based on the coordinate transformation matrix of the current pose of the sampling device, transform the point cloud of the sampling region from the global coordinate system to the coordinate system of the sampling device, that is:

[0044]

[0045] Among them, P i s ampler For each point in the point cloud in the coordinate system of the sampling device, P i global For each point in the point cloud in the global coordinate system, This is the coordinate transformation matrix from the global coordinate system to the sampling device coordinate system, which is the inverse of the current pose matrix;

[0046] (2) In the coordinate system of the sampling device, the point cloud of the sampling area is projected onto the XOZ plane. The point closest to the origin (0,0) of the plane is searched to obtain the point cloud projection point of the robotic arm wrist center. The point closest to (K,0) is searched to obtain the point cloud projection point of the rotation axis center of the sampling device, which is the projection point of the actual sampling point. The actual sampling point in the three-dimensional point cloud is determined by the projection point on the XOZ plane, and the actual sampling depth at the sampling point is calculated based on its three-dimensional coordinates (x0,y0,z0) in the coordinate system of the sampling device.

[0047] D = R0 + y0

[0048] (3) In the coordinate system of the sampling device, based on the distance K from the wrist center to the rotation axis of the metering device, the equation of the straight line of the rotation axis of the metering device in the coordinate system of the sampling device is obtained:

[0049] x = K

[0050] Based on the effective acquisition width of the table-attachment device, an X-coordinate interval is defined. For all points in the local point cloud whose X-coordinates meet the interval requirements, the distance from their X-coordinates to the rotation axis of the table-attachment device is calculated.

[0051]

[0052] Points whose distance is less than the radius of the device are marked as collectable points, and their RGB values ​​are increased to give them a prominent visual effect in point cloud visualization. Among all collectable points, the maximum value of the x-coordinate is found. max With minimum value x min Therefore, the entry and exit angles of the sampling process of the sampling device are calculated:

[0053]

[0054]

[0055] (4) In the coordinate system of the sampling device, take the three-dimensional point cloud point corresponding to the XOZ plane projection point of the robotic arm wrist as the sphere center, and search for the local area point cloud according to the method of step S3.

[0056] (5) Establish a KD tree for the local area point cloud in the sampling device coordinate system. Transform the standard sampling device point cloud into the sampling device coordinate system according to the established coordinate system. Then, for each standard sampling device point cloud point, use the KD tree to search for the nearest neighbor point in the local area point cloud, thereby obtaining the minimum distance d from that point to the terrain. i After traversing all points, according to d... i The size of the point cloud is changed by altering the RGB values ​​of each point in the standard scan point cloud, so that the color of the point cloud changes smoothly between red, green, and blue according to the distance from the terrain from small to large.

[0057] (6) Visualize the 3D point cloud of the sampling area and the processed point cloud of the standard sampling device together on the computer software window. The operator evaluates the data based on the reference information provided above and decides whether further adjustments are needed. After confirming that everything is correct, the sampling command is sent to start sampling.

[0058] In the above process, the intrinsic parameters of the binocular camera refer to the intrinsic parameter matrices and distortion coefficients of the left and right cameras, respectively. The intrinsic parameter matrices include parameters such as the principal point coordinates of the camera, lens focal length, and detector pixel size. The extrinsic parameters of the binocular camera refer to the coordinate rotation and translation matrices between the left and right cameras. The intrinsic parameter matrices correspond to the transformation relationship from the camera coordinate system to the image coordinate system and can be uniquely determined through camera calibration. The distortion coefficients represent the degree of distortion of the camera lens and can be determined through camera calibration and used for image distortion correction. The camera extrinsic parameter coordinate transformation matrix represents the transformation relationship between the spatial point coordinates in the left and right camera coordinate systems and can be uniquely determined through camera calibration.

[0059] In the above process, to ensure the speed and quality of point cloud analysis, the 3D point cloud in the global coordinate system downsampling region reconstructed in step S2 can be downsampled and outlier removed. The downsampling process involves dividing the point cloud into voxel grids of a set size, ensuring complete coverage. For each voxel grid, the centroid of all point cloud points within it is taken as the representative point, replacing all points within the voxel. After replacing all voxels with representative points, a new downsampled point cloud is obtained. This method, at the cost of slight accuracy loss, significantly reduces the computational load, making it suitable for applications with high real-time requirements. The outlier removal process involves building a KD-tree for the point cloud. For each point in the point cloud, the KD-tree is used to search for its ten nearest neighbors. The average distance from this point to the ten nearest neighbors is calculated. If the average distance exceeds a set threshold, it is considered an outlier and removed. This method effectively removes abnormal values ​​caused by pixel matching failures during reconstruction, preventing interference.

[0060] In the above process, the target sampling point can be determined by the operator based on the 3D point cloud information of the sampling area in the global coordinate system reconstructed in step S2, by evaluating the region of interest and obstacle distribution. Alternatively, the target sampling point coordinates in the global coordinate system can be specified by other means, and the target sampling point can be determined by projecting it onto the reconstructed 3D point cloud through a search for the nearest neighbor.

[0061] Furthermore, in the above process, the sampling device can be towed by a robotic arm to reach all terrain areas where sampling is planned to be carried out, and these areas should be fully captured by the binocular camera.

[0062] Furthermore, the search radius of the adjacent spherical enclosed area should be set according to the actual size of the sampling device. Specifically, with the center of the robotic arm wrist as the center, the sphere determined by the search radius should completely enclose the entire sampling device with a certain margin. This ensures that the projection of the sampling device onto the terrain point cloud during calculations is always within the local area of ​​the point cloud determined by the search radius. Based on this, the sphere should be as small as possible to reduce the computational load.

[0063] Furthermore, in the above steps, the sampling device is a sampling component that can rotate around a fixed axis, and sampling is achieved by rotational motion on the sampling device.

[0064] Furthermore, the standard sampling device point cloud in the above steps is a complete scanned point cloud of the sampling device, or a standard point cloud obtained through a three-dimensional engineering model of the sampling device, which can accurately reflect the complete external structure of the sampling device.

[0065] This invention provides an apparatus for fine-tuning a table sampling robotic arm based on binocular stereo vision 3D reconstruction, which performs the aforementioned fine-tuning method for a table sampling robotic arm based on binocular stereo vision 3D reconstruction.

[0066] As described above, this invention discloses a method for fine-tuning a robotic arm for surface sampling based on binocular stereo vision 3D reconstruction. Through binocular stereo vision 3D reconstruction, the method utilizes the 3D spatial information contained in the binocular images to perform a detailed and accurate analysis of the sampling status of the sampling device. On one hand, this method can calculate the pose of the target sampling device based on the expected sampling target point, enabling the robotic arm to automatically and quickly guide the sampling device to the expected position and pose. On the other hand, this method can provide adjustment reference strategies and intuitively display key sampling reference information to the operator, thereby assisting in achieving rapid and accurate pose adjustment operations and improving the overall efficiency and sampling quality of the sampling process. Compared with existing technologies, this invention overcomes the problems of low efficiency in fine-tuning the sampling device in surface sampling tasks and the high difficulty of fine-tuning due to limited reference information. It allows technicians to easily and conveniently perform fine-tuning work on the robotic arm for surface sampling, greatly ensuring the smooth progress and completion of the sampling process. Attached Figure Description

[0067] Figure 1 This is a schematic diagram illustrating an example of the system composition involved in an embodiment of the present invention.

[0068] The system includes: 1. A binocular camera, fixed to the base of the robotic arm, which can capture the entire sampling area; 2. A multi-joint traction robotic arm; 3. A sampling device, connected to the end joint of the robotic arm; and 4. A sampling device, located at the front end of the sampling device, which can rotate around its axis to perform sampling.

[0069] Figure 2 This is a schematic diagram of an example of the coordinate system of the sampling device in an embodiment of the present invention.

[0070] Figure 3 This is a flowchart illustrating the process of determining the slope angle of the area adjacent to the target sampling point in an embodiment of the present invention.

[0071] Figure 4 This is a flowchart illustrating the determination of the target pose matrix in an embodiment of the present invention. Detailed Implementation

[0072] The embodiments of the present invention will now be described with reference to the accompanying drawings.

[0073] This invention discloses a method for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction. In this embodiment, the system composition is as follows: Figure 1 As shown, the device comprises a multi-joint sampling robotic arm (2) and a binocular camera (1) fixed to the base of the robotic arm. The robotic arm has four joint angles: shoulder yaw, shoulder pitch, elbow pitch, and wrist pitch. A sampling device (3) is connected to the wrist joint at its end. The sampling device has a digging-type sampling device (4) that can rotate around a fixed axis at its front end. The specific implementation steps are as follows:

[0074] S1. Control the robotic arm to pull the sampling device, move it outside the field of view of the binocular camera, and send a shooting command to the camera so that the binocular camera can take pictures of the complete and unobstructed sampling area;

[0075] S2. Using the calibration parameters of the binocular camera, the binocular images of the sampled area are corrected and aligned with the epipolar lines. After stereo matching, the point cloud of the sampled area is obtained by 3D reconstruction, and then it is transferred to the coordinate system of the robotic arm through the hand-eye calibration matrix.

[0076] S3. According to Figure 3 The process is as follows: First, a KD tree is built using the 3D point cloud of the sampling area. Then, the nearest neighbor of the target sampling point in the sampling area point cloud is searched. If the distance is less than R, it is added to the point set, and the search for the next nearest neighbor is performed. This process is repeated until the distance between the found nearest neighbor and the target sampling point is greater than R, thus determining the point set surrounding the target sampling point. Next, several points are randomly selected from the point set to determine the plane Ax + By + Cz + D = 0. The distance from all points in the point set to this plane is then iterated, and points with a distance less than a certain threshold are considered interior points. This process of randomly selecting points to determine the plane is repeated, and the results are updated when the plane has more interior points. After a certain number of iterations, a better fitting plane surrounding the point set is obtained, along with the fitting plane's normal vector (A, B, C) and slope angle β.

[0077]

[0078] S4. Reference Figure 2 Establish a coordinate system for the sampling device, with the wrist center of the robotic arm as the origin. The X-axis is the direction perpendicular to the rotation axis of the sampling device, and the Y-axis is the direction perpendicular to the X-axis downwards within the rotation plane of the sampling device. The Z-axis is determined by the right-hand rule. Figure 4 The process shown first determines the sampling depth h and the target sampling point coordinates (x0, y0, z0) in the robotic arm coordinate system based on actual sampling requirements. Then, the azimuth angle ψ is calculated based on the target sampling point coordinates (x0, y0, z0).

[0079]

[0080] Where F is the total lateral offset of the robotic arm joint, which is the fixed lateral distance between the wrist center of the robotic arm and the origin of the robotic arm base, determined by the assembly relationship of the robotic arm.

[0081] Then, based on the azimuth angle ψ and the slope angle β determined in step S3, and the fitting plane normal vector (A,B,C), the pitch angle α is calculated:

[0082]

[0083] Then, based on the table, determine the wrist center coordinates (x, y, z) in the robotic arm coordinate system using the depth h, azimuth angle ψ, and pitch angle α:

[0084] H = R0 - h

[0085]

[0086] x=L cosψ+F sinψ

[0087] y = L sinψ - F cosψ

[0088]

[0089] Where K is the distance from the wrist of the robotic arm to the rotation axis of the dial indicator, and R0 is the rotation radius of the dial indicator, which is determined by the assembly relationship of the robotic arm.

[0090] Finally, based on the coordinate system rotation relationship between the traction robotic arm and the sampling device connected at the wrist joint, the target pose matrix T of the sampling device is determined using the azimuth angle ψ, pitch angle α, and the wrist center coordinates (x, y, z).

[0091]

[0092]

[0093]

[0094] S5. Based on the size information of the robotic arm, the target pose matrix is ​​inversely solved into the four joint angles of the robotic arm, and the information is transmitted to the robotic arm motor so that it can pull the sampling device to the approximate target position and pose.

[0095] S6. Input camera shooting command to make the binocular camera capture images containing the sampling device within its field of view, use image features to match the visual feature points and geometric contours of the sampling device in the binocular image, and calculate the current pose information T′ of the sampling device based on the principle of binocular stereo vision.

[0096] S7. Using the current pose information and based on the constraint properties of the four-joint robotic arm, calculate the corresponding XYZ coordinates and pitch angle information (x′, y′, z′, α′) under the current pose T′:

[0097] x′=T′ 14 y′=T′ 24 z′=T′ 34

[0098]

[0099] R′=R z (-ψ′) -1 T′

[0100] α′=-arccos(R′ 11 )

[0101] Simultaneously, based on the target pose XYZ coordinates and pitch angle information (x,y,z,α) determined in step S4, the adjustment amount (Δx,Δy,Δz,Δα) is obtained by subtraction:

[0102] (Δx,Δy,Δz,Δa)=(x,y,z,α)-(x′,y′,z′,α′)

[0103] S8. Combine the adjustment amount (Δx, Δy, Δz, Δα) with the current joint angle of the robotic arm, and inversely solve to obtain the adjustment amount of the four joint angles. Then, input the adjustment amount into the robotic arm motor to make a small adjustment to the position and attitude of the sampling device.

[0104] S9. Repeat S6 to update the current pose information. Use the 3D point cloud and known standard sampling device to scan the point cloud. Analyze the accessible terrain area of ​​the sampling device under the current pose and the sampling depth, angle and other information. Calculate the distance of the sampling device from the terrain at each point and visualize it with color. The operator evaluates the above reference information and decides whether to continue to make adjustments.

[0105] S10. After personnel confirm that no further adjustments are needed, a sampling command is transmitted, and the sampling device is powered on to collect the sample.

[0106] In this embodiment, the robot arm coordinate system is taken as the global coordinate system.

[0107] In this embodiment, the known standard sampling device scan point cloud can be obtained by scanning the sampling device separately using a laser scanning system.

[0108] In this embodiment, the binocular camera, the robotic arm base, and the ground are fixedly connected, meaning that the robotic arm base and the binocular camera do not move relative to the ground throughout the entire process.

Claims

1. A fine adjustment method for a table sampling mechanical arm based on three-dimensional reconstruction of binocular stereo vision, characterized in that, Includes the following steps: S1. The binocular camera captures images of the complete sampling area, and the captured images are corrected based on the camera's intrinsic and extrinsic parameters determined by the binocular camera calibration. S2. Perform stereo matching on the two corrected images, estimate the pixel correspondence to obtain disparity information, then calculate the depth based on the camera extrinsic parameters, and map the two-dimensional image to the three-dimensional space of the global coordinate system to obtain the point cloud information of the sampling area; S3. Using the coordinates of the target sampling point as the center of the sphere, obtain a spherical enclosed region according to the set search radius, and use the points in the sampling region point cloud located within the enclosed region to fit the plane and obtain the slope angle of the local area near the target sampling point; S4. Based on the coordinates of the sampling points and the slope angle information of the local area, and according to the set sampling depth, determine the coordinates of the robotic arm wrist center and the pitch angle of the sampling device, thereby determining the precise target pose matrix of the sampling device; S5. The robotic arm pulls the sampling device to the target area according to the target pose matrix. By obtaining the actual pose of the current sampling device, the deviation between the current pose and the target pose is calculated and the robotic arm is controlled to make adjustments. S6. Based on the current pose of the sampler after adjustment, use the point cloud of the sampling area and the point cloud of the standard sampling device to analyze the accessible terrain area of ​​the sampling device and the sampling depth, angle and other information, calculate and visualize the distance of each part of the sampling device from the terrain, and provide intuitive information feedback to the operator, who will then confirm and decide whether to continue the adjustment.

2. The fine adjustment method of the table sampling mechanical arm based on binocular stereo vision three-dimensional reconstruction according to claim 1, characterized in that, The specific content of S3 includes: (1) Establish a KD search tree for the point cloud of the sampling area, traverse the distance from each point to the target sampling point, and retain the points whose distance is less than the set search radius R, that is, the set of points in the spherical enclosed area near the target sampling point; (2) Establish a plane mathematical model Ax+By+Cz+D=0, use the RANSAC method to find the plane parameters that maximize the number of points in the point set, and determine the fitting plane. (3) Get the fitting plane normal vector by plane model Thus, the angle between the normal vector and the Z axis of the global coordinate system is determined, i.e. the slope angle:

3. The fine adjustment method of a table sampling mechanical arm based on binocular stereo vision three-dimensional reconstruction according to claim 2, characterized in that, The specific content of S4 includes: (1) Set the sampling depth h according to the actual sampling requirements; (2) In the global coordinate system, the azimuth angle is calculated according to the target sampling point coordinates , and the angle between the arm span direction of the robot arm and the X axis of the global coordinate system, that is, the azimuth angle. Where F is the total lateral offset of the robotic arm joint, which is the fixed lateral distance between the wrist center of the robotic arm and the origin of the robotic arm base, determined by the assembly relationship. (3) In the global coordinate system, calculate the component of the slope angle along the azimuth direction of the sampling device, i.e., the pitch angle of the sampling device, based on the azimuth angle: (4) Determine the coordinates of the robot arm wrist center in the global coordinate system using the azimuth angle, pitch angle, and target sampling point coordinates. : Where K is the distance from the center of the robotic arm wrist to the rotation axis of the dial indicator. The radius of rotation of the component is determined by the assembly relationship; (5) With the wrist center of the robotic arm as the origin of the coordinate system, the direction perpendicular to the rotation axis of the sampling device is the X-axis, the direction perpendicular to the X-axis downward in the rotation plane of the sampling device is the Y-axis, and the Z-axis is established by the right-hand rule. The sampling device coordinate system is established, and the coordinate transformation matrix between the two coordinate systems, i.e. the target pose matrix, is determined by the azimuth angle, pitch angle and wrist center coordinates based on the rotation relationship between the sampling device coordinate system and the global coordinate system.

4. The method for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction according to claim 1, characterized in that, The specific content of S5 includes: (1) Based on the target pose matrix obtained in step S4, the accurate rotation angles of each joint of the robot arm are obtained by inverse solution of the pose matrix according to the sampled robot arm structure and constraint properties. (2) The solved joint rotation angle is transmitted to the control motor of the robotic arm, so that the robotic arm moves and pulls the sampling device connected to the end to reach the vicinity of the target position; (3) Obtain the current pose information of the sampling device by binocular vision or other measurement methods, compare it with the target pose, calculate the pose deviation, convert the deviation into the adjustment amount of the joint angle of the robotic arm, and transmit it to the robotic arm control motor for adjustment.

5. The method for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction according to claim 3, characterized in that, The specific content of S6 includes: (1) Based on the coordinate transformation matrix of the current pose of the sampling device The point cloud of the sampling region is converted from its coordinate system to its global coordinate system. Switch to the coordinate system of the sampling device ,Right now: (2) In the coordinate system of the sampling device, the point cloud of the sampling area is projected onto the XOZ plane. The point closest to the origin (0,0) of the plane is searched to obtain the point cloud projection point of the robotic arm wrist center. The point closest to (K,0) is searched to obtain the point cloud projection point of the rotation axis center of the sampling device, which is the projection point of the actual sampling point. The actual sampling point in the three-dimensional point cloud is determined by the projection point on the XOZ plane, and its three-dimensional coordinates in the coordinate system of the sampling device are used to determine the actual sampling point. Calculate the actual surface depth at the sampling point: (3) In the coordinate system of the sampling device, based on the distance K from the wrist center to the rotation axis of the metering device, the equation of the straight line of the rotation axis of the metering device in the coordinate system of the sampling device is obtained: Based on the effective acquisition width of the table-attachment device, an X-coordinate interval is defined. For all points in the local point cloud whose X-coordinates meet the interval requirements, the distance from their X-coordinates to the rotation axis of the table-attachment device is calculated. Points whose distance is less than the radius of the device to be sampled are marked as collectable points, and their RGB values ​​are increased to give them a prominent visual effect in point cloud visualization. Among all collectable points, find Maximum value of coordinates and minimum value Thus, the entry angle of the sampling process of the table-taking device is calculated. and the corner of the excavation : (4) In the coordinate system of the sampling device, take the three-dimensional point cloud point corresponding to the XOZ plane projection point of the robotic arm wrist as the sphere center, and search for the local area point cloud according to the method of step S3. (5) In the coordinate system of the sampling device, a KD tree is established for the local area point cloud. The standard sampling device point cloud is transformed into the sampling device coordinate system according to the establishment method of the sampling device coordinate system. Then, for each standard sampling device point cloud point, the nearest neighbor point in the local area point cloud is searched using the KD tree to obtain the minimum distance of the point from the terrain. After traversing all points, according to The size of the point cloud is changed by altering the RGB values ​​of each point in the standard scan point cloud, so that the color of the point cloud changes smoothly between red, green, and blue according to the distance from the terrain from small to large. (6) Visualize the three-dimensional point cloud of the sampling area and the processed point cloud of the standard sampling device together and display them on the computer software window. The operator evaluates the data based on the reference information provided by the above method, and after confirming that there are no errors, sends a sampling command to start sampling.

6. The method for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction according to claim 1, characterized in that, All terrain areas that can be reached by a robotic arm towing the sampling device and where sampling is planned should be fully captured by the binocular camera.

7. The method for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction according to claim 1, characterized in that, The search radius of the adjacent spherical enclosed area should be set according to the actual size of the sampling device. When the center of the robotic arm wrist is taken as the center of the sphere, the set radius can completely enclose the entire sampling device with a certain margin.

8. The method for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction according to claim 1, characterized in that, The sampling device is a sampling component that can rotate around a fixed axis. It achieves sampling by rotating on the sampling device.

9. The method for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction according to claim 1, characterized in that, The standard sampling device point cloud is a complete scanned point cloud of the sampling device, or a standard point cloud obtained through a three-dimensional engineering model of the sampling device, which can accurately reflect the complete external structure of the sampling device.

10. A device for fine-tuning a sampling robotic arm based on binocular stereo vision 3D reconstruction, characterized in that, The fine-tuning method for the sampling robotic arm based on binocular stereo vision 3D reconstruction according to any one of claims 1 to 9.

Images