Intelligent control system of drilling and anchoring mechanical arm

Abstract

The invention relates to an intelligent control system of a drilling and anchoring mechanical arm, and belongs to the technical field of intelligent control systems. Comprising a working space analysis module used for carrying out working space analysis on the drilling and anchoring mechanical arm; the visual positioning module is used for collecting a real-time operation image of the drilling and anchoring mechanical arm and analyzing the real-time operation image; the rough control module is used for guiding the drilling and anchoring mechanical arm to approach the anchor hole by taking the three-dimensional coordinate of the anchor hole in the mechanical arm coordinate system as a control target; the accurate control module is used for obtaining anchor hole expected features and anchor hole real-time features when the anchor hole pose depth is smaller than a preset threshold value, the anchor hole expected features serve as control targets, anchor hole feature deviation between the anchor hole real-time features and the anchor hole expected features is attenuated to 0 through a visual servo controller, and the anchor hole pose depth is obtained. And the drilling and anchoring mechanical arm is guided to reach the expected drilling and anchoring working position. The problem of control errors caused by insufficient visual positioning precision is solved, the control time is shortened, and the control efficiency, the supporting efficiency and the intelligent level are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent control systems, and in particular to an intelligent control system for a drilling and anchoring robotic arm. Background Art

[0002] Drilling and anchoring manipulators are used to perform anchoring support tasks. A drilling rig and other support equipment are mounted on the end of the manipulator, which then accurately moves the equipment to its working position. Automated support programs then control the equipment to complete the drilling and anchoring task. Currently, support equipment has a high degree of automation and can autonomously perform support tasks in fixed positions. However, the technology for manipulators to autonomously find their working positions has been relatively slow to develop.

[0003] Since the object of the drilling and anchor support operation is the anchor hole, most of the existing technologies use visual positioning technology to solve the anchor hole posture, take this position as the control target, perform trajectory planning on the drilling and anchor robot arm to obtain a reasonable trajectory for the drilling and anchor robot arm to reach this posture, and then use the trajectory tracking control method to control the drilling and anchor robot arm to reach this position according to the trajectory.

[0004] However, this method is limited by the following: 1. The anchor hole pose obtained by visual positioning technology is used as the target, which is significantly affected by the visual positioning technology, easily leading to large positioning errors and affecting control accuracy. 2. The entire process of anchor hole pose solution and manipulator pose control is required. Under high-precision control objectives, the computational complexity is large, the control time is long, and the support efficiency is low. 3. As it integrates multiple technologies, the connectivity between the various technologies is poor, resulting in a low level of intelligent operation of the drilling and anchoring manipulator. Summary of the Invention

[0005] To solve the above technical problems, the present invention provides an intelligent control system for a drilling and anchoring robot arm. The technical solution of the present invention is as follows: An intelligent control system for a drilling and anchoring robot arm, comprising a workspace analysis module, a visual positioning module, a rough control module, and a precise control module; The workspace analysis module is used to perform workspace analysis on the drilling and anchoring manipulator arm according to the pre-stored DH model of the drilling and anchoring manipulator arm, determine the working range of the drilling and anchoring manipulator arm, and judge whether the anchor hole is within the working range of the drilling and anchoring manipulator arm through the three-dimensional coordinates of the anchor hole in the manipulator arm coordinate system fed back by the visual positioning module. If the anchor hole is not within the working range of the drilling and anchoring manipulator arm, the drilling and anchoring manipulator arm body is controlled to move until the anchor hole enters the working range of the drilling and anchoring manipulator arm; if the anchor hole is within the working range of the drilling and anchoring manipulator arm, the workspace analysis module stops working; The visual positioning module is used to collect and analyze the real-time operation images of the drilling and anchoring robot arm to obtain the three-dimensional coordinates of the anchor hole in the robot arm coordinate system, and then feed the three-dimensional coordinates of the anchor hole in the robot arm coordinate system back to the workspace analysis module and the rough control module; The coarse control module is used to: use the three-dimensional coordinates of the anchor hole in the robot arm coordinate system as the control target, use the trajectory planning algorithm to determine the optimal trajectory of the drilling and anchoring robot arm, guide the drilling and anchoring robot arm to approach the anchor hole according to the optimal trajectory, and stop working when it is determined that the anchor hole posture depth is less than a preset threshold; The precise control module is used to obtain the expected anchor hole features and real-time anchor hole features when the anchor hole posture depth is less than a preset threshold. Taking the expected anchor hole features as the control target, the visual servo controller is used to attenuate the anchor hole feature deviation between the real-time anchor hole features and the expected anchor hole features to 0, so as to guide the drilling and anchoring robot arm to the desired drilling and anchoring working position.

[0006] Optionally, the visual positioning module includes a depth camera and an edge computer. The depth camera is installed on the drilling and anchoring robot arm, and the depth camera is electrically connected to the edge computer. When the visual positioning module analyzes the real-time operation image and obtains the three-dimensional coordinates of the anchor hole in the robot arm coordinate system, it includes: S21, the edge computer uses an adaptive histogram equalization algorithm to dehaze the real-time operation image; At S22, the edge computer uses the YOLOv5s model combined with the self-attention mechanism to extract the image features of the dehazed image, and identifies the anchor holes based on the image features to obtain the anchor hole image; S23, the edge computer processes the anchor hole image through morphological operations and Canny edge detection algorithm to obtain a set of anchor hole edge points; S24, the edge computer uses the Hough circle detection algorithm to fit the points in the anchor hole edge point set to obtain the anchor hole contour, and determines the pixel coordinates of the anchor hole center point based on the anchor hole contour; S25, the edge computer converts the pixel coordinates of the center point of the anchor hole into three-dimensional coordinates in the depth camera coordinate system, and calculates the three-dimensional coordinates of the anchor hole in the robotic arm coordinate system based on the relative position of the depth camera and the drilling and anchoring robotic arm.

[0007] Optionally, when guiding the drilling and anchoring robot arm to approach the anchor hole according to the optimal trajectory, the rough control module is specifically used to: obtain in real time the angle data and displacement data collected by the joint angle sensor installed on the rotating joint of the drilling and anchoring robot arm and the position sensor installed on the moving joint, and use the PID control algorithm to guide the drilling and anchoring robot arm to approach the anchor hole based on the angle data and displacement data.

[0008] Optionally, when guiding the drilling and anchoring robot arm to approach the anchor hole according to the optimal trajectory, the rough control module is also used to: determine whether the anchor hole posture depth is less than a preset threshold based on the three-dimensional coordinates of the anchor hole in the robot arm coordinate system, and stop working when the anchor hole posture depth is less than the preset threshold.

[0009] Optionally, the precise control module, when taking the desired anchor hole feature as the control target and using a visual servo controller to attenuate the anchor hole feature deviation between the real-time anchor hole feature and the desired anchor hole feature to zero, includes: S31, obtaining the real-time characteristics of the anchor hole; S32, calculating the image Jacobian matrix according to the real-time characteristics of the anchor hole; S33, establishing a first mapping relationship between the anchor hole real-time feature change speed and the depth camera motion speed based on the image Jacobian matrix; S34, determining the Jacobian matrix of the drilling and anchoring manipulator arm according to the DH model of the drilling and anchoring manipulator arm, and establishing a second mapping relationship between the depth camera motion speed and the drilling and anchoring manipulator arm joint speed according to the Jacobian matrix of the drilling and anchoring manipulator arm; S35, determining a third mapping relationship between the change speed of the anchor hole real-time characteristic parameter and the joint speed based on the first mapping relationship and the second mapping relationship; S37, establishing a fourth mapping relationship between the error change rate and the joint speed according to the anchor hole feature deviation between the real-time anchor hole feature and the expected anchor hole feature; S38, controlling the error change rate to be an exponential descent control rate, determining a visual servo controller according to the exponential descent control rate and the fourth mapping relationship, and using the visual servo controller to attenuate the anchor hole feature deviation between the real-time anchor hole feature and the expected anchor hole feature to 0.

[0010] Optionally, the S31 includes: S311, extracting an anchor hole image from the real-time operation image, and extracting anchor hole edge points from the anchor hole image; S312: Fit the edge points of the anchor hole into an ellipse, and use the ellipse feature as the real-time feature of the anchor hole.

[0011] Optionally, the S32 includes: calculating the image Jacobian matrix according to the real-time anchor hole feature using formula (1): (1); In formula (1), L E is the image Jacobian matrix, and the anchor hole real-time feature is A ,and A =[ A 1; A 2; A 3; A 4; A 5]; the ellipse is represented by ; ; ; ;in, r ( α , β , c ) is the parameter of the plane where the anchor hole is located in the world coordinate system. The plane where the anchor hole is located is expressed as , ( , , ) represents the three-dimensional coordinates of the center point of the anchor hole.

[0012] Optionally, the first mapping relationship is expressed as: ;in, is the real-time characteristic change speed of the anchor hole, is the depth camera motion speed, is the image Jacobian matrix; The second mapping relationship is expressed as ;in, is the Jacobian matrix of the drilling and anchoring robot arm, is the joint velocity; The third mapping relationship is expressed as: ; Wherein, + represents the pseudo-inverse matrix; The fourth mapping relationship is expressed as: ;in, , A d is the expected feature of the anchor hole, A It is the real-time feature of anchor hole; The exponentially decreasing control rate is expressed as: ;in, is the drop coefficient; The visual servo controller is expressed as .

[0013] Optionally, the anchor hole real-time feature A =[ A 1; A 2; A 3; A 4; A 5; x ; y ], x and y Respectively represent the horizontal and vertical coordinates of the center point of the ellipse; the image Jacobian matrix is L =[ L E ; L D ] (2); In formula (2), Z is the depth of the anchor hole.

[0014] Optionally, the preset threshold is calculated by formula (3): (3); In formula (3), d is the actual diameter of the anchor hole, f is the focal length of the depth camera, z is the distance from the depth camera to the anchor hole, the resolution of the real-time operation image is W×H, S is the imaging area of ​​the anchor hole, and when S When it is equal to 0.2×W×H, the distance from the depth camera to the anchor hole calculated by formula (3) is used as the preset threshold, where W and H represent the pixel length and width of the depth camera, respectively.

[0015] All the above optional technical solutions can be combined arbitrarily, and the present invention does not provide detailed descriptions of the structures after each combination.

[0016] By means of the above solution, the beneficial effects of the present invention are as follows: The precise control module, based on real-time operational images, eliminates control errors caused by insufficient visual positioning accuracy in the drilling and anchoring manipulator's visual servo controller. By combining coarse and precise positioning modules, control time is reduced, improving both control and support efficiency. The workspace analysis module ensures intelligent and unmanned operation throughout the entire support process, enabling the intelligent control system to operate autonomously and continuously without human intervention.

[0017] The above description is only an overview of the technical solution of the present invention. In order to more clearly understand the technical means of the present invention and implement it according to the contents of the specification, the following is a detailed description of the preferred embodiments of the present invention with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 The figure is a schematic diagram of the structure of the intelligent control system of the drilling and anchoring robot arm provided in an embodiment of the present invention.

[0019] Figure 2 The present invention provides an intelligent control system for an anchor drilling robot arm. DETAILED DESCRIPTION

[0020] The following embodiments of the present invention are described in further detail with reference to the accompanying drawings and examples. The following examples are used to illustrate the present invention but are not intended to limit the scope of the present invention.

[0021] like Figure 1 and Figure 2 As shown, the intelligent control system of the drilling and anchoring robot arm provided by the embodiment of the present invention includes a workspace analysis module, a visual positioning module, a rough control module and a precise control module; The workspace analysis module is used to perform workspace analysis on the drilling and anchoring manipulator arm according to the pre-stored DH model of the drilling and anchoring manipulator arm, determine the working range of the drilling and anchoring manipulator arm, and judge whether the anchor hole is within the working range of the drilling and anchoring manipulator arm through the three-dimensional coordinates of the anchor hole in the manipulator arm coordinate system fed back by the visual positioning module. If the anchor hole is not within the working range of the drilling and anchoring manipulator arm, the drilling and anchoring manipulator arm body is controlled to move until the anchor hole enters the working range of the drilling and anchoring manipulator arm; if the anchor hole is within the working range of the drilling and anchoring manipulator arm, the workspace analysis module stops working; The visual positioning module is used to collect and analyze the real-time operation images of the drilling and anchoring robot arm to obtain the three-dimensional coordinates of the anchor hole in the robot arm coordinate system, and then feed the three-dimensional coordinates of the anchor hole in the robot arm coordinate system back to the workspace analysis module and the rough control module; The coarse control module is used to: use the three-dimensional coordinates of the anchor hole in the robot arm coordinate system as the control target, use the trajectory planning algorithm to determine the optimal trajectory of the drilling and anchoring robot arm, guide the drilling and anchoring robot arm to approach the anchor hole according to the optimal trajectory, and stop working when it is determined that the anchor hole posture depth is less than a preset threshold; The precise control module is used to obtain the expected anchor hole features and real-time anchor hole features when the anchor hole posture depth is less than a preset threshold. Taking the expected anchor hole features as the control target, the visual servo controller is used to attenuate the anchor hole feature deviation between the real-time anchor hole features and the expected anchor hole features to 0, so as to guide the drilling and anchoring robot arm to the desired drilling and anchoring working position.

[0022] Specifically, the workspace analysis module is primarily run by an edge computer. The visual positioning module includes a depth camera, a white ring light source, and an edge computer. The depth camera is electrically connected to the edge computer. The depth camera and the white ring light source are fixed to the drilling and anchoring manipulator arm to capture real-time operating images of the drilling and anchoring manipulator arm. The white ring light source provides fill light for the depth camera. The depth camera captures real-time operating images of the drilling and anchoring manipulator arm and transmits them to the edge computer for analysis. The edge computer is installed in the tunnel or on the drilling and anchoring equipment where the drilling and anchoring manipulator arm is located.

[0023] The DH model of the drilling and anchoring robot arm is a model that describes the geometric relationship between the joints and links of the drilling and anchoring robot arm.

[0024] The rough control module uses the trajectory planning algorithm to determine the optimal trajectory of the drilling and anchoring manipulator, which is achieved through the existing path planning algorithm. The embodiment of the present invention will not elaborate on this. The optimal trajectory can be determined in combination with the obstacle avoidance technology of the drilling and anchoring manipulator.

[0025] The core of the coarse control module's control process lies in finding a roughly reasonable trajectory within the complex underground tunnel environment, combining it with the drill and anchor manipulator's obstacle avoidance technology to plan a safe and efficient motion trajectory, and effectively tracking and controlling this trajectory to prevent the drill and anchor manipulator from engaging in dangerous movements such as interference and collisions during operation in the complex tunnel environment. At the same time, since this part is coarse positioning control, the control results are not considered final, so the control requirements are not strict, effectively avoiding control errors caused by visual positioning errors and trajectory tracking control errors. At the same time, the visual positioning module participates in this process throughout. When the anchor hole posture depth is less than the preset threshold, the coarse control module stops working and the precise positioning module begins working.

[0026] The precise control module employs visual servo control based on the real-time anchor hole features, avoiding control errors caused by visual positioning. The intelligent control system requires manual positioning beforehand to move the drilling and anchoring robot arm to the correct drilling and anchoring working position. A depth camera then records the desired image at that time. Visual processing technology is then used to obtain the anchor hole features from this desired image, which serve as the desired anchor hole features. During the specific control process, this component obtains the real-time anchor hole features from the real-time working image. Using the desired anchor hole features as the control target, a visual servo controller is used to reduce the deviation between the real-time and desired anchor hole features to zero, thereby guiding the drilling and anchoring robot arm to the desired drilling and anchoring working position.

[0027] In a specific embodiment, when the visual positioning module analyzes the real-time operation image to obtain the three-dimensional coordinates of the anchor hole in the robot arm coordinate system, the following steps S21 to S24 are included: S21, the edge computer uses an adaptive histogram equalization algorithm to dehaze the real-time operation image.

[0028] Among them, the real-time operation image is the working environment image of the drilling and anchoring robot arm; through dehazing processing, the image quality of the real-time operation image can be improved.

[0029] At S22, the edge computer uses the YOLOv5s model combined with the self-attention mechanism to extract the image features of the dehazed image, and identifies the anchor holes based on the image features to obtain the anchor hole image; Specifically, the YOLOv5s model combined with the self-attention mechanism takes the dehazed image as input and outputs the anchor hole image identified from the dehazed image. The anchor hole image includes the anchor hole and the background.

[0030] S23, the edge computer processes the anchor hole image through morphological operations and Canny edge detection algorithm to obtain a set of anchor hole edge points.

[0031] Specifically, the anchor hole image can be segmented into anchor holes and background through morphological operations. The edge points corresponding to the anchor holes can be determined through the Canny edge detection algorithm, and all edge points constitute the anchor hole edge point set.

[0032] S24, the edge computer uses the Hough circle detection algorithm to fit the points in the anchor hole edge point set to obtain the anchor hole contour, and determines the pixel coordinates of the anchor hole center point based on the anchor hole contour.

[0033] S25, the edge computer converts the pixel coordinates of the center point of the anchor hole into three-dimensional coordinates in the depth camera coordinate system, and calculates the three-dimensional coordinates of the anchor hole in the robotic arm coordinate system based on the relative position of the depth camera and the drilling and anchoring robotic arm.

[0034] Among them, the three-dimensional coordinates of the anchor hole in the robot arm coordinate system are used in the workspace analysis module and the rough control module, and serve as the boundary basis for whether the rough control module and the precise positioning module are working or not.

[0035] In a specific embodiment, when guiding the drilling and anchoring robot arm to approach the anchor hole according to the optimal trajectory, the rough control module is specifically used to: obtain in real time the angle data and displacement data collected by the joint angle sensor installed on the rotating joint of the drilling and anchoring robot arm and the position sensor installed on the moving joint, and use the PID control algorithm to guide the drilling and anchoring robot arm to approach the anchor hole based on the angle data and displacement data.

[0036] In a specific embodiment, when the rough control module guides the drilling and anchoring robot arm to approach the anchor hole according to the optimal trajectory, it is also used to: determine whether the anchor hole posture depth is less than a preset threshold based on the three-dimensional coordinates of the anchor hole in the robot arm coordinate system, and stop working when the anchor hole posture depth is less than the preset threshold.

[0037] In one specific embodiment, the preset threshold is designed based on the appearance of the anchor hole in the real-time working image. When the depth camera is far from the anchor hole, the anchor hole image is small, the various parameters are not clearly visible, and the fitting accuracy of the anchor hole's real-time features is low, making it unsuitable for visual servo control based on the anchor hole image features. Therefore, this embodiment of the present invention uses a coarse control module to control the anchor drilling robot arm. When the depth camera is closer to the anchor hole, the anchor hole appears more clearly in the real-time working image, and the fitting accuracy of the anchor hole's real-time features is higher. Using a visual servo control method based on the anchor hole's real-time features can ensure accurate anchor hole control. Therefore, the preset threshold should be designed to ensure that the anchor hole image is sufficiently distinct, while not being too large, causing the anchor hole to fall out of the depth camera's field of view or be incompletely displayed. To ensure the effectiveness of the visual servo controller, this embodiment of the present invention selects an anchor hole area of ​​1 / 5 of the real-time working image area as the basis for switching control modes. In this case, the anchor hole image is not too small to affect fitting accuracy, while also preventing the anchor hole from falling out of the depth camera's field of view.

[0038] Based on the above, the preset threshold is calculated by formula (3): (3); In formula (3), d is the actual diameter of the anchor hole, f is the focal length of the depth camera, z is the distance from the depth camera to the anchor hole, the resolution of the real-time operation image is W×H, S is the imaging area of ​​the anchor hole, and when S When it is equal to 0.2×W×H, the distance from the depth camera to the anchor hole calculated by formula (3) is used as the preset threshold, where W and H represent the pixel length and width of the depth camera, respectively.

[0039] For example, assuming the resolution of the depth camera is 1920 × 1080 pixels, and the area of ​​the anchor hole image in the real-time operation image occupies 1 / 5 of the area of ​​the real-time operation image, that is, the imaging area of ​​the anchor hole image in the real-time operation image is: ; Assume the focal length of the depth camera is f =50 mm, actual diameter of the anchor hole d is 50 mm, and the above formula (3) is used to obtain , the solution is z=158.1mm, that is, the preset threshold is 158.1mm.

[0040] In a specific embodiment, the precise control module, taking the desired anchor hole feature as the control target and using the visual servo controller to attenuate the anchor hole feature deviation between the real-time anchor hole feature and the desired anchor hole feature to zero, includes the following steps S31 to S38: S31, obtaining the real-time features of the anchor hole.

[0041] Specifically, the S31 includes: S311, extracting an anchor hole image from a real-time operation image, and extracting anchor hole edge points from the anchor hole image; S312, fitting the anchor hole edge points into an ellipse, and using the ellipse feature as the anchor hole real-time feature.

[0042] More specifically, steps S311 and S312 include image enhancement, image grayscaling, edge detection, noise reduction, Hough circle fitting, and ellipse parameter fitting on the real-time operation image. Through image enhancement, image grayscaling, and edge detection, edge information is extracted from the real-time operation image captured by the depth camera in the tunnel. Noise reduction and Hough circle fitting are then performed to further retain the anchor hole edge points in the edge detection results, while removing all other elements. At this point, only the anchor hole edge points remain in the image. Finally, an ellipse is fitted to these anchor hole edge points using the least squares method to obtain the real-time anchor hole features of the real-time operation image. These real-time anchor hole features are represented by key points within the ellipse.

[0043] The advantage of using the ellipse as the real-time feature of the anchor hole is that the geometric points of the anchor hole edge points are used as features, and the control task can still be performed stably even if the workpiece is blocked or the rest of the image is not fully displayed.

[0044] S32, calculating the image Jacobian matrix according to the real-time features of the anchor hole.

[0045] Exemplarily, the S32 includes: calculating the image Jacobian matrix according to the real-time anchor hole feature by formula (1): (1);

[0046] In formula (1), L E is the image Jacobian matrix, and the anchor hole real-time feature is A ,and A =[ A 1; A 2; A 3; A 4; A 5]; the ellipse is represented by ; ;

[0047] ; ; in, r ( α , β , c ) is the parameter of the plane where the anchor hole is located in the world coordinate system. The plane where the anchor hole is located is expressed as , ( , , ) represents the three-dimensional coordinates of the center point of the anchor hole.

[0048] Specifically determining α 、 β and c When Rewritten as: The method for determining the plane is as follows: select three non-collinear anchor hole pixel points from the anchor hole image, where the center point is P1, read the depth Zi corresponding to each pixel point, and use formula (4) to determine the three-dimensional coordinates Pi of these three points ( X i , Y i , Z i ): (4); In formula (4), ( u i , v i ) represents the coordinate of pixel i in the image coordinate system, ( u 0 , v 0 ) represents the coordinate origin of the image coordinate system.

[0049] Construct two vectors using these three points: and , the normal vector of the drilling plane is , after normalizing the normal vector, we get , calculated , and finally the plane equation is: , A 、 B and C The values ​​of α 、 β and c The values ​​are the same.

[0050] S33: Establish a first mapping relationship between the anchor hole real-time feature change speed and the depth camera motion speed based on the image Jacobian matrix.

[0051] The first mapping relationship is expressed as: ;in, is the real-time characteristic change speed of the anchor hole, is the depth camera motion speed, is the image Jacobian matrix.

[0052] S34, determining the Jacobian matrix of the drilling and anchoring robot arm according to the DH model of the drilling and anchoring robot arm, and establishing a second mapping relationship between the depth camera motion speed and the drilling and anchoring robot arm joint speed according to the Jacobian matrix of the drilling and anchoring robot arm.

[0053] Among them, the second mapping relationship is expressed as ;in, is the Jacobian matrix of the drilling and anchoring robot arm, is the joint velocity.

[0054] S35 , determining a third mapping relationship between the anchor hole real-time characteristic parameter change speed and the joint speed according to the first mapping relationship and the second mapping relationship.

[0055] The third mapping relationship is expressed as: ; where + represents the pseudo-inverse matrix.

[0056] S37 , establishing a fourth mapping relationship between the error change rate and the joint speed according to the anchor hole feature deviation between the real-time anchor hole feature and the expected anchor hole feature.

[0057] The fourth mapping relationship is expressed as: ; , A d is the expected feature of the anchor hole, A It is the real-time feature of anchor hole.

[0058] S38, controlling the error change rate to be an exponential descent control rate, determining a visual servo controller according to the exponential descent control rate and the fourth mapping relationship, and using the visual servo controller to attenuate the anchor hole feature deviation between the real-time anchor hole feature and the expected anchor hole feature to 0.

[0059] Among them, the exponential decline control rate is expressed as: ;in, is the reduction coefficient, due to A d is a constant, so ; The visual servo controller is expressed as .

[0060] In summary, the embodiment of the present invention calculates the image Jacobian matrix based on the real-time anchor hole feature parameters. The deviation between the real-time anchor hole feature and the desired anchor hole feature is used as the control error. This control error is then converted into the correct motion of the depth camera using the image Jacobian matrix to reduce the control error. The Jacobian matrix of the drilling and anchoring manipulator is then used to convert the motion of the depth camera deployed at the end of the manipulator into the motion of the manipulator joint. The manipulator motion drives the depth camera motion, and control is completed when the deviation of the anchor hole feature decays to 0.

[0061] Furthermore, in order to ensure the stability of visual servo control, the embodiment of the present invention can also increase the coordinates of the ellipse center point ( x , y ) as an auxiliary feature, the anchor hole real-time feature A =[ A 1; A 2; A3; A 4; A 5; x ; y ], x and y Respectively represent the horizontal and vertical coordinates of the center point of the ellipse; the image Jacobian matrix is L =[ L E ; L D ] (2); In formula (2), Z is the depth of the anchor hole.

[0062] On this basis, the visual servo controller is expressed as .

[0063] The intelligent control system of the drilling and anchoring manipulator provided in the embodiment of the present invention can autonomously identify the anchor hole posture, adjust the body position, and autonomously control the drilling and anchoring manipulator to move to the accurate working position through the drilling and anchoring manipulator. At the same time, by combining the rough control module with the precise control module, the final control result is effectively avoided from being affected by the visual positioning accuracy of the anchor hole. While ensuring the control accuracy and stability, the operating efficiency of the autonomous hole-finding control of the drilling and anchoring manipulator is effectively improved. At the same time, the rough control module can be combined with the obstacle avoidance function of the manipulator during operation to reduce the possibility of collision of the drilling and anchoring manipulator during the control process. One-time DH modeling and anchor hole expected feature acquisition of the drilling and anchoring manipulator can ensure that the drilling and anchoring manipulator can perform multiple unmanned autonomous support operations in coal mine tunnels.

[0064] The intelligent control system of the drilling and anchoring manipulator provided by the embodiment of the present invention has the functions of the drilling and anchoring manipulator automatically identifying the anchor hole and autonomously searching for the hole, realizing the automation and intelligence of the hole-finding control throughout the entire process. Equipped with support equipment, it can autonomously complete the support operation without human intervention, providing a new idea for the unmanned and less-manned operation of coal mine tunnels. At the same time, it combines the advantages of position-based and image-based visual servoing, ensuring the accuracy of automatic hole-finding control while effectively reducing the control time. The elliptical geometric image of the anchor hole under the depth camera is used as the real-time feature of the anchor hole, which greatly improves the reliability of the image-based visual servo controller under harsh conditions such as occlusion and low illumination in coal mine tunnels.

[0065] In summary, the intelligent control system for the drilling and anchoring manipulator provided by the embodiment of the present invention first uses a precise control module to solve the control error caused by insufficient positioning accuracy based on the visual servo controller of the drilling and anchoring manipulator based on the real-time operation image. At the same time, the conversion of the real-time operation image into control instructions improves the coupling between visual technology and control technology. Then, a method combining a coarse positioning module with a precise positioning module is used to reduce the control time and improve the control efficiency. Finally, it is equipped with a workspace analysis module to ensure the intelligence and unmanned operation of the entire support process, allowing the intelligent control system to operate autonomously and continuously without human intervention.

[0066] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the technical principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. An intelligent control system for a drilling and anchoring robot arm, characterized in that: It includes workspace analysis module, visual positioning module, rough control module and precise control module; The workspace analysis module is used to perform workspace analysis on the drilling and anchoring manipulator arm according to the pre-stored DH model of the drilling and anchoring manipulator arm, determine the working range of the drilling and anchoring manipulator arm, and judge whether the anchor hole is within the working range of the drilling and anchoring manipulator arm through the three-dimensional coordinates of the anchor hole in the manipulator arm coordinate system fed back by the visual positioning module. If the anchor hole is not within the working range of the drilling and anchoring manipulator arm, the drilling and anchoring manipulator arm body is controlled to move until the anchor hole enters the working range of the drilling and anchoring manipulator arm; if the anchor hole is within the working range of the drilling and anchoring manipulator arm, the workspace analysis module stops working; The visual positioning module is used to collect and analyze the real-time operation images of the drilling and anchoring robot arm to obtain the three-dimensional coordinates of the anchor hole in the robot arm coordinate system, and then feed the three-dimensional coordinates of the anchor hole in the robot arm coordinate system back to the workspace analysis module and the rough control module; The coarse control module is used to: use the three-dimensional coordinates of the anchor hole in the robot arm coordinate system as the control target, use the trajectory planning algorithm to determine the optimal trajectory of the drilling and anchoring robot arm, guide the drilling and anchoring robot arm to approach the anchor hole according to the optimal trajectory, and stop working when it is determined that the anchor hole posture depth is less than a preset threshold; The precise control module is used to obtain the expected anchor hole features and real-time anchor hole features when the anchor hole posture depth is less than a preset threshold. Taking the expected anchor hole features as the control target, the visual servo controller is used to attenuate the anchor hole feature deviation between the real-time anchor hole features and the expected anchor hole features to 0, so as to guide the drilling and anchoring robot arm to the desired drilling and anchoring working position.

2. The intelligent control system of the anchor drilling robot arm according to claim 1, characterized in that: The visual positioning module includes a depth camera and an edge computer. The depth camera is installed on the drilling and anchoring robot arm. The depth camera is electrically connected to the edge computer. When the visual positioning module analyzes the real-time operation image and obtains the three-dimensional coordinates of the anchor hole in the robot arm coordinate system, it includes: S21, the edge computer uses an adaptive histogram equalization algorithm to dehaze the real-time operation image; At S22, the edge computer uses the YOLOv5s model combined with the self-attention mechanism to extract the image features of the dehazed image, and identifies the anchor holes based on the image features to obtain the anchor hole image; S23, the edge computer processes the anchor hole image through morphological operations and Canny edge detection algorithm to obtain a set of anchor hole edge points; S24, the edge computer uses the Hough circle detection algorithm to fit the points in the anchor hole edge point set to obtain the anchor hole contour, and determines the pixel coordinates of the anchor hole center point based on the anchor hole contour; S25, the edge computer converts the pixel coordinates of the center point of the anchor hole into three-dimensional coordinates in the depth camera coordinate system, and calculates the three-dimensional coordinates of the anchor hole in the robotic arm coordinate system based on the relative position of the depth camera and the drilling and anchoring robotic arm.

3. The intelligent control system of the anchor drilling robot arm according to claim 1, characterized in that: When guiding the drilling and anchoring robot arm to approach the anchor hole according to the optimal trajectory, the rough control module is specifically used to: obtain in real time the angle data and displacement data collected by the joint angle sensor installed on the rotating joint of the drilling and anchoring robot arm and the position sensor installed on the moving joint, and guide the drilling and anchoring robot arm to approach the anchor hole using a PID control algorithm based on the angle data and displacement data.

4. The intelligent control system of the anchor drilling robot arm according to claim 1, characterized in that: When guiding the drilling and anchoring robot arm to approach the anchor hole according to the optimal trajectory, the rough control module is also used to: determine whether the anchor hole posture depth is less than a preset threshold based on the three-dimensional coordinates of the anchor hole in the robot arm coordinate system, and stop working when the anchor hole posture depth is less than the preset threshold.

5. The intelligent control system of the anchor drilling robot arm according to claim 1, characterized in that: The precise control module, taking the desired anchor hole feature as the control target and using the visual servo controller to attenuate the anchor hole feature deviation between the real-time anchor hole feature and the desired anchor hole feature to zero, includes: S31, obtaining the real-time characteristics of the anchor hole; S32, calculating the image Jacobian matrix according to the real-time characteristics of the anchor hole; S33, establishing a first mapping relationship between the anchor hole real-time feature change speed and the depth camera motion speed based on the image Jacobian matrix; S34, determining the Jacobian matrix of the drilling and anchoring manipulator arm according to the DH model of the drilling and anchoring manipulator arm, and establishing a second mapping relationship between the depth camera motion speed and the drilling and anchoring manipulator arm joint speed according to the Jacobian matrix of the drilling and anchoring manipulator arm; S35, determining a third mapping relationship between the change speed of the anchor hole real-time characteristic parameter and the joint speed based on the first mapping relationship and the second mapping relationship; S37, establishing a fourth mapping relationship between the error change rate and the joint speed according to the anchor hole feature deviation between the real-time anchor hole feature and the expected anchor hole feature; S38, controlling the error change rate to be an exponential descent control rate, determining a visual servo controller according to the exponential descent control rate and the fourth mapping relationship, and using the visual servo controller to attenuate the anchor hole feature deviation between the real-time anchor hole feature and the expected anchor hole feature to 0.

6. The intelligent control system of the anchor drilling robot arm according to claim 5, characterized in that: The S31 includes: S311, extracting an anchor hole image from the real-time operation image, and extracting anchor hole edge points from the anchor hole image; S312: Fit the edge points of the anchor hole into an ellipse, and use the ellipse feature as the real-time feature of the anchor hole.

7. The intelligent control system of the anchor drilling robot arm according to claim 6, characterized in that: The S32 includes: calculating the image Jacobian matrix according to the real-time anchor hole feature by formula (1): （1）； In formula (1), L E is the image Jacobian matrix, and the anchor hole real-time feature is A ,and A =[ A 1; A 2; A 3; A 4; A 5]; the ellipse is represented by ; ; ; ;in, ρ ( α , β , γ ) is the parameter of the plane where the anchor hole is located in the world coordinate system. The plane where the anchor hole is located is expressed as , ( , , ) represents the three-dimensional coordinates of the center point of the anchor hole.

8. The intelligent control system of the anchor drilling robot arm according to claim 5, characterized in that: The first mapping relationship is expressed as: ;in, is the real-time characteristic change speed of the anchor hole, is the depth camera motion speed, is the image Jacobian matrix; The second mapping relationship is expressed as ;in, is the Jacobian matrix of the drilling and anchoring robot arm, is the joint velocity; The third mapping relationship is expressed as: ; Wherein, + represents the pseudo-inverse matrix; The fourth mapping relationship is expressed as: ;in, , A d is the expected feature of the anchor hole, A It is the real-time feature of anchor hole; The exponentially decreasing control rate is expressed as: ;in, is the drop coefficient; The visual servo controller is expressed as .

9. The intelligent control system of the anchor drilling robot arm according to claim 7, characterized in that: The anchor hole real-time feature A =[ A 1; A 2; A 3; A 4; A 5; x ; y ], x and y Respectively represent the horizontal and vertical coordinates of the center point of the ellipse; the image Jacobian matrix is L =[ L E ; L D ] (2); in formula (2), Z is the depth of the anchor hole.

10. The intelligent control system of the anchor drilling robot arm according to claim 1, characterized in that: The preset threshold is calculated by formula (3): （3）； In formula (3), d is the actual diameter of the anchor hole, f is the focal length of the depth camera, z is the distance from the depth camera to the anchor hole, the resolution of the real-time operation image is W×H, S is the imaging area of ​​the anchor hole, and when S When it is equal to 0.2×W×H, the distance from the depth camera to the anchor hole calculated by formula (3) is used as the preset threshold, where W and H represent the pixel length and width of the depth camera, respectively.

Images