A real-time positioning method and system for robotic arm operation

Abstract

The present application belongs to the technical field of image processing, and particularly relates to a real-time positioning method and system for mechanical arm operation, which comprises the following steps: calculating an initial matching cost volume of a stereo image pair; generating an adaptive aggregation path field according to a local structure tensor of the image, and determining an adaptive penalty term based on a structure strength; performing cost aggregation along the adaptive path field by using the adaptive penalty term to obtain an aggregated cost volume; determining a disparity map according to the aggregated cost volume and solving a three-dimensional pose of a target point to be drilled. The present application generates an aggregation path that conforms to the scene geometry and dynamically adjusts the smoothing constraint, thereby significantly improving the accuracy and robustness of three-dimensional positioning in a complex environment and providing reliable real-time pose guidance for high-precision operation of the mechanical arm.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of image processing. More particularly, the present application relates to a real-time positioning method and system for mechanical arm operation. BACKGROUND

[0002] In modern tunnel engineering, stable and reliable wireless communication is a core element to ensure construction safety and operational efficiency. As an effective signal coverage solution, communication leaky cable is widely used in highway, railway and subway tunnels. Traditional leaky cable installation operation mainly relies on manual point fixing, drilling and bracket fixing. This method not only has high labor intensity and low operation efficiency, but also has high safety risks in complex tunnel environments. Therefore, using intelligent construction mechanical arm for automatic drilling operation has become an important development direction to improve the intelligent level of tunnel construction.

[0003] To realize the automation of drilling operation, the intelligent construction mechanical arm must be equipped with a high-precision three-dimensional visual perception system to obtain the geometric information of the tunnel wall surface in real time. The core task of this system is to accurately calculate the spatial three-dimensional coordinates of the target point to be drilled and the surface normal vector of the position where it is located, so as to guide the drill bit at the end of the mechanical arm to drill vertically to the tunnel wall surface with the correct attitude, ensuring the stability and standardization of the cable bracket installation. At present, the non-contact three-dimensional measurement technology based on binocular stereo vision has become the preferred technical solution for such guidance systems, because it can quickly obtain dense depth information of the scene.

[0004] However, the tunnel wall is usually a non-cooperative complex curved surface formed by spraying concrete, with uneven surface and variable curvature. Traditional stereo matching algorithms (such as semi-global block matching SGBM) mostly use fixed linear paths for cost aggregation. These paths cannot effectively fit the actual geometric profile of the tunnel wall. When the aggregation path passes through the curved surface or the depth discontinuous area, it will produce serious disparity mismatch, resulting in a serious decrease in the accuracy of the three-dimensional reconstruction model near the key drilling point. At the same time, the texture distribution on the tunnel wall surface is uneven. In the traditional stereo matching algorithm, a fixed smoothing penalty term is used, which may cause insufficient smoothing in the texture-rich area and fail to effectively suppress noise, and may over-smooth in the texture-simple area and lose necessary structural details, affecting the accuracy of the drilling point surface normal vector calculation, and making it difficult to meet the requirements of high-precision drilling of the mechanical arm. SUMMARY

[0005] To solve the technical problem that the existing stereo matching algorithm has low positioning accuracy on complex curved surfaces due to fixed aggregation paths and penalty terms, and cannot meet the requirements of high-precision operation of the mechanical arm, the present application provides solutions in the following aspects.

[0006] In a first aspect, the present application provides a real-time positioning method for mechanical arm operation, comprising:

[0007] An image pair to be processed is acquired, and an initial matching cost volume is calculated, the initial matching cost volume being composed of matching costs of pixel points in the image pair under different disparities; local structure tensors of each pixel point are calculated according to one image in the image pair, and an adaptive aggregation path field covering the one image is generated according to the local structure tensors; structure strengths of each pixel point are determined according to the local structure tensors of each pixel point, and adaptive penalty terms for cost aggregation are determined based on the structure strengths; the initial matching cost volume is aggregated along each aggregation path in the adaptive aggregation path field by using the adaptive penalty terms, so as to obtain an aggregated cost volume; a disparity map is determined according to the aggregated cost volume, and a three-dimensional pose of a target point to be drilled is determined according to the disparity map, so as to guide a mechanical arm drill bit to perform vertical drilling operation.

[0008] The present application can accurately identify the trend of the target object surface contour in the image by analyzing the local structure tensors of the image, the adaptive aggregation path generated based on this information can closely fit the real geometric structure of the scene, and the cost aggregation process strictly obeys the depth continuity assumption, so that the matching error problem caused by the traditional fixed path crossing the depth boundary is fundamentally solved, and the positioning accuracy at irregular curved surfaces is significantly improved.

[0009] Preferably, the matching cost acquisition method is as follows: the image pair is subjected to distortion correction and stereoscopic correction; for each pixel point in one of the corrected images, the matching cost of the pixel point with a corresponding pixel point in the other corrected image is calculated within a preset disparity range, and the matching cost is composed of a Census transformation cost and a SAD cost weighted.

[0010] The present application can effectively resist the interference of complex light in industrial field by weightedly fusing the Census transformation cost which is robust to light change and the SAD cost which is sensitive to gray scale details, while retaining the fine texture information of the operation surface, so as to provide reliable original data for subsequent cost aggregation.

[0011] Preferably, the local structure tensors of each pixel point are calculated according to one image in the image pair, comprising: calculating the horizontal gradient and the vertical gradient of each pixel point in the one image; for each pixel point, the local structure tensor thereof is constructed according to the horizontal gradient and the vertical gradient of each pixel point in the neighborhood window thereof.

[0012] Preferably, generating the adaptive aggregated path field covering the image according to the local structure tensor comprises: performing eigenvalue decomposition on the local structure tensor of each pixel point to obtain two eigenvalues and corresponding eigenvectors; taking the eigenvector corresponding to the minimum eigenvalue among the two eigenvalues as the local optimal structure direction of the pixel point; and extending from the boundary pixel points of the image to the interior of the image according to the local optimal structure direction of each pixel point to generate a set of aggregated paths, thereby constituting the adaptive aggregated path field.

[0013] The local structure tensor is subjected to eigenvalue decomposition, the direction with the most gentle gradient change is identified as the local optimal structure direction, and the generated aggregated path field is guided by the direction, so that the aggregated path field can extend along the structure edges such as the contour lines of objects in the image, the cost aggregation is ensured to be performed in the pixel neighborhood with similar depth values, the error smoothing across different depth planes is avoided, and the definition and accuracy of the disparity map at the edges of objects are ensured.

[0014] Preferably, the structure strength of each pixel point is determined according to the local structure tensor of the pixel point, comprising: performing eigenvalue decomposition on the local structure tensor of each pixel point, and taking the maximum eigenvalue among the two eigenvalues obtained by the decomposition as the structure strength of the pixel point.

[0015] Preferably, the adaptive penalty term for cost aggregation is determined based on the structure strength, comprising: determining the structure strength weight of each pixel point according to the structure strength of the pixel point. , wherein, is the structure strength weight of the pixel point . is the structure strength of the pixel point . is a coefficient for controlling the sensitivity of the weight; is a natural exponential function; and the adaptive penalty term is constructed according to the structure strength weight of each pixel point.

[0016] The maximum eigenvalue of the local structure tensor is quantified as the structure strength, and the structure strength weight is generated accordingly, so that the texture complexity of the local region of the image is accurately evaluated, the structure strength weight is used to dynamically adjust the smoothing penalty term in the cost aggregation process, so that greater smoothing constraint is applied in the strong structure region with clear contour to suppress noise, and smaller constraint is applied in the flat region with single texture to retain details, and the adaptive mechanism can effectively solve the drawbacks of the fixed penalty term, and realize the intelligent balance between noise suppression and detail retention.

[0017] Preferably, the cost aggregation of the initial matching cost volume along each of the adaptive aggregation paths in the adaptive aggregation path field to obtain an aggregated cost volume comprises: determining path aggregation costs of the adaptive aggregation paths at each pixel point under different disparities, accumulating path aggregation costs of all the adaptive aggregation paths passing through each pixel point at the pixel point under the same disparity as the final aggregation cost of the pixel point under the corresponding disparity, and constructing the final aggregation costs of all the pixel points under all the disparities as the aggregated cost volume.

[0018] Preferably, the path aggregation cost satisfies the expression:

[0019] ;

[0020] In the expression, is the path aggregation cost of the target path at the pixel point under the disparity ; is the matching cost of the pixel point under the disparity and the corresponding pixel point in the right image; is the previous pixel point of the pixel point on the target path; is the aggregation cost of the previous pixel point under the same disparity ; is the path aggregation cost of the previous pixel point under the disparity ; is the path aggregation cost of the previous pixel point under the disparity ; is the penalty term applied to the change of the disparity by 1; is the minimum aggregation cost of the previous pixel point under all the disparities; is the penalty term applied to the change of the disparity by more than 1.

[0021] The adaptive penalty term associated with the local structure strength is introduced in the cost aggregation of the dynamic programming, so that the cost aggregation process can intelligently perceive the image content. In the edge area with dramatic structure change, the larger penalty term can inhibit the dramatic jump of the disparity, ensuring the smooth and continuous contour. In the texture flat area, the smaller penalty term allows the small change of the disparity, thereby protecting the fine structure of the surface and ensuring that the aggregated cost volume can more truly reflect the depth information of the scene, laying a foundation for generating a high-quality disparity map.

[0022] Preferably, the disparity map is determined according to the aggregation cost volume, and a three-dimensional pose of a target point to be drilled is determined according to the disparity map, comprising: for each pixel point in the aggregation cost volume, selecting a disparity that minimizes the aggregation cost as the optimal disparity of the pixel point to form an initial disparity map; performing optimization processing on the initial disparity map to obtain a final disparity map; performing triangulation on the final disparity map according to an intrinsic parameter matrix of a camera and a baseline length of binoculars to obtain a three-dimensional point cloud model; in the three-dimensional point cloud model, extracting a local point cloud in a neighborhood of the target point to be drilled and calculating a covariance matrix of the local point cloud, and taking a feature vector corresponding to a minimum eigenvalue of the covariance matrix as a surface normal vector of the target point to be drilled; and constructing the three-dimensional pose of the target point to be drilled by using the three-dimensional coordinates of the target point to be drilled and the surface normal vector of the target point to be drilled.

[0023] After obtaining the disparity map, the present application converts it into a dense three-dimensional point cloud through the principle of triangulation, which provides an accurate spatial geometric description of the scene.

[0024] In the second aspect, the present application provides a real-time positioning system for mechanical arm operation, comprising a processor and a memory, wherein the memory stores computer program instructions, and when the computer program instructions are executed by the processor, the above-mentioned real-time positioning method for mechanical arm operation is realized.

[0025] By adopting the above technical solution, the above-mentioned real-time positioning method for mechanical arm operation is generated into a computer program and stored in the memory to be loaded and executed by the processor, so that a terminal device is manufactured according to the memory and the processor, and the use is facilitated.

[0026] The present application has the advantages that: the present application constructs an adaptive aggregation path that can closely fit the curved profile of a target object, and introduces an adaptive penalty mechanism based on local structure strength, which fundamentally solves the limitations of traditional stereo matching algorithms in dealing with complex geometric structures and variable texture surfaces, improves the accuracy of the disparity map in the edge of an object and irregular curved surface areas, and enhances the robustness to complex lighting environments, and the present application can accurately calculate the three-dimensional pose of a target point for operation in real time, which provides reliable visual guidance for high-precision automatic operation of a mechanical arm. BRIEF DESCRIPTION OF DRAWINGS

[0027] Figure 1 is a flowchart schematically showing a real-time positioning method for mechanical arm operation in the present application;

[0028] Figure 2is a schematic diagram of a left image of a tunnel wall;

[0029] Figure 3 is a schematic diagram of a right image of a tunnel wall;

[0030] Figure 4 is a visualization image of a structural strength weight;

[0031] Figure 5 is a visualization image of a penalty term P2. DETAILED DESCRIPTION

[0032] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are some but not all of the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0033] The specific embodiments of the present application will be described in detail below with reference to the drawings.

[0034] The embodiments of the present application disclose a real-time positioning method for mechanical arm operation, referring to Figure 1 , comprising steps S1-S4:

[0035] S1, collecting and pre-processing a stereo image pair, and calculating an initial matching cost volume, wherein the initial matching cost volume is composed of matching costs between pixel points in the stereo image pair under different disparities.

[0036] It should be noted that, in order to reconstruct the irregular curved surface three-dimensional model of the tunnel wall and provide a reliable basis for subsequent calculation of the drilling normal, it is necessary to ensure that the input data of stereo matching is accurate. Since the inherent distortion of the camera lens and the non-ideal parallel state of the binocular camera installation will introduce model errors from the source, it is necessary to strictly correct the geometry before calculating the stereo matching. In addition, the tunnel is usually illuminated by a small amount of construction searchlight, which will form strong local highlights and hard shadows on the surface of concrete or rock. To solve this problem, the present application adopts a cost calculation method which is not sensitive to light changes to ensure the reliability of the initial matching information under complex lighting.

[0037] Specifically, a binocular vision sensor deployed at the end of the mechanical arm is used to collect a stereo image pair of the tunnel wall in real time, which is taken as a left image and a right image respectively. Exemplarily, Figure 2 is a schematic diagram of a left image of a tunnel wall, Figure 3 is a schematic diagram of a right image of a tunnel wall.

[0038] Using pre-calibrated binocular camera parameters, distortion correction and stereo correction are performed on the left and right images acquired in real time from the end of the robotic arm, so that the epipolar lines of the two images are horizontally collinear, thus obtaining the corrected left and right images.

[0039] Furthermore, to reduce the impact of illumination changes, for each pixel in the corrected left image... Within the preset parallax range Within the image, the matching cost between the image and the corresponding pixel in the right image is calculated. The matching cost is composed of a weighted sum of the Census transform cost and the SAD cost. To ensure consistency of dimensions, the Census transform cost and the SAD cost are normalized before the weighted summation, so that the Census transform cost and the SAD cost are mapped to the interval [0,1].

[0040] Specifically, the matching cost satisfies the expression:

[0041]

[0042] In the formula, For pixels At parallax The matching cost between the current and the corresponding pixel in the right image; For pixels At parallax The cost of the normalized Census transform of the corresponding pixels in the right image; For pixels At parallax The normalized SAD cost of the corresponding pixel in the right image; These are weighting coefficients. This invention uses weighting coefficients... Balancing the advantages of both costs, the Census transform cost provides robustness to illumination changes, while the SAD cost preserves sensitivity to pixel grayscale details. When the value is high, the matching cost More focused on resisting changes in light; when When the value is low, the matching cost is low. It places more emphasis on the absolute difference in grayscale values.

[0043] The parallax range Based on the robotic arm's working distance and the camera's baseline length, in this embodiment, it is set to [0, 128] pixels. In other embodiments, the implementer can set it according to the actual implementation situation. Weighting coefficient Its function is to balance illumination robustness and detail sensitivity. The value range is [0,1], with an empirical value of 0.8. In other embodiments, the implementer can dynamically adjust it according to the lighting complexity of the actual working environment. The values ​​of are as follows. The Census transform cost and SAD cost are well-known techniques, and their calculation process will not be described in detail here.

[0044] Each pixel in the corrected left image is within the disparity range. Each parallax The matching costs of corresponding pixels in the lower and right images constitute a three-dimensional initial matching cost volume. This three-dimensional initial matching cost volume is a dimensional... A three-dimensional data volume, where each element stores a pixel from the corrected left image. Its corresponding pixel in the right image The matching cost between them, where This indicates the size of the corrected left image. This indicates the row number of the corrected left image. This indicates the column number of the corrected left image.

[0045] S2: Calculate the local structure tensor of each pixel based on one of the stereo image pairs, and generate an adaptive aggregated path field covering the image based on the local structure tensor.

[0046] It should be noted that the mismatch between the cost aggregation path of the traditional SGBM algorithm and the curved surface geometry of the tunnel wall is the core reason for the decrease in positioning accuracy. A fixed linear path will violate the assumption of depth continuity of pixels on the curved surface. An ideal aggregation path should be able to extend along the natural surface contour formed by the tunnel wall in the image. In the image, the direction of these surface contours usually represents the direction of the gentlest local gradient change. The local structure tensor can describe the gradient distribution in the neighborhood. Therefore, this invention analyzes the local structure tensor to find the structural direction that represents the surface contour, thereby providing accurate guidance for generating an adaptive path that fits the geometric features of the scene.

[0047] Specifically, the horizontal gradient of each pixel in the corrected left image is calculated using the Sobel operator. with vertical gradient .

[0048] Furthermore, for each pixel in the corrected left image Calculate pixel points Local structure tensor :

[0049]

[0050] In the formula, For pixels The local structure tensor; Indicated by A neighborhood window centered on the center; and Each is a pixel in the neighborhood. The horizontal and vertical gradients. The local structure tensor can describe the structural features of a region, effectively distinguishing between structured and unstructured regions in an image, and indicating the orientation of the structure when a pixel... When the local structure tensor is located on the arc-shaped contour formed by the tunnel wall in the image, the gradient directions of the pixels in its neighborhood are highly consistent and perpendicular to the arc-shaped contour. One eigenvalue will be much larger than the other, and their corresponding eigenvectors point to the gradient direction and the contour direction, respectively; conversely, if the pixel... In a flat region, the gradient values ​​of pixels in its neighborhood are small and their directions are random. In this case, the local structure tensor... Both eigenvalues ​​will approach zero. In this embodiment, the neighborhood window The size is 15×15. In other embodiments, implementers can adjust the size of the neighborhood window according to the actual implementation situation.

[0051] Furthermore, for the local structure tensor of each pixel Eigenvalue decomposition yields two eigenvalues. and the corresponding feature vector , will be with smaller eigenvalues corresponding feature vector The local optimal structural direction of this pixel The local optimal structure direction is the direction that extends along the contour in the image. Simultaneously, larger eigenvalues... This represents the structural strength of that pixel.

[0052] Starting from all pixels at the four boundaries of the corrected left image, based on the local optimal structure direction of each pixel... The path extends pixel by pixel into the corrected left image until it reaches the other boundary of the corrected left image, thereby generating a set of aggregated paths that can cover the corrected left image and bend with the content of the corrected left image, forming an adaptive aggregated path field.

[0053] S3. Determine the structural strength of each pixel based on the local structural tensor of each pixel, determine the adaptive penalty term for cost aggregation based on the structural strength, and perform cost aggregation on the initial matching cost volume along each aggregation path in the adaptive aggregation path field using the adaptive penalty term to obtain the aggregated cost volume.

[0054] It should be noted that even if the aggregated path can fit the geometric profile of the tunnel wall, the characteristics of the tunnel wall surface itself are changing, some areas are clear in profile, and some areas are flat and blurred. Using a fixed smooth penalty term for cost aggregation will result in insufficient smoothing in clear profile areas and excessive detail loss in flat and blurred areas. Therefore, the present application dynamically adjusts the smooth penalty term in the dynamic programming formula using the structural strength of the pixel points, so that the algorithm applies greater smooth penalty in areas with clear profile and determined structure, forcing the disparity to remain continuous, and applies smaller penalty in areas with fuzzy structure and uncertainty, allowing the disparity to have certain flexibility.

[0055] Specifically, the structural strength weight of the pixel points is determined according to the structural strength of each pixel point:

[0056]

[0057] In the formula, is the structural strength weight of the pixel point , and the value range is ; is the structural strength of the pixel point , that is, the maximum eigenvalue of the local structure tensor of the pixel point ; is a coefficient for controlling the sensitivity of the weight, used to adjust the response speed of the structural strength weight to the eigenvalue. The present application takes the mean value of the structural strength of all pixel points with structural strength greater than 0 as , and in other embodiments, the implementer can set according to the actual implementation situation. The present application uses an exponential function to map the eigenvalue with an indefinite distribution range to a standard weight interval. When the pixel point is located in a textureless area, tends to 0, also tends to 0; when the point is located in a clear profile strong structure area, the value is very large, so that tends to 1.

[0058] Exemplarily, Figure 4 is a structural strength weight visualization image.

[0059] According to the structural strength weight of the pixel points, an adaptive penalty term for cost aggregation is constructed:

[0060]

[0061]

[0062] In the formula, and is the adaptive penalty term of the pixel point . are the minimum and maximum values of the penalty term , respectively. are the minimum and maximum values of the penalty term . is the structure strength weight of the pixel point . The present application establishes a linear relationship between the penalty value and the structure strength, which is used to calculate the adaptive penalty term. The core function is to intelligently adjust the smoothing constraint strength in the cost aggregation process according to the characteristics of the local region of the image. In the region with clear structure such as the edge of the object, the weight tends to 1, making the penalty term tend to its maximum value, so as to exert strong constraint to suppress noise; in the flat region with single texture, the weight tends to 0, making the penalty term remain near its minimum value to avoid excessive smoothing and loss of details; when the structure strength weight changes from 0 to 1, the penalty term also smoothly transitions from the minimum value to the maximum value, realizing the adaptive adjustment of the penalty strength.

[0063] The empirical value of the range of the penalty term is , and in other embodiments, the implementer can adjust the range of the penalty term according to the actual implementation, but needs to meet , because the sharp jump of the disparity should be applied with greater punishment.

[0064] Figure 5 Exemplarily, is the visualization image of the penalty term P2.

[0065] Further, for any one aggregation path in the adaptive aggregation path field as a target path, the path aggregation cost of the target path at each pixel point of the target path under different disparities is determined:

[0066] ;

[0067] In the formula, is the path aggregation cost of the target path at the pixel point when the disparity is ; is the matching cost of the pixel point with the corresponding pixel point in the right image when the disparity is ; is the previous pixel point of the pixel point on the target path; is the aggregation cost of the previous pixel point under the same disparity ; is the matching cost of the previous pixel point with the corresponding pixel point in the right image when the disparity is the path aggregation cost at the time; the previous pixel point the path aggregation cost at the time; the path aggregation cost at the time; a penalty term imposed on the disparity change being 1; the previous pixel point the minimum aggregation cost under all disparities; a penalty term imposed on the disparity change being greater than 1; the third term in the expression to prevent the path aggregation cost from growing infinitely in the recursion process, maintain the effectiveness of the penalty term, and avoid numerical overflow.

[0068] The present application calculates the minimum cost path from the path starting point to the current pixel point by dynamic programming as the path aggregation cost, in the calculation process of the path aggregation cost, an adaptive penalty term and is introduced, so that in the area with strong structure, the disparity is continuous, and in the area with weak structure, greater disparity change is allowed, thereby effectively balancing noise suppression and detail preservation, avoiding the problems of insufficient smoothing at the clear outline and detail loss at the flat blur due to excessive constraints, so that the algorithm can give the optimal smoothing constraint when facing the variable working conditions of strong and weak surfaces, contours, etc., and enhance the robustness and environmental adaptability of the algorithm.

[0069] Further, for any pixel point in the corrected left image, the path aggregation costs of all aggregation paths passing through the pixel point at the same disparity at the pixel point are accumulated as the final aggregation cost of the pixel point at the corresponding disparity.

[0070] The final aggregation cost of each pixel point in the corrected left image at each disparity within the disparity range is used to form a three-dimensional aggregation cost volume, and the aggregation cost volume is a three-dimensional data body with a dimension , wherein represents the size of the corrected left image, represents the number of rows of the corrected left image, represents the number of columns of the corrected left image.

[0071] S4: Determine the disparity map according to the aggregation cost volume, determine the three-dimensional pose of the target point to be drilled according to the disparity map, and guide the mechanical arm drill bit to perform vertical drilling operation.

[0072] It should be noted that after adaptive path aggregation and penalty term constraints, the resulting aggregated cost volume can more accurately reflect the true depth information of the tunnel wall. In order to generate accurate three-dimensional pose commands that can guide the drilling actuator, this invention extracts the optimal disparity from the aggregated cost volume, optimizes the disparity map, and converts the depth information into spatial coordinates and normal vectors.

[0073] Specifically, for each pixel The disparity that minimizes the final aggregation cost is selected as the optimal disparity value for that pixel, and the optimal disparity values ​​of all pixels are used to form the initial disparity map. .

[0074] Furthermore, for the initial disparity map A left-right consistency check is performed to remove unreliable matching points, and the resulting hole regions are filled. Then, noise is smoothed through operations such as weighted median filtering to obtain a high-quality final disparity map. .

[0075] Furthermore, based on the pre-calibrated camera intrinsic parameter matrix and binocular baseline length Using the principle of triangulation, the final disparity map is obtained. Each pixel in and its disparity value Convert to 3D points in camera coordinate system .

[0076] In the obtained 3D point cloud model, the target point to be drilled and its local point cloud within its neighborhood are extracted. Principal component analysis (PCA) is applied to calculate the covariance matrix of this local point cloud, and the eigenvector corresponding to the smallest eigenvalue is determined as the surface normal vector of the target point. It should be noted that the neighborhood size used in PCA defines the local surface region used to calculate the normal. In this invention, the neighborhood used in PCA is a spherical region with a radius of 5 cm centered on the target point to be drilled. In other embodiments, the implementer can adjust the radius of the neighborhood according to the actual implementation situation.

[0077] The target pose, which is composed of the three-dimensional coordinates of the target point to be drilled and its surface normal vector, is sent to the robotic arm motion controller to guide the drill bit to complete high-precision positioning and vertical drilling operations.

[0078] This invention also discloses a real-time positioning system for robotic arm operations, including a processor and a memory. The memory stores computer program instructions, which, when executed by the processor, implement a real-time positioning method for robotic arm operations according to the present invention.

[0079] The system also comprises other components well known to those skilled in the art such as a communication bus and a communication interface, the arrangement and function of which are known in the art and thus will not be described here in more detail.

Claims

1. A real-time positioning method for a robot arm operation, characterized by, The method comprises the following steps: acquire a stereo image pair to be processed, and calculate an initial matching cost volume composed of matching costs of pixel points in the stereo image pair at different disparities; calculate local structure tensors of each pixel point in one image of the stereo image pair; generate an adaptive aggregation path field covering the one image according to the local structure tensors, including: perform eigenvalue decomposition on the local structure tensor of each pixel point to obtain two eigenvalues and corresponding eigenvectors; take the eigenvector corresponding to the minimum eigenvalue in the two eigenvalues as the local optimal structure direction of the pixel point; extend from the boundary pixel points of the one image to the interior of the one image according to the local optimal structure direction of each pixel point to generate a group of aggregation paths, thereby constituting the adaptive aggregation path field; determine the structure strength of each pixel point according to the local structure tensor of the pixel point; determining an adaptive penalty term for cost aggregation based on the structure strength, including: determining a structure strength weight of each pixel point according to the structure strength of the pixel point: , is the structure strength weight of the pixel point , is the structure strength of the pixel point , is a coefficient for controlling weight sensitivity, is a natural exponential function; constructing the adaptive penalty term according to the structure strength weight of each pixel point; perform cost aggregation on the initial matching cost volume along each aggregation path in the adaptive aggregation path field by using the adaptive penalty term to obtain an aggregated cost volume; determine a disparity map according to the aggregated cost volume, and determine the three-dimensional pose of a target point to be drilled according to the disparity map to guide the drilling head of a mechanical arm to perform vertical drilling operation.

2. The real-time positioning method for mechanical arm operation according to claim 1, wherein, The method for obtaining the matching cost comprises the following steps: perform distortion correction and stereo correction on the stereo image pair; for each pixel point in one of the corrected images, calculate the matching cost of the pixel point with the corresponding pixel point in the other corrected image within a preset disparity range, wherein the matching cost is composed of a Census transformation cost and a SAD cost.

3. The real-time positioning method for mechanical arm operation according to claim 1, wherein, The method for calculating the local structure tensor of each pixel point in one image of the stereo image pair comprises the following steps: calculate the horizontal gradient and the vertical gradient of each pixel point in the one image; for each pixel point, construct its local structure tensor according to the horizontal gradient and the vertical gradient of each pixel point in its neighborhood window.

4. The real-time positioning method for mechanical arm operation according to claim 1, wherein, The method for determining the structure strength of each pixel point according to the local structure tensor of the pixel point comprises the following steps: perform eigenvalue decomposition on the local structure tensor of each pixel point, and take the maximum eigenvalue in the two eigenvalues obtained by the decomposition as the structure strength of the pixel point.

5. The real-time positioning method for mechanical arm operation according to claim 1, wherein, The method for performing cost aggregation on the initial matching cost volume along each aggregation path in the adaptive aggregation path field by using the adaptive penalty term to obtain an aggregated cost volume comprises the following steps: determine the path aggregation cost of the aggregation path at each pixel point at different disparities, add the path aggregation cost of all aggregation paths passing through each pixel point at the same disparity at the pixel point to obtain the final aggregation cost of the pixel point at the corresponding disparity, and constitute the aggregated cost volume by using the final aggregation cost of all pixel points at all disparities.

6. The real-time positioning method for mechanical arm operation according to claim 5, wherein, The path aggregation cost satisfies the expression: ； wherein, is the path aggregation cost of the target path at pixel point when the disparity is ; is the matching cost of pixel point in the left image when the disparity is ; is the previous pixel point of pixel point on the target path; is the aggregation cost of the previous pixel point at the same disparity ; is the path aggregation cost of the previous pixel point when the disparity is ; is the path aggregation cost of the previous pixel point when the disparity is ; is the penalty term imposed on the disparity change being 1; is the minimum aggregation cost of the previous pixel point at all disparities; is the penalty term imposed on the disparity change being greater than 1.

7. The real-time positioning method for mechanical arm operation according to claim 1, wherein, The method for determining a disparity map according to the aggregated cost volume, and determining the three-dimensional pose of a target point to be drilled according to the disparity map comprises the following steps: For each pixel point in the aggregation cost volume, an optimal disparity is selected as the optimal disparity of the pixel point, and an initial disparity map is formed; the initial disparity map is optimized to obtain a final disparity map; the final disparity map is triangulated according to a camera intrinsic matrix and a binocular baseline length to obtain a three-dimensional point cloud model; in the three-dimensional point cloud model, a local point cloud in a neighborhood of a target point to be drilled is extracted, a covariance matrix of the local point cloud is calculated, and a feature vector corresponding to a minimum eigenvalue of the covariance matrix is taken as a surface normal vector of the target point to be drilled; and a three-dimensional pose of the target point to be drilled is formed by the three-dimensional coordinates of the target point to be drilled and the surface normal vector.

8. A real-time positioning system for use in a robot operation, characterized by Comprise: A processor and a memory, the memory stores computer program instructions, when the computer program instructions are executed by the processor, a real-time positioning method for mechanical arm operation according to any one of claims 1-7 is realized.

Images