A rock sampling pose calculation method for a field geological exploration robot

Abstract

The present application relates to the field of exploration survey, disclose a kind of rock sampling pose calculation method for field geological reconnaissance robot, comprising: the color image and depth image of target area are collected by depth camera, and three-dimensional point cloud is generated;Based on multi-scale geometric feature analysis, combined with spatial distribution constraint and confidence score, candidate points are automatically screened from point cloud, and stable sampling points and their pixel coordinates are output by time sequence smoothing and stability discrimination;Local depth data is extracted in the neighborhood of sampling point and converted into local point cloud, and the three-dimensional position and surface normal vector of the point in camera coordinate system are calculated using plane fitting algorithm;The position and normal vector are converted to the base coordinate system of mechanical arm using the pre-calibrated hand-eye conversion matrix, and the complete pose of the end effector is constructed based on the normal vector, and the target pose matrix is obtained by combination.The present application realizes the stable, accurate and automatic calculation of rock sampling point pose in complex outdoor environment.

Description

Technical Field

[0001] This invention relates to the field of exploration and surveying, and in particular to a method for calculating the rock sampling pose of a field geological reconnaissance robot. Background Technology

[0002] In field geological exploration, rock sampling is a crucial step in obtaining raw geological information and determining the properties and value of ore bodies. Its accuracy and efficiency directly impact the reliability of the exploration results. Traditional rock sampling methods rely primarily on manual labor. Workers must select target rocks in complex and ever-changing field environments and manually knock or cut them. This not only suffers from low efficiency and high labor intensity but also carries significant safety risks. Especially in rugged mountainous areas, steep slopes, or areas with accumulated gravel, manual access is often difficult and dangerous, and the sampling location is hard to control accurately, limiting the representativeness and validity of the obtained samples.

[0003] In recent years, exploration robots have gradually become a new type of tool to assist field operations, capable of replacing manual sampling tasks to a certain extent. However, existing robots still have significant shortcomings in calculating the pose of sampling points. On the one hand, traditional detection methods based on two-dimensional images cannot effectively acquire the spatial geometric information of the rock surface, resulting in insufficient positioning accuracy of sampling points and easy deviations during robotic arm execution. On the other hand, solutions relying solely on depth sensors are less stable under noise interference and camera shake, easily causing jumps in the position of sampling points and affecting the reliability of sampling actions. In addition, most systems can only provide the three-dimensional position of the sampling point, lacking accurate estimation of the local surface normal vector, making it difficult for the robotic arm to generate a sampling posture that matches the rock surface, resulting in a low success rate for tapping or cutting operations.

[0004] Therefore, there is an urgent need for a method for calculating the pose of rock sampling points in complex field environments. This method should be able to automatically and stably select sampling points and calculate their complete poses by combining point cloud geometric feature analysis and attitude estimation after completing the three-dimensional positioning of the rock. This would provide reliable pose input for exploration robots, improve the accuracy and robustness of rock sampling, and realize the intelligent and efficient operation of field geological exploration. Summary of the Invention

[0005] The purpose of this invention is to propose a method for calculating the rock sampling pose of a field geological reconnaissance robot, thereby solving the technical problem of low operational efficiency caused by inaccurate calculation of sampling point pose in complex environments of existing exploration robots.

[0006] Specifically, the present invention provides a method for calculating the rock sampling pose of a field geological reconnaissance robot, comprising the following steps: S1. Using a depth camera mounted on the robot, acquire color and depth images of the target rock area, and map the pixels into a three-dimensional point cloud based on the camera's intrinsic parameters. S2. Process the three-dimensional point cloud and automatically select a stable sampling point, including screening candidate points from the point cloud based on multi-scale geometric feature analysis, applying spatial distribution constraints and confidence scores to the candidate points to determine the initial sampling point, and then combining historical frame data to perform temporal smoothing and stability discrimination on the initial sampling point, and outputting the final stable sampling point and its pixel coordinates. S3. Extract local depth data in the neighborhood of the pixel coordinates of the stable sampling point, convert it into a local three-dimensional point cloud, and use a plane fitting algorithm to calculate the three-dimensional position of the sampling point in the camera coordinate system and the unit normal vector of the local surface where it is located. S4. Using a pre-calibrated hand-eye conversion extrinsic parameter matrix, the sampling point position and normal vector in the camera coordinate system are converted to the robot arm base coordinate system, and the complete posture of the robot arm end effector is constructed based on the normal vector. The combined posture matrix is ​​used to obtain the target pose matrix that can be used for robot arm motion control.

[0007] Furthermore, the specific steps for automatically selecting stable sampling points in step S2 include: S21. For each point in the 3D point cloud, calculate its covariance matrix in at least two neighborhoods of different scales and perform eigenvalue decomposition. Calculate the curvature index at each scale based on the eigenvalues ​​and obtain the comprehensive curvature index of the point through weighted fusion. S22. Sort all points according to the comprehensive curvature index, and select the top-ranked points to form a candidate point set; S23. Spatial distribution constraints and confidence scores are applied to the candidate point set, and the point with the highest confidence and closest to the center of the target rock area is selected as the initial sampling point at the current moment. S24. Combining the historical initial sampling point sequence of multiple consecutive frames, the weighted moving average method is used to calculate the smooth position, and it is determined whether the difference between its position and normal of the previous stable sampling point is less than a set threshold. If so, the stable sampling point is updated as the current smooth point; if not, the previous stable sampling point is retained until all three-dimensional point cloud data is traversed to obtain the final stable sampling point and its pixel coordinates.

[0008] Furthermore, in step S21, for any point in the 3D point cloud... p i In radius r Within the neighborhood of , the formula for calculating its comprehensive curvature index is:

[0009] in , , These are the eigenvalues ​​of the neighborhood point covariance matrix. It is the smallest eigenvalue of the neighborhood point covariance matrix.

[0010] Furthermore, in step S23, for candidate points p j confidence level S j The calculation formula is:

[0011] in, α and β For adjustable weighting coefficients, For this point p j The density of neighboring points, Let be the set of neighborhood point densities of all points in the candidate point set. The maximum neighborhood point density value in this set; For point The comprehensive curvature index, K is the set of comprehensive curvature indices of all points in the candidate point set. This is the maximum composite curvature index value in this set.

[0012] Furthermore, in step S24, when the weighted moving average method is used to calculate the smooth position, the weight used in this weighted moving average method is an exponentially decaying weight, with the most recent frame having the largest weight and the weight of historical frames decaying over time; the stability judgment conditions include both the position deviation threshold δ and the normal vector angle threshold θ.

[0013] Furthermore, step S3 specifically includes: S31. Using the pixel coordinates of the stable sampling point as the center, a region of interest is set, and the effective depth data in the region is extracted. After filtering and downsampling, the data is back-projected onto the camera coordinate system to form a local dense point cloud. S32. The local dense point cloud is fitted with a random sampling consensus algorithm to obtain the optimal plane equation, and the unit normal vector is extracted from it. n fit ; S33, the unit normal vector n fit The direction is uniformly adjusted to face forward towards the camera, resulting in the final stable normal vector. n ; S34. Based on the pixel coordinates and corresponding depth values ​​of the stable sampling points, and in conjunction with the camera intrinsic parameters, directly calculate their three-dimensional position in the camera coordinate system. P .

[0014] Furthermore, step S4 specifically involves: using the extrinsic parameter matrix obtained through hand-eye calibration. T cb Position in the camera coordinate system P and stable normal vector n Transform to the robot arm's base coordinate system, and the calculation formula is as follows:

[0015] in, R cb and t cb These are the extrinsic parameter matrices. T cb The rotation matrix and translation vector in the equation.

[0016] Furthermore, in step S4, the method for constructing the attitude of the robotic arm end effector is as follows: S41, the normal vector n in the base coordinate system of the robotic arm base The Z-axis direction serves as the coordinate system of the robotic arm's end effector. S42. Select a fixed reference vector. v ref and n base Perform a cross product to obtain the normalized x-axis direction; S43. According to the right-hand rule, by n base The cross product of the x-axis direction and the x-axis direction yields the y-axis direction; S44, by x , y , n base The attitude rotation matrix of the end effector is composed of three orthogonal basis vectors. R tool ; S45. Finally, the target pose matrix. T target Depend on R tool and P base Combination and composition.

[0017] A storage medium storing instructions and data for implementing a method for calculating the pose of rock sampling for a field geological survey robot.

[0018] A rock sampling pose calculation device for a field geological survey robot includes: a processor and a storage medium; the processor loads and executes instructions and data in the storage medium to implement a rock sampling pose calculation method for a field geological survey robot.

[0019] The beneficial effects provided by this invention are: 1. Improve the stability and representativeness of sampling point selection: Through multi-scale curvature analysis, spatial distribution constraints and temporal smoothing mechanisms, sampling points with significant geometric features and stable positions can be automatically screened from noisy point clouds, avoiding selection deviations caused by environmental interference or flat surfaces, and ensuring that the sampling positions are representative and operable.

[0020] 2. Enhanced anti-interference capability and accuracy of pose calculation: The position and normal vector are calculated by using a local point cloud fitting method, replacing the traditional single-point depth dependence. This effectively suppresses the influence of depth noise, illumination changes and surface reflection, and provides a more robust and accurate surface normal estimation, providing reliable guidance for the robotic arm to make vertical contact with the rock.

[0021] 3. Achieve high-precision unification of perception and operation space: Through pre-calibration of hand and eye, the pose in the camera coordinate system is accurately converted to the robot arm base coordinate system, and the complete end-effector pose is constructed by combining the normal vector, which ensures that the robot arm can reach the target point with the correct pose, and greatly reduces the execution error caused by coordinate system mismatch.

[0022] 4. Improve the success rate and efficiency of automated field sampling operations: The entire method operates fully automatically, reducing reliance on human experience. It can continuously output stable target poses in complex and turbulent field environments, thereby significantly improving the success rate and operational efficiency of robots performing sampling actions such as tapping and cutting, and promoting the development of geological exploration towards intelligence and unmanned operation. Attached Figure Description

[0023] Figure 1 This is a schematic diagram of the method flow of the present invention; Figure 2 This is a schematic diagram of the hardware device operation according to an embodiment of the present invention. Detailed Implementation

[0024] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be further described below with reference to the accompanying drawings.

[0025] Before formally describing the present invention, a general description of the solution of the present invention will be given first to facilitate understanding.

[0026] Example 1 Please refer to Figure 1 The present invention provides a method for calculating the rock sampling pose of a field geological reconnaissance robot, comprising the following steps: S1. Using a depth camera mounted on the robot, acquire color and depth images of the target rock area, and map the pixels into a three-dimensional point cloud based on the camera's intrinsic parameters. It should be noted that the exploration robot in this invention is pre-equipped with a depth camera. The depth camera integrates a depth sensor and an RGB color camera, capable of simultaneously outputting depth and color images. The specific selection of the depth camera is not the focus of this application; active depth cameras are preferred, followed by hybrid or passive cameras. This invention is used for illustrative purposes only and should not be used to limit the scope of the invention.

[0027] After acquiring the corresponding images, this invention maps the pixels within the target area into a 3D point cloud based on the intrinsic parameters of the depth camera, thus obtaining a point set. P ={ p 1, p 2, , p n For any point p i Neighborhood point sets are constructed at different neighborhood scales (e.g., radii r1, r2, r3). N i (r) And calculate the covariance matrix. C i (r) The calculation formula is as follows:

[0028] in, The number of neighboring points. The three-dimensional geometric centroid of a neighborhood point set is defined as:

[0029] The covariance matrix describes the point p i The geometric distribution characteristics of points within the neighborhood, through the analysis of... Eigenvalue decomposition yields three real eigenvalues. , , , which correspond to the degree of dispersion of the point cloud in the three main directions, respectively.

[0030] S2. Process the three-dimensional point cloud and automatically select a stable sampling point, including screening candidate points from the point cloud based on multi-scale geometric feature analysis, applying spatial distribution constraints and confidence scores to the candidate points to determine the initial sampling point, and then combining historical frame data to perform temporal smoothing and stability discrimination on the initial sampling point, and outputting the final stable sampling point and its pixel coordinates. It should be noted that there are two modes for selecting sampling points: manual and automatic.

[0031] Manual selection involves operators clicking directly on the target area in the camera's color image to specify the sampling location of interest. While this method is intuitive, it relies on human experience in complex environments, making it difficult to guarantee the consistency and efficiency of sampling point selection.

[0032] To address this, the present invention proposes an automatic selection method based on point cloud geometric features and temporal stabilization, which can autonomously determine the sampling point location without human intervention.

[0033] In automatic selection mode, this invention not only adopts the traditional candidate point screening method based on local curvature, but also further introduces multi-scale geometric analysis, spatial distribution constraints and temporal stabilization enhancement mechanisms to improve the robustness and accuracy of point selection.

[0034] Specifically, the steps for automatically selecting stable sampling points in step S2 include: S21. For each point in the 3D point cloud, calculate its covariance matrix in at least two neighborhoods of different scales and perform eigenvalue decomposition. Calculate the curvature index at each scale based on the eigenvalues ​​and obtain the comprehensive curvature index of the point through weighted fusion. For any point in a 3D point cloud p i In radius r Within the neighborhood of , the formula for calculating its comprehensive curvature index is:

[0035] in , , These are the eigenvalues ​​of the neighborhood point covariance matrix. Let be the minimum eigenvalue of the neighborhood point covariance matrix, where the minimum eigenvalue characterizes the variation of the local surface normal direction.

[0036] To balance details with overall geometric trends, this invention employs a weighted fusion strategy:

[0037] Among them, curvatures with smaller radii are more sensitive to local protrusions, while curvatures with larger radii are more stable. Therefore, the weights can be set based on experimental experience or dynamic adjustment mechanisms. For example, increasing the small-scale curvature weight in rough areas of a rock surface and increasing the large-scale curvature weight in smooth areas.

[0038] S22. Sort all points according to the comprehensive curvature index, and select the top-ranked points to form a candidate point set; It should be noted that this invention sorts the comprehensive curvature indices of all points and selects the top percentages (e.g., 10% to 15%) of points as the candidate set.P top .

[0039] S23. Spatial distribution constraints and confidence scores are applied to the candidate point set, and the point with the highest confidence and closest to the center of the target rock area is selected as the initial sampling point at the current moment. It should be noted that, in the candidate set, the present invention further introduces spatial distribution constraints and confidence evaluation: on the one hand, by setting a minimum point spacing limit, the selection deviation caused by excessive concentration of candidate points is avoided; on the other hand, based on the consistency of local density and curvature of the point cloud, a confidence score is calculated for the candidate points.

[0040] In step S23, for candidate points p j confidence level S j The calculation formula is:

[0041] in, α and β For adjustable weighting coefficients, For this point p j The density of neighboring points, K j For point p j The comprehensive curvature index, Let be the set vector comprising the density of the neighborhood points of all points in the candidate point set. This is a set vector comprising the comprehensive curvature indices of all points in the candidate point set. Finally, the point with the highest confidence level and closest to the geometric center of the target region is selected as the initial sampling point P. t .

[0042] S24. Combining the historical initial sampling point sequence of multiple consecutive frames, the weighted moving average method is used to calculate the smooth position, and it is determined whether the difference between its position and normal of the previous stable sampling point is less than a set threshold. If so, the stable sampling point is updated as the current smooth point; if not, the previous stable sampling point is retained until all three-dimensional point cloud data is traversed to obtain the final stable sampling point and its pixel coordinates.

[0043] It should be noted that, in order to ensure the temporal stability of the sampling points, this invention designs an enhanced historical point constraint mechanism. Specifically, when calculating the smoothed position using the weighted moving average method in step S24, the weights used in this weighted moving average method are exponentially decaying weights, with the most recent frame having the largest weight, and the weights of historical frames decaying over time; the stability judgment condition also includes a position deviation threshold. δ Threshold of the angle between the normal vector and the normal vector θ .

[0044] As one embodiment, the sampling points that record consecutive moments are a sequence. H t ={ p t-N+1 ， ，p t The smoothing point is obtained by using a weighted moving average:

[0045] in, This represents the total window length of consecutive historical observation frames used for time-series smoothing calculations; This represents the three-dimensional spatial coordinates of the initial sampling point independently extracted by the system at the (t-i+1)th historical moment in the timeline. This is the exponential decay coefficient, used to control the decay rate of the weight over time; The weights are exponentially decaying to ensure that the latest points contribute more to the results, while the contributions of older points gradually decrease.

[0046] In determining location stability, this invention not only compares the Euclidean distance between smooth points and historical stable sampling points, but also incorporates a normal consistency constraint. Stable sampling points are only updated when the following conditions are met:

[0047] in, This represents the three-dimensional spatial coordinates of the previous stable sampling point, which has been confirmed and locked by the algorithm at the previous moment during the system's historical iterations. This represents the local unit normal vector in three-dimensional space corresponding to the surface of the previous stable sampling point. Fit a normal vector to the current frame. and These are the spatial location threshold and the normal angle threshold, respectively. If both conditions are met, the sampling points are considered to have converged, and the stable sampling points are updated. .

[0048] The sampling points selected by the method described above are typically located in areas with significant geometric features and prominent curvature on the rock surface. These points reflect the true surface morphology of the rock and avoid falling into flat or noisy areas. Simultaneously, the spatial distribution of candidate points is constrained and optimized to ensure that the selected locations are near the center of the target area, possessing representativeness and operability. Furthermore, by combining temporal smoothing and normal consistency judgment, the influence of camera shake and sensor noise is effectively eliminated, ensuring that the final determined sampling points remain stable in both spatial position and normal direction. Therefore, the sampling points selected by this invention not only facilitate accurate alignment and contact of the robotic arm's end effector but also improve the success rate and repeatability of striking or cutting operations, meeting the accuracy and robustness requirements for automatic rock sampling in complex field environments.

[0049] S3. Extract local depth data in the neighborhood of the pixel coordinates of the stable sampling point, convert it into a local three-dimensional point cloud, and use a plane fitting algorithm to calculate the three-dimensional position of the sampling point in the camera coordinate system and the unit normal vector of the local surface where it is located. It should be noted that after the sampling point is selected, its pixel coordinates are obtained. (u,v) Extract the effective depth value z within its local neighborhood. c The three-dimensional coordinates of the sampling point in the camera coordinate system are calculated according to the following formula. P=(x,y,z) T .

[0050]

[0051] in K This is the camera intrinsic parameter matrix.

[0052] However, depth sensors in the field are often affected by various factors such as strong light, reflection from rock surfaces, vegetation obstruction, and dust interference, leading to fluctuations and instability in single-point depth measurements. If only single-point data is used as the pose input for the robotic arm, it is very easy to cause positioning errors, resulting in the end effector deviating from the target position and reducing the sampling success rate.

[0053] To address this issue, this invention proposes a pose calculation method based on local point cloud fitting, which can provide more stable and accurate sampling point poses in complex environments.

[0054] Specifically, step S3 includes: S31. Using the pixel coordinates of the stable sampling point as the center, a region of interest is set, and the effective depth data in the region is extracted. After filtering and downsampling, the data is back-projected onto the camera coordinate system to form a local dense point cloud. As one embodiment, the present invention firstly locates the depth map position corresponding to the sampling point. (u,v)A Region of Interest (ROI) is constructed around the sampling point to extract depth information from its neighborhood. By default, the ROI size is set to 150×150 pixels to ensure sufficient point cloud data within the neighborhood. However, in scenarios with sparse depth or significant surface undulations, the ROI size can be dynamically adjusted to ensure sufficient point cloud data while covering as many local surface features as possible of the sampling point. Before being converted into a 3D point cloud, the extracted depth data undergoes statistical filtering and voxel downsampling to remove outliers, suppress noise interference, and reduce computational complexity.

[0055] Within the ROI, the camera back-projection function is used to map each effective depth point. （u i ，v i ，z i ） Transform to camera coordinate system:

[0056] in, K For the camera intrinsic parameter matrix, z i This represents the depth value at that point. This allows us to obtain the locally dense point cloud set { P i} is used for fitting analysis.

[0057] S32. The local dense point cloud is fitted with a random sampling consensus algorithm to obtain the optimal plane equation, and the unit normal vector is extracted from it. n fit ; To fit the geometry of the sampled point surface, the RANSAC algorithm is used to perform planar fitting on the local point cloud. This algorithm iterates multiple times, randomly selecting a small number of points from the point cloud to generate a planar model, and continuously statistically analyzing points that meet a distance threshold. Number of interior points:

[0058] in, (a, b, c, d) This represents the parameters of the plane equation. After the number of iterations reaches a set value, the plane model with the most interior points is selected as the optimal solution. To further improve accuracy, after obtaining the initial model, least-squares optimization is performed using all interior points to obtain more accurate plane parameters. Finally, the unit normal vector of the plane can be expressed as: .

[0059] S33, the unit normal vector n fitThe direction is uniformly adjusted to face forward towards the camera, resulting in the final stable normal vector. n ; It should be noted that, during the determination of the normal vector, to avoid control ambiguity caused by inconsistent normal vector directions, the stable normal vector direction must be ensured to point towards the camera:

[0060] in Z cam = (0,0,1) T This indicates the forward direction in the camera coordinate system.

[0061] Through the above steps, the three-dimensional position of the sampling point in the camera coordinate system is finally obtained. P=(x,y,z) T and its corresponding stable normal vector The location reflects the accurate coordinates of the sampling point in three-dimensional space, while the stable normal vector... This describes the spatial orientation of the local surface where the point is located. The combination of these two not only provides a reliable motion target for the end effector of the robotic arm, but also ensures that the sampling tool can contact the rock in a direction perpendicular to the surface, effectively avoiding problems such as slippage, deviation, and uneven force during the sampling process.

[0062] S34. Based on the pixel coordinates and corresponding depth values ​​of the stable sampling points, and in conjunction with the camera intrinsic parameters, directly calculate their three-dimensional position in the camera coordinate system. P .

[0063] Due to the location of the sampling point P The stable normal vector is calculated in the camera coordinate system. However, in actual sampling operations, the camera and the robotic arm are not located in the same coordinate system, and there is a fixed installation deviation between them in space. If the pose information in the camera coordinate system is directly used as the control input of the robotic arm, the motion trajectory of the end effector will be inconsistent with the target sampling point, resulting in significant position and attitude deviations. Therefore, the pose results in the camera coordinate system must be transformed into the robotic arm's base coordinate system to achieve a unification of the perception space and the operation space.

[0064] Therefore, hand-eye calibration was performed in advance during the deployment phase of the reconnaissance robot system. By placing calibration boards within the camera's field of view and collecting multiple sets of camera observation data and robotic arm end-effector pose data, the extrinsic parameter matrix from the camera coordinate system to the robotic arm's base coordinate system was calculated using the hand-eye calibration method.

[0065] in, It is a 3×3 rotation matrix that describes the spatial rotation relationship between the camera coordinate system and the robot arm base coordinate system; The extrinsic parameter matrix is ​​a 3×1 translation vector, reflecting the spatial translation deviation between the two coordinate systems. Once calibrated, the extrinsic parameter matrix remains fixed throughout the sampling task, thus achieving stable and reliable coordinate transformation.

[0066] S4. Using a pre-calibrated hand-eye conversion extrinsic parameter matrix, the sampling point position and normal vector in the camera coordinate system are converted to the robot arm base coordinate system, and the complete posture of the robot arm end effector is constructed based on the normal vector. The combined posture matrix is ​​used to obtain the target pose matrix that can be used for robot arm motion control.

[0067] Therefore, step S4 specifically involves using the extrinsic parameter matrix obtained through hand-eye calibration. T cb Position in the camera coordinate system P and stable normal vector n Transform to the robot arm's base coordinate system, and the calculation formula is as follows:

[0068] in, R cb and t cb These are the extrinsic parameter matrices. T cb The rotation matrix and translation vector in the equation.

[0069] Similarly, the stable normal vector of the sampling point in the robot arm base coordinate system, after transformation by the rotation matrix, is expressed as:

[0070] It should be noted that the method for constructing the attitude of the robotic arm end effector in step S4 is as follows: S41, the normal vector n in the base coordinate system of the robotic arm base The Z-axis direction serves as the coordinate system of the robotic arm's end effector. Specifically, in the robotic arm coordinate system, the Z-axis of the end effector sampling tool is aligned with the normal vector. This involves vertically striking the rock surface along the normal direction to obtain a sample.

[0071] S42. Select a fixed reference vector. v ref and n base Perform a cross product to obtain the normalized x-axis direction; Specifically, to construct a complete robotic arm end effector coordinate system, a fixed reference vector is selected. v ref =(1,0,0) T And the cross product of the vector and the normal vector gives the end tool. x Axial direction: .

[0072] S43. According to the right-hand rule, by n base The cross product of the x-axis direction and the x-axis direction yields the y-axis direction; Specifically, the y-axis is obtained according to the orthogonal right-hand rule: .

[0073] S44, by x , y , n base The attitude rotation matrix of the end effector is composed of three orthogonal basis vectors. R tool ; Specifically, the complete rotation matrix of the robotic arm's end effector is obtained: .

[0074] S45. Finally, the target pose matrix. T target Depend on R tool and P base Combination and composition.

[0075] Specifically, the combination position P base with posture R tool Construct the target pose matrix at the end effector of the robotic arm:

[0076] The target pose matrix can be directly input into the robotic arm motion control system, enabling the end effector to reach the target sampling point in the correct position and orientation, and to perform hammering or cutting sampling operations along the normal direction of the rock surface.

[0077] Example 2: Please see Figure 2 , Figure 2 This is a schematic diagram of the hardware device in operation according to an embodiment of the present invention. The hardware device specifically includes: a rock sampling pose calculation device 401 for a field geological survey robot, a processor 402, and a storage medium 403.

[0078] A rock sampling pose calculation device 401 for a field geological survey robot: The rock sampling pose calculation device 401 for a field geological survey robot implements the rock sampling pose calculation method for a field geological survey robot.

[0079] Processor 402: The processor 402 loads and executes the instructions and data in the storage medium 403 to implement the rock sampling pose calculation method for a field geological survey robot.

[0080] Storage medium 403: The storage medium 403 stores instructions and data; the storage medium 403 is used to implement the rock sampling pose calculation method for a field geological survey robot.

[0081] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for calculating the pose of rock sampling for a field geological reconnaissance robot, characterized in that: Includes the following steps: S1. Using a depth camera mounted on the robot, acquire color and depth images of the target rock area, and map the pixels into a three-dimensional point cloud based on the camera's intrinsic parameters. S2. Process the three-dimensional point cloud and automatically select a stable sampling point, including screening candidate points from the point cloud based on multi-scale geometric feature analysis, applying spatial distribution constraints and confidence scores to the candidate points to determine the initial sampling point, and then combining historical frame data to perform temporal smoothing and stability discrimination on the initial sampling point, and outputting the final stable sampling point and its pixel coordinates. S3. Extract local depth data in the neighborhood of the pixel coordinates of the stable sampling point, convert it into a local three-dimensional point cloud, and use a plane fitting algorithm to calculate the three-dimensional position of the sampling point in the camera coordinate system and the unit normal vector of the local surface where it is located. S4. Using a pre-calibrated hand-eye conversion extrinsic parameter matrix, the sampling point position and normal vector in the camera coordinate system are converted to the robot arm base coordinate system, and the complete posture of the robot arm end effector is constructed based on the normal vector. The combined posture matrix is ​​used to obtain the target pose matrix that can be used for robot arm motion control.

2. The method for calculating the rock sampling pose of a field geological reconnaissance robot as described in claim 1, characterized in that: The specific steps for automatically selecting stable sampling points in step S2 include: S21. For each point in the 3D point cloud, calculate its covariance matrix in at least two neighborhoods of different scales and perform eigenvalue decomposition. Calculate the curvature index at each scale based on the eigenvalues ​​and obtain the comprehensive curvature index of the point through weighted fusion. S22. Sort all points according to the comprehensive curvature index, and select the top-ranked points to form a candidate point set; S23. Spatial distribution constraints and confidence scores are applied to the candidate point set, and the point with the highest confidence and closest to the center of the target rock area is selected as the initial sampling point at the current moment. S24. Combining the historical initial sampling point sequence of multiple consecutive frames, the weighted moving average method is used to calculate the smooth position, and it is determined whether the difference between its position and normal of the previous stable sampling point is less than a set threshold. If so, the stable sampling point is updated as the current smooth point; if not, the previous stable sampling point is retained until all three-dimensional point cloud data is traversed to obtain the final stable sampling point and its pixel coordinates.

3. The method for calculating the rock sampling pose of a field geological reconnaissance robot as described in claim 2, characterized in that: In step S21, for any point in the 3D point cloud... p i In radius r Within the neighborhood of , the formula for calculating its comprehensive curvature index is: in , , These are the eigenvalues ​​of the neighborhood point covariance matrix. It is the smallest eigenvalue of the neighborhood point covariance matrix.

4. The method for calculating the rock sampling pose of a field geological reconnaissance robot as described in claim 3, characterized in that: In step S23, for candidate points p j confidence level S j The calculation formula is: in, α and β For adjustable weighting coefficients, For this point p j The density of neighboring points, Let be the set of neighborhood point densities of all points in the candidate point set. The maximum neighborhood point density value in this set; For point The comprehensive curvature index, K is the set of comprehensive curvature indices of all points in the candidate point set. This is the maximum composite curvature index value in this set.

5. The method for calculating the rock sampling pose of a field geological reconnaissance robot as described in claim 2, characterized in that: When calculating the smooth position using the weighted moving average method in step S24, the weights used in this weighted moving average method are exponentially decaying weights, with the most recent frame having the largest weight and the weights of historical frames decaying over time; the stability judgment conditions include both the position deviation threshold δ and the normal vector angle threshold θ.

6. The method for calculating the rock sampling pose of a field geological reconnaissance robot as described in claim 1, characterized in that: Step S3 specifically includes: S31. Using the pixel coordinates of the stable sampling point as the center, a region of interest is set, and the effective depth data in the region is extracted. After filtering and downsampling, the data is back-projected onto the camera coordinate system to form a local dense point cloud. S32. The local dense point cloud is fitted with a random sampling consensus algorithm to obtain the optimal plane equation, and the unit normal vector is extracted from it. n fit ; S33, the unit normal vector n fit The direction is uniformly adjusted to face forward towards the camera, resulting in the final stable normal vector. n ; S34. Based on the pixel coordinates and corresponding depth values ​​of the stable sampling points, and in conjunction with the camera intrinsic parameters, directly calculate their three-dimensional position in the camera coordinate system. P .

7. The method for calculating the rock sampling pose of a field geological reconnaissance robot as described in claim 6, characterized in that, Step S4 specifically involves using the extrinsic parameter matrix obtained through hand-eye calibration. T cb Position in the camera coordinate system P and stable normal vector n Transform to the robot arm's base coordinate system, and the calculation formula is as follows: in, R cb and t cb These are the extrinsic parameter matrices. T cb The rotation matrix and translation vector in the equation.

8. The method for calculating the rock sampling pose of a field geological reconnaissance robot as described in claim 7, characterized in that, In step S4, the method for constructing the attitude of the robotic arm's end effector is as follows: S41, the normal vector n in the base coordinate system of the robotic arm base The Z-axis direction serves as the coordinate system of the robotic arm's end effector. S42. Select a fixed reference vector. v ref and n base Perform a cross product to obtain the normalized x-axis direction; S43. According to the right-hand rule, by n base The cross product of the x-axis direction and the x-axis direction yields the y-axis direction; S44, by x , y , n base The attitude rotation matrix of the end effector is composed of three orthogonal basis vectors. R tool ; S45. Finally, the target pose matrix. T target Depend on R tool and P base Combination and composition.

9. A storage medium, characterized in that: The storage medium stores instructions and data to implement the rock sampling pose calculation method for a field geological reconnaissance robot as described in any one of claims 1 to 8.

10. A rock sampling pose calculation device for a field geological reconnaissance robot, characterized in that: include: A processor and a storage medium; the processor loads and executes instructions and data in the storage medium to implement the rock sampling pose calculation method for a field geological reconnaissance robot as described in any one of claims 1 to 8.

Images