A Control Method for Anchor Drill Arm Hole-Setting Operations Based on Multimodal Sensing and Servo Control
The anchor drill arm alignment method using multimodal sensing and servo control solves the problems of blind zone distortion of visual sensors and drill slippage in tunnel dust and water mist environments, achieving high-precision and safe drilling operations.
Patent Information
- Application Number
- CN202611586536.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-10-14
- Publication Date
- 2026-11-17
AI Technical Summary
In the case of high dust and high-pressure water mist dust suppression after blasting of large sections of hard rock in tunnels, the existing anchor drill arm suffers from blind spots caused by visual sensor image occlusion, which leads to distortion of the reconstructed feature map. Furthermore, the drill bit is prone to lateral slippage when it contacts the rock surface, resulting in drill rod breakage and mechanical damage.
By using multimodal perception and servo control, and utilizing spatiotemporal registration of two-dimensional images and three-dimensional point clouds, eigenvalue decomposition of local covariance matrix, and constant torque propulsion mode, multipath scattering noise is filtered out, rock surface point sets are extracted, and normal vectors are calculated to achieve constant torque propulsion of the drill bit.
It improves the accuracy of borehole operations and the safety of mechanical execution under harsh working conditions, reduces equipment wear and tear and the frequency of operator intervention, and ensures the precision of tunnel construction and the lifespan of equipment.
Smart Images

Figure CN123113633A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of anchor drill arm control technology, specifically to an anchor drill arm control method for hole alignment based on multimodal sensing and servo control. Background Technology
[0002] Tunnel support is a crucial link in ensuring the safety of underground space construction. Among them, the automatic drilling arm alignment in advanced support and anchoring operations is a typical high-precision control task for heavy equipment in unstructured environments. With the advancement of intelligent tunnel construction, multimodal perception technologies based on vision and radar are widely used in the positioning and addressing of anchor drill arms. For example, Chinese patent application CN115877400A discloses a drilling positioning method for roadway roof support steel strip based on radar and vision fusion. It extracts two-dimensional target hole positions through visual sensors and combines them with three-dimensional point cloud depth information obtained by lidar to generate drilling orientation information for the drilling rig, thereby guiding automated drilling.
[0003] However, existing multimodal drilling arm guidance technology has significant engineering defects in high dust and high-pressure water mist dust suppression conditions after large-section blasting of hard rock tunnels. First, in environments with extremely dense dust and water mist, the two-dimensional images of the visual sensor will have large blind spots due to light attenuation. To compensate for visual failure, the radar's transmission power is usually increased to improve signal penetration. However, high-gain laser pulses will cause severe multipath scattering on the surface of suspended droplets. If fusion positioning is performed directly, a large number of free scattering noise point clouds will be mistakenly used as real rock surface features for image repair, resulting in severe three-dimensional spatial topological distortion in the reconstructed feature map.
[0004] On the other hand, the underlying mechanical execution is highly dependent on the rock surface normal vector output by the sensing end. For example, Chinese patent application CN115082548A discloses an automatic positioning system and method for anchor drilling based on binocular vision. It directly controls the action of the drilling robot by calculating the three-dimensional spatial coordinates. However, at the tunnel face, once the sensing layer outputs distorted geometric guidance commands, the servo control system is very likely to force the drill bit to press against the uneven hard rock at an incorrect non-orthogonal angle. Existing drill arms mostly adopt position control based on spatial coordinates. At the moment when the drill bit rigidly contacts the rock surface, the huge thrust applied by the propulsion mechanism will generate a huge lateral component torque, causing the heavy drill arm to slip severely laterally. Not only will it be unable to complete orthogonal hole alignment, but it is also very likely to cause the drill rod to break or even the rigidity of the mechanical transmission structure to be damaged. Summary of the Invention
[0005] To address the shortcomings of existing technologies in effectively mitigating spatial topological distortion in tunnel environments and the lack of underlying force constraint mechanisms to address instantaneous slippage from rigid rock surfaces, which leads to low success rates and equipment safety in automated drill arm operations under harsh conditions, this invention proposes a multimodal sensing and servo control method for anchor drill arm borehole operation control. This method includes: S1. Acquire a two-dimensional image and a three-dimensional point cloud of the tunnel face, acquire the stator current signal of each joint drive motor, perform spatiotemporal registration of the two-dimensional image and the three-dimensional point cloud using a preset extrinsic parameter matrix, and then construct a local covariance matrix. Perform eigenvalue decomposition on the local covariance matrix, extract principal component eigenvalues, and calculate surface variability. S2. Set a preset threshold. For data points whose surface variability is greater than the threshold, treat them as volume distribution manifolds and remove them. For data points whose surface variability is less than or equal to the threshold, treat them as surface distribution manifolds and extract them to obtain a rock surface point set. S3. Use a preset projection matrix to inversely project the rock surface point set onto the area to be repaired in the two-dimensional image to generate a fusion feature map, and then extract the three-dimensional spatial coordinates of the target hole position. Calculate the rock surface normal vector based on the rock surface point set of the target hole position. S4. Drive the drill arm according to the three-dimensional spatial coordinates. When the drill reaches the preset safety contact line, switch to constant torque propulsion mode and control the drill to cut into the target hole position along the rock surface normal vector.
[0006] This invention effectively filters out multipath scattering interference through manifold space classification, and at the same time, combined with the bottom constant torque loop, it eliminates the force offset of the drill bit at the moment of contact with the rock surface, thereby improving the accuracy of hole operation and the safety of mechanical execution.
[0007] Furthermore, the spatiotemporal registration includes: aligning the timestamps of the two-dimensional image and the three-dimensional point cloud based on a hardware trigger signal, and projecting the pixel coordinates of the two-dimensional image and the spatial coordinates of the three-dimensional point cloud onto the global coordinate system through the extrinsic parameter matrix.
[0008] This invention eliminates data misalignment caused by asynchronous sampling from heterogeneous sensors at the underlying control timing level, and constructs a geometric mapping channel for cross-modal data in the spatial dimension, providing a clean basic data source for subsequent high-precision feature extraction and image inpainting. Furthermore, the construction of the local covariance matrix includes: extracting a set of spatial nearest neighbors of the target data point within a preset search radius, obtaining the spatial centroid coordinates of the set of spatial nearest neighbors, and constructing the local covariance matrix based on the spatial geometric deviation of each point in the set of spatial nearest neighbors relative to the spatial centroid coordinates.
[0009] Further, the calculation of surface variability includes: arranging the extracted principal component feature values according to the trend of data dispersion, obtaining the feature value representing the thickness variance and the feature value representing the overall variance, and determining the surface variability based on the proportional relationship between the feature value representing the thickness variance and the sum of the feature values.
[0010] This invention, by evaluating the local topological morphology of point clouds in three-dimensional space, can distinguish between chaotic suspended medium noise and continuously distributed solid rock walls, providing a reliable numerical criterion for manifold classification in complex working environments.
[0011] Further, the generation of the fused feature map includes: identifying dark field pixel regions in the two-dimensional image whose grayscale values are lower than a preset visibility limit, as the regions to be repaired; using the projection matrix, projecting the rock surface point set onto the regions to be repaired, and converting the echo data of the rock surface point set into grayscale pixel values for filling, thereby obtaining the fused feature map.
[0012] This invention utilizes the physical advantage of the high penetration of three-dimensional radar data to specifically repair the information loss caused by high concentration of dust obstruction in visual sensors, ensuring the integrity and continuity of the visual features of the aperture.
[0013] Further, the extraction of the three-dimensional spatial coordinates of the target hole includes: using an edge detection operator to perform a global scan of the fused feature map, extracting the geometric center of the edge region that conforms to the preset aperture feature as the target positioning point; retrieving the depth data corresponding to the target positioning point, and performing inverse three-dimensional calculation in combination with the perspective matrix to obtain the three-dimensional spatial coordinates of the target hole.
[0014] Further, the calculation of the rock surface normal vector includes: extracting the rock surface point set around the target borehole, fitting the extracted rock surface point set to a local tangent plane, and obtaining the rock surface normal vector perpendicular to the local tangent plane.
[0015] Furthermore, driving the drill arm according to the three-dimensional spatial coordinates specifically includes: inputting the three-dimensional spatial coordinates into the inverse kinematics equations to obtain the expected joint angle sequence corresponding to the multi-axis actuator of the drill arm, and driving the drill arm to approach the target hole position based on the expected joint angle sequence.
[0016] Furthermore, the constant torque propulsion mode includes: obtaining the torque constant corresponding to each joint drive motor, combining the stator current signal with the corresponding torque constant, and converting it into actual output torque, which serves as a feedforward reference quantity in the constant torque propulsion mode.
[0017] Furthermore, controlling the drill bit to cut into the target hole along the rock surface normal vector includes: using the rock surface normal vector as the reference axis for the drill bit's propulsion force, suppressing the target value of the lateral component torque that is not parallel to the rock surface normal vector, and adjusting the pulse duty cycle of the drive motors of each joint to control the drill bit to output feed torque in the positive direction of the rock surface normal vector.
[0018] This invention transforms the geometric posture extracted by the upper-layer sensing computing power into the mechanical constraint boundary of the lower-layer hardware, thereby cutting off the lateral slippage path that is very likely to occur when the drill bit contacts the uneven rock surface from the control source, effectively reducing the risk of rigid damage to the mechanical transmission structure.
[0019] The present invention has the following technical effects: This invention accurately separates multipath scattering noise generated by the environmental medium from the geometric distribution dimension through spatiotemporal registration and eigenvalue decomposition of the local covariance matrix. Furthermore, it utilizes the purified surface distribution rock surface point set to perform reverse perspective compensation for visual blind spots. Even in harsh working conditions with low visibility, it can still extract the target borehole coordinates and rock surface attitude information, avoiding drill rod breakage and equipment lateral slippage caused by attitude errors. This effectively reduces the frequency of operator intervention in high-risk environments, lowers the equipment wear rate of heavy engineering machinery in complex unstructured construction interfaces, and ensures the straightness and stability of each borehole cut. Therefore, it comprehensively improves the accuracy and long-term service life of automated construction equipment in underground space, and has high engineering application value in tunnel excavation and mining projects in various complex geological environments. Attached Figure Description
[0020] Figure 1 This is a flowchart of the anchor drill arm hole-to-hole operation control method based on multimodal perception and servo control provided in the embodiments of the present invention; Figure 2 This is a comparison diagram of the effects of the prior art provided in the embodiments of the present invention and the present invention. Detailed Implementation
[0021] This invention provides a method for controlling the drilling operation of an anchor bolt drill arm based on multimodal sensing and servo control, referring to... Figure 1 This includes steps S1-S4: S1: Data Acquisition and Preprocessing.
[0022] Specifically, the process involves acquiring two-dimensional images and three-dimensional point clouds of the tunnel face, obtaining stator current signals of the drive motors for each joint, using a preset extrinsic parameter matrix to perform spatiotemporal registration of the two-dimensional images and three-dimensional point clouds, constructing a local covariance matrix, performing eigenvalue decomposition on the local covariance matrix, extracting principal component eigenvalues, and calculating surface variability.
[0023] Before performing the anchor bolt drilling operation, it is necessary to continuously acquire multimodal environmental data of the tunnel face. In this embodiment, it is preferable to install an explosion-proof industrial camera and a frequency-modulated continuous wave lidar on the end effector of the anchor bolt drilling arm. The explosion-proof industrial camera is used to continuously capture the visible photon radiation of the tunnel face and convert it into two-dimensional image data. In this embodiment, the sampling frequency of the camera is preferably set to 60Hz. The frequency-modulated continuous wave lidar is used to acquire three-dimensional point cloud data containing environmental reflection point information. In dusty conditions, this embodiment preferably controls the lidar to activate the high-gain penetration mode and sets the sampling frequency to 10Hz.
[0024] Simultaneously, the stator current signal of each joint drive motor is acquired. In this embodiment, Hall current sensors are preferably embedded inside the stator coil of each servo motor to sense changes in motor phase current. The acquired stator current signal is cached in the controller as a feedforward reference for subsequent constant torque control.
[0025] After acquiring the aforementioned heterogeneous data, spatiotemporal registration is performed on the 2D image and the 3D point cloud. In this embodiment, a unified data acquisition clock source is preferably achieved through a low-level hardware interrupt signal. The current camera image frame and the LiDAR point cloud frame are synchronously latched using pin levels to complete timestamp alignment. Subsequently, spatial registration is performed using a preset extrinsic parameter matrix. The pixel plane coordinates of the 2D image and the spatial polar coordinates of the 3D point cloud are extracted. Homogeneous transformation matrix multiplication is then performed using this extrinsic parameter matrix to project the two sets of coordinates onto a global Cartesian coordinate system, completing the spatiotemporal registration. It should be noted that this extrinsic parameter matrix was obtained through an offline scanning calibration experiment. The specific steps are as follows: First, in a clear, dust-free environment, a composite calibration plate is placed in front of the anchor drill arm. In this embodiment, the composite calibration plate preferably has a high-contrast black and white checkerboard texture printed on its surface, and three-dimensional geometric circular holes are cut out at its four corners. Next, the camera and LiDAR simultaneously scan the composite calibration plate. Using a corner point extraction algorithm, the two-dimensional optical center coordinates of the calibration plate's feature points in the pixel coordinate system are extracted from the camera's image. Simultaneously, using a spatial clustering algorithm, the three-dimensional reflection point coordinates of the corresponding feature points in the polar coordinate system are extracted from the radar data. Then, based on the geometric correspondence of the same feature point in different coordinate systems, a spatial constraint equation set for the same feature points is established. In this embodiment, the nonlinear least squares method is preferably used to iteratively solve this constraint equation set until the error converges. After solving, the rigid rotation matrix and translation vector between the camera's optical center and the radar's transmission center are obtained. This rigid rotation matrix and translation vector are combined and stored in the controller's read-only memory as the aforementioned preset extrinsic parameter matrix.
[0026] After completing spatiotemporal registration, a local covariance matrix is constructed using the processed 3D point cloud. In this embodiment, the target data points are traversed in the 3D point cloud with a preset search radius, preferably set to 20mm. The spatial nearest neighbor set within the search radius is extracted, and the arithmetic mean of all 3D coordinates in the nearest neighbor set is calculated to obtain the spatial 3D centroid coordinates of the local region. Then, the spatial geometric deviation of the coordinates of each point in the nearest neighbor set relative to the spatial 3D centroid coordinates is calculated. The outer product operation and summation of the above spatial geometric deviations are performed on the matrix to finally generate a 3x3 symmetric matrix. This symmetric matrix is the local covariance matrix that reflects the scattering direction and characteristics of the point cloud in a local small space.
[0027] Subsequently, eigenvalue decomposition is performed on the local covariance matrix to extract principal component eigenvalues. In this embodiment, the standard linear algebraic eigenvalue decomposition algorithm is preferably used to reduce the dimensionality of the matrix and extract the principal component eigenvalues that represent the dispersion of the data in three mutually orthogonal directions. The extracted principal component eigenvalues are arranged in descending order according to the direction representing the dispersion of the data. After the arrangement, the eigenvalue with the largest value represents the longest axis variance of the distribution, the second largest value represents the second longest axis variance, and the smallest value represents the shortest axis variance along the local surface normal direction, i.e., the thickness variance.
[0028] Finally, the surface variability is calculated based on the extracted principal component eigenvalues. In this embodiment, the minimum eigenvalue representing the thickness variance is extracted, the sum of the three principal component eigenvalues is calculated, the eigenvalue representing the thickness variance is used as the dividend, and the sum of the eigenvalues is used as the divisor to perform a division operation, thereby obtaining the surface variability of the local area where the target data point is located. This surface variability represents the morphological characteristics of the spatial point cloud on the local topology, providing underlying numerical support for the subsequent differentiation of scattered water mist and continuous rock walls.
[0029] S2: Spatial manifold classification and rock surface point set extraction.
[0030] Specifically, a preset threshold is set. Data points with surface variability greater than the threshold are treated as volume distribution manifolds and removed. Data points with surface variability less than or equal to the threshold are treated as surface distribution manifolds and extracted to obtain a rock surface point set.
[0031] After obtaining the surface variability of each data point, it is necessary to classify and identify the phase of the massive point cloud data by using a preset limit threshold. This limit threshold represents the geometric tolerance for the transition of spatial data from a two-dimensional plane to a three-dimensional volume distribution. Under tunnel dust and high-pressure spray dust suppression conditions, suspended water mist particles will cause multipath scattering of laser pulses. This scattering noise presents a highly disordered volume distribution with equal dispersion in all directions in the local space, and its surface variability value is relatively large. In contrast, even if there are uneven textures in the real solid rigid rock wall, it still presents a continuous surface distribution in the local area, and its thickness variance along the normal direction is extremely small, corresponding to a smaller surface variability value.
[0032] In order to effectively distinguish the two environmental characteristics mentioned above, it is necessary to pre-set a boundary threshold. In this embodiment, it is preferred to define the range of the boundary threshold between 0.05 and 0.15. Considering that the tunnel face usually presents a rough rock cross-section, this embodiment preferably sets the preset boundary threshold to 0.1. Using this value can filter out the noise of suspended water mist while retaining the tiny pits and crack textures on the rock surface without damage, avoiding the accidental deletion of rock wall features due to harsh conditions.
[0033] After setting the threshold, iterate through all target data points in the point cloud data and compare the surface variability of each data point with the threshold: For data points whose surface variability exceeds the threshold, it is determined that the spatial variance of the local area is too large, which is consistent with the spatial distortion characteristics caused by multipath scattering of suspended water mist. Such data points are treated as volume distribution manifolds and removed from the point cloud dataset. For data points whose surface variability is less than or equal to the threshold, the thickness variance of the local area is determined to be minimal, which conforms to the geometric topological continuity characteristics of the solid rock wall. These data points are treated as surface distribution manifolds and extracted and retained to obtain the rock surface point set.
[0034] As can be seen, through the above point-by-point traversal and manifold classification filtering, the scattered spatial noise generated by the environmental medium is effectively separated, and a high-fidelity rock surface point set is finally obtained, providing clean spatial data for subsequent feature map fusion and normal vector calculation.
[0035] S3: Fusion feature map generation and rock surface normal vector calculation.
[0036] Specifically, using a preset projection matrix, the rock surface point set is inversely projected onto the area to be repaired in the two-dimensional image to generate a fused feature map, and then the three-dimensional spatial coordinates of the target borehole are extracted. Based on the rock surface point set of the target borehole, the rock surface normal vector is calculated.
[0037] After extracting the pure rock surface point set, the three-dimensional spatial data needs to be used to repair the visual occlusion blind spots in the two-dimensional image, thereby locating the drilling target and calculating the true pose of the rock wall.
[0038] First, a global pixel scan is performed on the spatiotemporally registered 2D image to identify dark-field pixel regions with grayscale values below a preset visibility lower limit. In this embodiment, the preset visibility lower limit is preferably set to a grayscale value of 20. These dark-field regions that have lost details due to obscuring by extremely high concentrations of dust are marked as areas to be repaired. Then, using a preset projection matrix, the rock surface point set extracted in S2 is inversely projected onto the aforementioned areas to be repaired. This preset projection matrix is derived by combining the internal optical perspective parameters of the industrial camera with the extrinsic parameter matrix obtained in S1. After completing the spatial projection, the radar echo intensity data of each spatial point in the rock surface point set is linearly scaled and mapped to standard grayscale pixel values, which are then filled onto the corresponding pixels in the areas to be repaired, thereby obtaining a fused feature map with continuous and complete visual features.
[0039] Next, the fused feature map is scanned pixel by pixel using an edge detection operator. This embodiment preferably employs an edge extraction algorithm based on pixel grayscale gradients to identify pixel contours where brightness and darkness change drastically in the image. From the identified contours, closed circular edge regions conforming to preset aperture characteristics are extracted. These preset aperture characteristics are set according to the standard diameter of the pilot hole in the previous construction, and are defined within the range of 30mm to 50mm. The geometric center of this circular edge region is extracted and used as the target positioning point. After obtaining the planar pixel coordinates of the target positioning point, they are added to the spatiotemporal mapping table established in S1. The radar 3D point cloud paired with the pixel coordinates is retrieved, and the depth value representing the distance from the target to the radar transmission center is extracted from the radar 3D point cloud data. Subsequently, the planar pixel coordinates, depth value, and camera intrinsic parameter matrix are input into the standard pinhole camera inverse projection equation. In this embodiment, the camera intrinsic parameter matrix is preferably obtained in advance and written into the memory using the Zhang Zhengyou checkerboard calibration method. Through this equation, the two-dimensional coordinates of the pixel plane are converted into three-dimensional coordinates in the camera coordinate system. Then, combined with the extrinsic parameter matrix, they are unified into the drill arm global coordinate system, and finally the three-dimensional spatial coordinates of the target hole position are obtained.
[0040] Finally, using the three-dimensional spatial coordinates of the target hole obtained above as the center, a spatial search sphere is constructed with a preset fitting radius. In this embodiment, the fitting radius is preferably set to 150mm, which can fully cover the effective supporting rock surface around the hole. The rock surface point set extracted in S2 is traversed, and all point cloud data falling within the spatial search sphere are extracted as the local rock surface point set for fitting. For the extracted pure surface distribution manifold data, a local tangent plane is fitted. In this embodiment, the least squares method is preferably used to derive the tangent plane equation. Its core execution logic is to find a spatial plane through iterative optimization, such that the sum of the squares of the vertical distances from all data points in the local rock surface point set to the spatial plane reaches the minimum value. After fitting the local tangent plane, the three-dimensional direction vector perpendicular to the local tangent plane is calculated according to the geometric algebraic equation of the tangent plane. Since the multipath scattering noise of the point set involved in the fitting has been removed in S2, the three-dimensional direction vector can accurately reflect the trend of the rock wall in the target drilling area. Therefore, it is output as the rock surface normal vector to the servo control layer.
[0041] S4: Constant torque anti-slip control and orthogonal hole alignment.
[0042] Specifically, the drill arm is driven according to three-dimensional spatial coordinates. When the drill bit reaches the preset safe contact line, it switches to constant torque propulsion mode and controls the drill bit to cut into the target hole along the rock surface normal vector.
[0043] After obtaining the three-dimensional spatial coordinates of the target hole position and the rock surface normal vector, the anchor drill arm is controlled to approach the target position. The three-dimensional spatial coordinates of the target hole position are input into the inverse kinematics equations. Based on the known connecting rod length and mechanical hardware configuration of the drill arm, the inverse kinematics equations deduce the spatial position of the end effector into the rotation angle of each drive joint. By solving, the expected joint angle sequence corresponding to the multi-axis actuator of the drill arm is obtained. Subsequently, based on the expected joint angle sequence, a position tracking command is issued to drive the drill arm to approach the target hole position.
[0044] Simultaneously, the stator current signal acquired and cached in S1 is extracted to calculate the real-time mechanical state; the torque constant corresponding to each joint drive motor is obtained. This torque constant is an inherent hardware parameter determined by the internal magnetic circuit design of the motor. In this embodiment, it is preferable to directly read the configuration from the factory nameplate data of the servo motor; the stator current signal is multiplied by the corresponding torque constant to convert it into the actual output torque of each joint drive motor, which serves as the feedforward reference quantity in the subsequent constant torque propulsion mode.
[0045] As the drill string approaches the face, based on the real-time feedback from the encoders of each drive joint, the coordinates of the drill string end are calculated in real time using the forward kinematic equation. Then, the normal distance from the local tangent plane is calculated and compared with the preset safety contact line. In this embodiment, the safety contact line is preferably set at a distance of 50mm from the normal distance of the actual rock surface. This distance setting takes into account the communication delay and mechanical braking inertia of the servo system, which can ensure that the heavy drill arm has sufficient mode switching buffer space before rigid contact occurs. When the drill string reaches the preset safety contact line, the position control mode based on coordinate tracking is cut off and the constant torque propulsion mode is switched.
[0046] In constant torque propulsion mode, the rock surface normal vector obtained in S3 is used as the sole reference axis for the force of the drill bit propulsion. Since the surface of the real hard rock face is uneven, it is very easy to generate lateral force that causes the drill arm to yaw at the moment of contact. Therefore, it is necessary to suppress all lateral force torques that are not parallel to the rock surface normal vector and set their control target value to zero. At the same time, the drill bit is controlled to output a constant feed torque only in the positive direction of the rock surface normal vector.
[0047] At the underlying electrical execution level, the precise distribution of the aforementioned torque is achieved by dynamically adjusting the pulse duty cycle of the drive motors of each joint. The control unit generates a high and low level pulse sequence after adjusting the duty cycle and inputs it into the power inverter bridge of the servo driver, thereby changing the magnitude of the effective phase current injected into the stator coil of the motor. By dynamically intervening in the phase current, the direction of the multi-axis torque vector synthesis of the robotic arm is corrected in real time, so that the resultant force converges on the rock surface normal vector, driving the drill bit to orthogonally cut into the target hole position, eliminating the risk of lateral slippage and mechanical damage at the moment of contact.
[0048] Figure 2 The diagram shows a comparison of the effects of the prior art and the present invention provided in the embodiments of the present invention. It can be seen that when the dust concentration increases, the prior art causes the image fusion algorithm to fail due to the aggravated multipath scattering, resulting in a decrease in accuracy. However, the present invention distinguishes the data at the bottom layer by using spatial manifold dimensionality reduction, and can still maintain a stable and high accuracy even in high concentration environments.
Claims
1. A method for anchor boom hole operation control based on multi-modal perception and servo control, characterized in that, include: S1. Acquire a two-dimensional image and a three-dimensional point cloud of the tunnel face, acquire the stator current signal of each joint drive motor, perform spatiotemporal registration of the two-dimensional image and the three-dimensional point cloud using a preset extrinsic parameter matrix, and then construct a local covariance matrix. Perform eigenvalue decomposition on the local covariance matrix, extract principal component eigenvalues, and calculate the surface variability. S2, preset a boundary threshold. For data points whose surface variability is greater than the boundary threshold, they are treated as volume distribution manifolds and removed. For data points whose surface variability is less than or equal to the boundary threshold, they are treated as surface distribution manifolds and extracted to obtain a rock surface point set. S3, using a preset projection matrix, the rock surface point set is inversely projected onto the area to be repaired in the two-dimensional image to generate a fusion feature map, and then the three-dimensional spatial coordinates of the target hole are extracted. Based on the rock surface point set of the target hole, the rock surface normal vector is calculated. S4. Drive the drill arm according to the three-dimensional spatial coordinates. When the drill bit reaches the preset safety contact line, switch to constant torque propulsion mode and control the drill bit to cut into the target hole along the rock surface normal vector.
2. The anchor drill arm hole-laying operation control method based on multimodal perception and servo control according to claim 1, characterized in that, The spatiotemporal registration includes: aligning the timestamps of the two-dimensional image and the three-dimensional point cloud based on a hardware trigger signal, and projecting the pixel coordinates of the two-dimensional image and the spatial coordinates of the three-dimensional point cloud onto the global coordinate system through the extrinsic parameter matrix.
3. The anchor drill arm hole-laying operation control method based on multimodal perception and servo control according to claim 1, characterized in that, The construction of the local covariance matrix includes: extracting a set of spatial nearest neighbors of the target data points within a preset search radius, obtaining the spatial centroid coordinates of the set of spatial nearest neighbors, and constructing the local covariance matrix based on the spatial geometric deviation of each point in the set of spatial nearest neighbors relative to the spatial centroid coordinates.
4. The anchor bolt drill arm hole-laying operation control method based on multimodal perception and servo control according to claim 1, characterized in that, The calculation of surface variability includes: arranging the extracted principal component feature values according to the trend of data dispersion, obtaining the feature value representing the thickness variance and the feature value representing the overall variance, and determining the surface variability based on the proportional relationship between the feature value representing the thickness variance and the sum of the feature values.
5. The anchor bolt drill arm hole-laying operation control method based on multimodal perception and servo control according to claim 1, characterized in that, The generation of the fused feature map includes: identifying dark field pixel regions in the two-dimensional image whose grayscale values are lower than a preset visibility limit, as the regions to be repaired; using the projection matrix, projecting the rock surface point set onto the regions to be repaired, and converting the echo data of the rock surface point set into grayscale pixel values for filling, thereby obtaining the fused feature map.
6. The anchor drill arm hole-laying operation control method based on multimodal perception and servo control according to claim 1, characterized in that, The extraction of the three-dimensional spatial coordinates of the target hole includes: using an edge detection operator to perform a global scan of the fused feature map, extracting the geometric center of the edge region that conforms to the preset aperture feature as the target positioning point; retrieving the depth data corresponding to the target positioning point, and performing inverse three-dimensional calculation in combination with the perspective matrix to obtain the three-dimensional spatial coordinates of the target hole.
7. The anchor bolt drill arm hole-laying operation control method based on multimodal perception and servo control according to claim 1, characterized in that, The calculation of the rock surface normal vector includes: extracting the rock surface point set around the target borehole, fitting the extracted rock surface point set to a local tangent plane, and obtaining the rock surface normal vector perpendicular to the local tangent plane.
8. The anchor drill arm hole-laying operation control method based on multimodal perception and servo control according to claim 1, characterized in that, The method of driving the drill arm according to the three-dimensional spatial coordinates specifically includes: inputting the three-dimensional spatial coordinates into the inverse kinematics equations to obtain the expected joint angle sequence corresponding to the multi-axis actuator of the drill arm, and driving the drill arm to approach the target hole position based on the expected joint angle sequence.
9. The anchor bolt drill arm hole-laying operation control method based on multimodal perception and servo control according to claim 1, characterized in that, The constant torque propulsion mode includes: obtaining the torque constant corresponding to each joint drive motor, combining the stator current signal with the corresponding torque constant, and converting it into actual output torque, which is used as a feedforward reference quantity in the constant torque propulsion mode.
10. The anchor drill arm hole-laying operation control method based on multimodal perception and servo control according to claim 1, characterized in that, Controlling the drill bit to cut into the target hole along the rock surface normal vector includes: using the rock surface normal vector as the reference axis for the drill bit's propulsion force, suppressing the target value of the lateral component torque that is not parallel to the rock surface normal vector, adjusting the pulse duty cycle of the drive motors of each joint, and controlling the drill bit to output feed torque in the positive direction of the rock surface normal vector.
Citation Information
Patent Citations
Binocular vision-based anchor drilling hole automatic positioning system and method
CN115082548A
Roadway roof support steel belt drilling positioning method based on radar and vision fusion
CN115877400A
