Intelligent control system of drilling anchor mechanical arm

Abstract

The present application relates to a kind of intelligent control system of drill anchor mechanical arm, belong to intelligent control system technical field.It includes: workspace analysis module is used to carry out workspace analysis to drill anchor mechanical arm;Visual positioning module is used to collect the real-time operation image of drill anchor mechanical arm, and the real-time operation image is analyzed;Rough control module is used to with the three-dimensional coordinates of anchor hole in the coordinate system of mechanical arm as control target, guide drill anchor mechanical arm to approach anchor hole;Accurate control module is used when anchor hole pose depth is less than pre-set threshold, obtains anchor hole expected feature and anchor hole real-time feature, with anchor hole expected feature as control target, using visual servo controller, anchor hole feature deviation between anchor hole real-time feature and anchor hole expected feature is attenuated to 0, to guide drill anchor mechanical arm to reach the expected drill anchor working position.The present application solves the control error caused by the insufficient visual positioning accuracy, reduces the control time length, improves control efficiency, support efficiency and intelligent level.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent control system, and particularly relates to an intelligent control system of a drill-anchor mechanical arm. BACKGROUND

[0002] The drill-anchor mechanical arm is used for performing an anchor supporting task, and a supporting device such as a drill is installed at the end of the mechanical arm, the supporting device is accurately operated to a working position by the mechanical arm, and then an automatic supporting program is used to control the supporting device to complete the drill-anchor supporting task. At present, the supporting device has a high degree of automation, and can complete the autonomous supporting task at a fixed position, while the technology of autonomously operating the mechanical arm to find the working position develops slowly.

[0003] Since the drill-anchor supporting operation object is an anchor hole, most of the existing technologies solve the anchor hole pose by using visual positioning technology, take the position as a control target, plan a reasonable trajectory of the drill-anchor mechanical arm to reach the pose, and then use a trajectory tracking control method to control the drill-anchor mechanical arm to reach the position according to the trajectory.

[0004] However, the limitations of this method are as follows: 1. The anchor hole pose solved by the visual positioning technology is taken as a target, and is greatly affected by the visual positioning technology, which easily brings a large positioning error and affects the control precision. 2. The anchor hole pose solving and the mechanical arm pose control are needed in the whole process, and under the high-precision control target, the operation amount is large, the control time is long, and the supporting efficiency is low. 3. It belongs to multi-technology fusion, and the connectivity between various technologies is poor, which leads to a low intelligent degree of the drill-anchor mechanical arm operation. SUMMARY

[0005] To solve the above technical problems, the present application provides an intelligent control system of a drill-anchor mechanical arm. The technical scheme of the present application is as follows:

[0006] An intelligent control system of a drill-anchor mechanical arm, comprising a working space analysis module, a visual positioning module, a rough control module and an accurate control module;

[0007] The working space analysis module is used for: performing working space analysis on the drill-anchor mechanical arm according to a pre-stored D-H model of the drill-anchor mechanical arm, determining the working range of the drill-anchor mechanical arm, and judging whether the anchor hole is in the working range of the drill-anchor mechanical arm through the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system fed back by the visual positioning module, if the anchor hole is not in the working range of the drill-anchor mechanical arm, controlling the drill-anchor mechanical arm body to run to the anchor hole to enter the working range of the drill-anchor mechanical arm, and if the anchor hole is in the working range of the drill-anchor mechanical arm, stopping the working of the working space analysis module;

[0008] The visual positioning module is configured to: collect a real-time working image of the anchor drilling mechanical arm, analyze the real-time working image, obtain three-dimensional coordinates of the anchor hole in a mechanical arm coordinate system, and feed back the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system to the working space analysis module and the rough control module;

[0009] The rough control module is configured to: take the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system as a control target, determine an optimal trajectory of the anchor drilling mechanical arm by using a trajectory planning algorithm, guide the anchor drilling mechanical arm to approach the anchor hole according to the optimal trajectory, and stop working when it is determined that the anchor hole pose depth is less than a preset threshold value;

[0010] The precise control module is configured to: when the anchor hole pose depth is less than the preset threshold value, obtain an anchor hole expected feature and an anchor hole real-time feature, take the anchor hole expected feature as a control target, and use a visual servo controller to attenuate an anchor hole feature deviation between the anchor hole real-time feature and the anchor hole expected feature to 0, so as to guide the anchor drilling mechanical arm to reach an expected anchor drilling working position.

[0011] Optionally, the visual positioning module comprises a depth camera and an edge computer, the depth camera is installed on the anchor drilling mechanical arm, the depth camera is electrically connected with the edge computer, and when the visual positioning module analyzes the real-time working image to obtain the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system, the visual positioning module comprises the following steps:

[0012] S21, the edge computer uses an adaptive histogram equalization algorithm to perform defogging processing on the real-time working image;

[0013] S22, the edge computer extracts image features of the defogged image by using a YOLOv5s model combined with a self-attention mechanism, identifies the anchor hole based on the image features, and obtains an anchor hole image;

[0014] S23, the edge computer processes the anchor hole image by morphological operation and a Canny edge detection algorithm to obtain a set of anchor hole edge points;

[0015] S24, the edge computer uses a Hough circle detection algorithm to fit the points in the set of anchor hole edge points to obtain an anchor hole contour, and determines pixel coordinates of an anchor hole center point according to the anchor hole contour;

[0016] S25, the edge computer converts the pixel coordinates of the anchor hole center point into three-dimensional coordinates in a depth camera coordinate system, and calculates the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system according to the relative position between the depth camera and the anchor drilling mechanical arm.

[0017] Optionally, the coarse control module is configured to, when guiding the anchor drilling manipulator to approach the anchor hole according to the optimal trajectory, acquire angle data and displacement data collected by a joint angle sensor installed on a rotating joint of the anchor drilling manipulator and a position sensor installed on a moving joint in real time, and guide the anchor drilling manipulator to approach the anchor hole by using a PID control algorithm according to the angle data and the displacement data.

[0018] Optionally, the coarse control module is further configured to, when guiding the anchor drilling manipulator to approach the anchor hole according to the optimal trajectory, determine whether a pose depth of the anchor hole is less than a preset threshold according to three-dimensional coordinates of the anchor hole in a manipulator coordinate system, and stop working when the pose depth of the anchor hole is less than the preset threshold.

[0019] Optionally, when the precise control module takes the expected feature of the anchor hole as a control target and adopts a visual servo controller to attenuate a feature deviation between the real-time feature of the anchor hole and the expected feature of the anchor hole to 0, the precise control module comprises the following steps:

[0020] S31, acquiring a real-time feature of an anchor hole;

[0021] S32, calculating an image Jacobian matrix according to the real-time feature of the anchor hole;

[0022] S33, establishing a first mapping relationship between a change speed of the real-time feature of the anchor hole and a motion speed of a depth camera based on the image Jacobian matrix;

[0023] S34, determining a Jacobian matrix of the anchor drilling manipulator according to a D-H model of the anchor drilling manipulator, and establishing a second mapping relationship between the motion speed of the depth camera and a joint speed of the anchor drilling manipulator according to the Jacobian matrix of the anchor drilling manipulator;

[0024] S35, determining a third mapping relationship between a parameter change speed of the real-time feature of the anchor hole and the joint speed according to the first mapping relationship and the second mapping relationship;

[0025] S36, establishing a fourth mapping relationship between an error change rate and the joint speed according to a feature deviation between the real-time feature of the anchor hole and the expected feature of the anchor hole;

[0026] S37, controlling the error change rate to be an exponential decline control rate, determining a visual servo controller according to the exponential decline control rate and the fourth mapping relationship, and adopting the visual servo controller to attenuate the feature deviation between the real-time feature of the anchor hole and the expected feature of the anchor hole to 0.

[0027] Optionally, the S31 comprises the following steps:

[0028] S311, extracting an anchor hole image from a real-time working image, and extracting anchor hole edge points from the anchor hole image;

[0029] S312, fitting the anchor hole edge points as an ellipse, and taking the ellipse feature as the anchor hole real-time feature.

[0030] Optionally, the S32 comprises: calculating the image Jacobian matrix according to the real-time anchor hole feature through formula (1):

[0031] (1);

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

[0033] ; ; ; wherein, p ( α , β , g ) are parameters of the anchor hole plane in the world coordinate system, the anchor hole plane is represented as , and , , ) represent the three-dimensional coordinates of the anchor hole center point.

[0034] Optionally, the first mapping relationship is represented as: ; wherein, is the anchor hole real-time feature change speed, is the depth camera motion speed, is the image Jacobian matrix;

[0035] The second mapping relationship is represented as ; wherein, is the drill anchor robot Jacobian matrix, is the joint speed;

[0036] The third mapping relationship is represented as: ; wherein, + represents the pseudo-inverse matrix;

[0037] The fourth mapping relationship is represented as: ; wherein, , A d is the anchor hole expected feature, A is the anchor hole real-time feature;

[0038] The exponential descending control rate is expressed as: ; wherein, is a descending coefficient;

[0039] The visual servoing controller is expressed as .

[0040] 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 ellipse center point; the image Jacobian matrix is L [ L E ; L D ]

[0041] (2);

[0042] In formula (2), Z is the depth of the anchor hole.

[0043] Optionally, the preset threshold is calculated by formula (3):

[0044] (3);

[0045] 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 WxH, S is the imaging area of the anchor hole, and when S is equal to 0.2xWxH, the distance from the depth camera to the anchor hole calculated by formula (3) is taken as the preset threshold, and W and H respectively represent the pixel length and width of the depth camera.

[0046] All the optional technical solutions described above can be combined arbitrarily, and the application does not perform detailed description on the structures after combination.

[0047] Through the above scheme, the application has the following beneficial effects:

[0048] The control error caused by insufficient visual positioning accuracy is solved by the accurate control module based on the visual servo controller of the drill-anchor mechanical arm according to the real-time working image. The control time is reduced and the control efficiency and the supporting efficiency are improved by the method of combining the coarse positioning module with the accurate positioning module. The intelligentization and unmannedization of the whole supporting process are ensured through the working space analysis module, so that the intelligent control system can work autonomously and continuously without manual intervention.

[0049] The above description is only a summary of the technical scheme of the present application. In order to make the technical means of the present application clearer and to implement the content of the description, the preferred embodiments of the present application are described in detail below with reference to the drawings. BRIEF DESCRIPTION OF DRAWINGS

[0050] Figure 1 FIG. 1 is a schematic structural diagram of an intelligent control system of a drill-anchor mechanical arm provided by an embodiment of the present application.

[0051] Figure 2 FIG. 2 is a work flow diagram of the intelligent control system of the drill-anchor mechanical arm provided by the embodiment of the present application. DETAILED DESCRIPTION

[0052] The specific embodiments of the present application are described in further detail below in combination with the drawings and the embodiments. The following embodiments are used to illustrate the present application, but are not used to limit the scope of the present application.

[0053] As shown in FIGS. 1 and 2, the intelligent control system of the drill-anchor mechanical arm provided by the embodiment of the present application includes a working space analysis module, a visual positioning module, a coarse control module and an accurate control module. Figure 1 and Figure 2 The working space analysis module is used to perform working space analysis on the drill-anchor mechanical arm according to a pre-stored D-H model of the drill-anchor mechanical arm, determine the working range of the drill-anchor mechanical arm, and judge whether the anchor hole is within the working range of the drill-anchor mechanical arm through the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system fed back by the visual positioning module. If the anchor hole is not within the working range of the drill-anchor mechanical arm, the drill-anchor mechanical arm body is controlled to run to the anchor hole to enter the working range of the drill-anchor mechanical arm. If the anchor hole is within the working range of the drill-anchor mechanical arm, the working space analysis module stops working.

[0054] The visual positioning module is used to collect real-time working images of the drill-anchor mechanical arm, analyze the real-time working images, obtain the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system, and feed back the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system to the working space analysis module and the coarse control module.

[0055] The visual positioning module is used to collect real-time working images of the drill-anchor mechanical arm, analyze the real-time working images, obtain the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system, and feed back the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system to the working space analysis module and the coarse control module.

[0056] The coarse control module is configured to: take the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system as a control target, determine an optimal trajectory of the anchor drilling mechanical arm by using a trajectory planning algorithm, guide the anchor drilling mechanical arm to approach the anchor hole according to the optimal trajectory, and stop working when it is determined that the anchor hole pose depth is less than a preset threshold value.

[0057] The precise control module is configured to: when the anchor hole pose depth is less than the preset threshold value, acquire an anchor hole expected feature and an anchor hole real-time feature, take the anchor hole expected feature as a control target, and use a visual servo controller to attenuate an anchor hole feature deviation between the anchor hole real-time feature and the anchor hole expected feature to 0, so as to guide the anchor drilling mechanical arm to reach an expected anchor drilling working position.

[0058] Specifically, the work space analysis module is mainly run by an edge computer. The visual positioning module includes a depth camera, a white ring-shaped light source and the edge computer, the depth camera is electrically connected with the edge computer, the depth camera and the white ring-shaped light source are fixed on the anchor drilling mechanical arm, are used for collecting real-time working images of the anchor drilling mechanical arm, and the white ring-shaped light source provides light compensation for the depth camera. The depth camera collects the real-time working images of the anchor drilling mechanical arm in real time, and transmits the real-time working images to the edge computer for analysis. The edge computer is installed in a roadway or on an anchor drilling device where the anchor drilling mechanical arm is located.

[0059] The D-H model of the anchor drilling mechanical arm is a model for describing the geometric relationship between each joint and connecting rod of the anchor drilling mechanical arm.

[0060] When the coarse control module determines the optimal trajectory of the anchor drilling mechanical arm by using the trajectory planning algorithm, the existing path planning algorithm is used, and details are not described in the embodiments of the present application. The optimal trajectory can be determined in combination with the anchor drilling mechanical arm obstacle avoidance technology.

[0061] The core of the control process of the coarse control module is to find a general reasonable trajectory in a complex roadway environment underground, plan a safe and efficient motion trajectory in combination with the anchor drilling mechanical arm obstacle avoidance technology, and effectively track and control the trajectory, so as to avoid dangerous actions such as interference and collision of the anchor drilling mechanical arm during operation in the complex roadway environment. At the same time, since this part is a coarse positioning control, the control result is not used as a final control result, and therefore the control result is not strictly required, so that the control error caused by the visual positioning error and the trajectory tracking control error can be effectively avoided. At the same time, the visual positioning module participates in this process throughout, the coarse control module stops working when the anchor hole pose depth is less than the preset threshold value, and the precise positioning module starts working.

[0062] The precise control module adopts visual servo control based on real-time features of anchor holes, avoiding control errors caused by visual positioning. The intelligent control system needs to be manually positioned in advance, running the drill anchor mechanical arm to the correct drill anchor working position, and recording the desired image at this time by the depth camera. The anchor hole features of the desired image are obtained through visual processing technology as the expected features of the anchor hole. In the specific control process, the real-time features of the anchor hole are obtained from the real-time working image, and the expected features of the anchor hole are taken as the control target. The visual servo controller is used to attenuate the anchor hole feature deviation between the real-time features of the anchor hole and the expected features of the anchor hole to 0, thereby guiding the drill anchor mechanical arm to reach the expected drill anchor working position.

[0063] In one specific embodiment, when analyzing the real-time working image to obtain the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system, the visual positioning module includes the following steps S21 to S24:

[0064] S21, the edge computer uses an adaptive histogram equalization algorithm to process the real-time working image.

[0065] The real-time working image is the working environment image of the drill anchor mechanical arm. Through the defogging processing, the image quality of the real-time working image can be improved.

[0066] S22, the edge computer uses a YOLOv5s model combined with a self-attention mechanism to extract image features of the defogged image, and identifies the anchor hole based on the image features to obtain an anchor hole image.

[0067] Specifically, the input of the YOLOv5s model combined with the self-attention mechanism is the defogged image, and the output is the anchor hole image identified from the defogged image. The anchor hole image includes the anchor hole and the background.

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

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

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

[0071] S25, the edge computer converts the pixel coordinates of the anchor hole center point into three-dimensional coordinates in the depth camera coordinate system, and calculates the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system according to the relative position of the depth camera and the drill anchor mechanical arm.

[0072] The three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system are used for the workspace analysis module and the coarse control module, and serve as a demarcation basis for whether the coarse control module works with the precise positioning module.

[0073] In one specific embodiment, when guiding the anchor drilling mechanical arm to approach the anchor hole according to the optimal trajectory, the coarse control module is specifically configured to: acquire angle data and displacement data collected by a joint angle sensor installed on a rotating joint of the anchor drilling mechanical arm and a position sensor installed on a moving joint in real time, and guide the anchor drilling mechanical arm to approach the anchor hole by using a PID control algorithm according to the angle data and the displacement data.

[0074] In one specific embodiment, when guiding the anchor drilling mechanical arm to approach the anchor hole according to the optimal trajectory, the coarse control module is further configured to: determine whether the anchor hole pose depth is less than a preset threshold according to the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system, and stop working when the anchor hole pose depth is less than the preset threshold.

[0075] In one specific embodiment, the preset threshold is designed according to the appearance of the anchor hole in the real-time working image. When the depth camera is far away from the anchor hole, the anchor hole image is small, the parameter performance of each part is not obvious, the fitting accuracy of the real-time feature of the anchor hole is low, and the visual servo control based on the anchor hole image feature is not suitable. Therefore, the embodiment of the present application uses the coarse control module to control the anchor drilling mechanical arm. When the depth camera is close to the anchor hole, the anchor hole in the real-time working image is more obvious, the fitting accuracy of the real-time feature of the anchor hole is higher, and the visual servo control method based on the real-time feature of the anchor hole can ensure the control accuracy of the anchor hole. Therefore, the design of the preset threshold should consider that the anchor hole image is obvious enough, and cannot be too large to cause the anchor hole to be out of the field of view of the depth camera or not to be completely displayed. In order to ensure the effectiveness of the visual servo controller, the embodiment of the present application selects 1 / 5 of the area of the anchor hole in the real-time working image as the basis for switching the control mode, so that the anchor hole image is not too small to affect the fitting accuracy, and the anchor hole is also avoided from being out of the field of view of the depth camera.

[0076] On the basis of the above, the preset threshold is calculated by formula (3):

[0077] (3);

[0078] 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 working image is WxH, S is the imaging area of the anchor hole, and when SWhen the distance is equal to 0.2*W*H, the distance from the depth camera to the anchor hole calculated by the formula (3) is taken as the preset threshold, and W and H represent the length and width of the pixels of the depth camera respectively.

[0079] For example, assuming that the resolution of the depth camera is 1920*1080 pixels, and the area of the anchor hole image in the real-time working image accounts for 1 / 5 of the area of the real-time working image, that is, the imaging area of the anchor hole image in the real-time working image is:

[0080] ;

[0081] Assuming that the focal length of the depth camera is 50 mm, and the actual diameter of the anchor hole is 50 mm, the formula (3) is used to obtain f , and the solution is z=158.1 mm, that is, the preset threshold is 158.1 mm. d

[0082] In one specific embodiment, when the anchor hole real-time feature deviation from the anchor hole expected feature is attenuated to 0 by using the visual servo controller with the anchor hole expected feature as the control target, the precise control module includes the following steps S31 to S37:

[0083] S31, obtaining the anchor hole real-time feature.

[0084] Specifically, the S31 includes: S311, extracting the anchor hole image from the real-time working image, and extracting the anchor hole edge point from the anchor hole image; S312, fitting the anchor hole edge point as an ellipse, and taking the ellipse feature as the anchor hole real-time feature.

[0085] More specifically, S311 and S312 include image enhancement, image graying, edge detection, noise reduction, Hough circle fitting, and ellipse parameter fitting on the real-time working image. The edge information in the real-time working image collected by the depth camera in the tunnel is extracted through image enhancement, image graying, and edge detection, and then the anchor hole edge points in the edge detection result are further retained through noise reduction and Hough circle fitting, and the remaining elements are removed. At this time, only the anchor hole edge points are in the image, and finally, the anchor hole edge points are fitted as an ellipse using the least square method to obtain the anchor hole real-time feature of the real-time working image. The anchor hole real-time feature is represented by the key points in the ellipse.

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

[0087] S32, calculating the image Jacobian matrix according to the anchor hole real-time feature.

[0088] ​​Exemplarily, the S32 comprises: calculating the image Jacobian matrix according to the real-time anchor hole features by formula (1):

[0089] (1);

[0090] In formula (1), L E is the image Jacobian matrix, the anchor hole real-time features are A , and A [ A 1; A 2; A 3; A 4; A 5]; ellipse is represented as ;

[0091] ;

[0092] ;

[0093] ;

[0094] wherein, p ( α , β , g ) are parameters of the plane where the anchor hole is located in the world coordinate system, the plane where the anchor hole is located is represented as , and , , represent three-dimensional coordinates of the center point of the anchor hole.

[0095] Specifically, when determining α , β and g , rewrite as: The determination method of the plane is as follows: from the anchor hole image, select three anchor hole pixel points which are not collinear, wherein the center point is P1, read the depth Zi corresponding to each pixel point, and determine the three-dimensional coordinates Pi of the three points by using formula (4): X i , Y i , Z i :

[0096] (4);

[0097] In formula (4), (x u i , v i ) represent the coordinates of the pixel point i in the image coordinate system, andu 0 , v 0 ) represents the coordinate origin of the image coordinate system.

[0098] Two vectors are constructed using the three points: and , and the normal vector of the plane where the drill hole is located is obtained as , and after unitizing the normal vector, we get , and the calculation is , and finally the plane equation is: , A , B and C The values of α , β and g g are the same as

[0099] S33, a first mapping relationship between the real-time feature change speed of the anchor hole and the depth camera motion speed is established based on the image Jacobian matrix.

[0100] Wherein, the first mapping relationship is expressed as: ; wherein, is the real-time feature change speed of the anchor hole, is the depth camera motion speed, is the image Jacobian matrix.

[0101] S34, the drill anchor robot D-H model is determined to determine the drill anchor robot Jacobian matrix, and the depth camera motion speed and the joint speed of the drill anchor robot are established according to the second mapping relationship of the drill anchor robot Jacobian matrix.

[0102] Wherein, the second mapping relationship is expressed as ; wherein, is the drill anchor robot Jacobian matrix, is the joint speed.

[0103] S35, the third mapping relationship between the real-time feature parameter change speed of the anchor hole and the joint speed is determined according to the first mapping relationship and the second mapping relationship.

[0104] Wherein, the third mapping relationship is expressed as: ; wherein, + represents the pseudo-inverse matrix.

[0105] S36, the fourth mapping relationship between the error change rate and the joint speed is established according to the anchor hole feature deviation between the real-time feature of the anchor hole and the expected feature of the anchor hole.

[0106] Wherein, the fourth mapping relationship is expressed as: ; ,A d For anchor hole desired features, A This represents the real-time characteristics of the anchor hole.

[0107] S37, the control error change rate is an exponentially decreasing control rate. The visual servo controller is determined based on the exponentially decreasing control rate and the fourth mapping relationship. The visual servo controller is used to reduce the anchor hole feature deviation between the real-time anchor hole feature and the desired anchor hole feature to 0.

[0108] The exponential decline control rate is expressed as: ;in, The decreasing coefficient is due to A d Since it is a constant, therefore, ;

[0109] Visual servo controller is represented as .

[0110] In summary, this embodiment of the invention calculates the image Jacobian matrix based on the real-time feature parameters of the anchor hole, then uses the anchor hole feature deviation between the real-time feature and the expected feature as the control error. This control error is then converted into the correct motion of the depth camera using the image Jacobian matrix, thereby reducing the control error. Next, the depth camera motion deployed at the end of the drilling and anchoring manipulator is converted into manipulator joint motion using the Jacobian matrix of the drilling and anchoring manipulator. The manipulator motion drives the depth camera motion, and control is completed when the anchor hole feature deviation decays to zero.

[0111] Furthermore, to ensure stable visual servo control, embodiments of the present invention may also add the coordinates of the ellipse's center point ( x , y As an auxiliary feature, the real-time feature of the anchor hole at this time A =[ A 1; A 2; A 3; A 4; A 5; x ; y ], x and y These represent the x and y coordinates of the center point of the ellipse, respectively; the Jacobian matrix of the image is... L =[ L E ; L D ]

[0112] (2);

[0113] In formula (2), Z This represents the depth of the anchor hole.

[0114] On this basis, the visual servo controller is represented as .

[0115] The intelligent control system of the drill-anchor mechanical arm provided by the embodiment of the present application can autonomously identify the anchor hole pose, adjust the position of the machine body, and autonomously control the drill-anchor mechanical arm to run to the accurate working position. Meanwhile, by combining the coarse control module and the precise control module, the influence of the anchor hole visual positioning accuracy on the final control result is effectively avoided, the control accuracy and stability are ensured, and the operation efficiency of the autonomous hole searching control of the drill-anchor mechanical arm is effectively improved. Meanwhile, the coarse control module can be combined with the mechanical arm obstacle avoidance function during work to reduce the possibility of collision of the drill-anchor mechanical arm in the control process. Once the D-H modeling of the drill-anchor mechanical arm and the anchor hole expected feature acquisition are performed, the drill-anchor mechanical arm can be ensured to perform multiple unmanned autonomous support operations in the coal mine roadway.

[0116] The intelligent control system of the drill-anchor mechanical arm provided by the embodiment of the present application has the functions of automatic anchor hole recognition and autonomous hole searching of the drill-anchor mechanical arm, realizes the automation and intelligentization of the whole process hole searching control, carries the support equipment, and can autonomously complete the support operation without human intervention, thereby providing a new idea for the few-person and unmanned coal mine roadway. Meanwhile, the advantages of the position-based visual servo and the image-based visual servo are combined, the automatic hole searching control accuracy is ensured, and the control time is effectively reduced. The elliptical geometric image of the anchor hole under the depth camera is taken as the real-time feature of the anchor hole, and the reliability of the image-based visual servo controller under the harsh conditions such as occlusion and low illumination in the coal mine roadway is greatly improved.

[0117] To sum up, the intelligent control system of the drill-anchor mechanical arm provided by the embodiment of the present application firstly uses the drill-anchor mechanical arm visual servo controller based on the real-time operation image of the precise control module to solve the control error caused by insufficient positioning accuracy, and meanwhile, converts the real-time operation image into a control instruction to improve the coupling of the visual technology and the control technology. Then, the method of combining the coarse positioning module and the precise positioning module is used to reduce the control time and improve the control efficiency. Finally, the workspace analysis module is carried to ensure the intelligentization and unmannedization of the whole support process, so that the intelligent control system can autonomously and continuously work without human intervention.

[0118] The above only describes the preferred embodiments of the present application and is not used to limit the present application, and it should be noted that, for ordinary skilled persons in the art, without departing from the technical principles of the present application, a number of improvements and modifications can be made, and these improvements and modifications should be regarded as the protection scope of the present application.

Claims

1. An intelligent control system for a drilling and anchoring robotic arm, characterized in that, The system comprises a workspace analysis module, a visual positioning module, a coarse control module and a precise control module; the visual positioning module comprises a depth camera installed on the drill-anchor mechanical arm; The workspace analysis module is configured to perform workspace analysis on the drill-anchor mechanical arm according to a pre-stored drill-anchor mechanical arm D-H model, determine a working range of the drill-anchor mechanical arm, and determine whether the anchor hole is within the working range of the drill-anchor mechanical arm according to three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system fed back by the visual positioning module; if the anchor hole is not within the working range of the drill-anchor mechanical arm, the drill-anchor mechanical arm body is controlled to move to the anchor hole so as to enter the working range of the drill-anchor mechanical arm; if the anchor hole is within the working range of the drill-anchor mechanical arm, the workspace analysis module stops working; the visual positioning module is configured to collect real-time working images of the drill-anchor mechanical arm, analyze the real-time working images, obtain three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system, and feed the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system to the workspace analysis module and the coarse control module; The coarse control module is configured to take the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system as a control target, determine an optimal trajectory of the drill-anchor mechanical arm by using a trajectory planning algorithm, guide the drill-anchor mechanical arm to approach the anchor hole according to the optimal trajectory, and stop working when it is determined that the anchor hole pose depth is less than a preset threshold value; The precise control module is configured to, when the anchor hole pose depth is less than the preset threshold value, obtain an anchor hole expected feature and an anchor hole real-time feature, take the anchor hole expected feature as a control target, and use a visual servo controller to attenuate an anchor hole feature deviation between the anchor hole real-time feature and the anchor hole expected feature to 0, so as to guide the drill-anchor mechanical arm to reach an expected drill-anchor working position; When the precise control module takes the anchor hole expected feature as a control target and uses the visual servo controller to attenuate the anchor hole feature deviation between the anchor hole real-time feature and the anchor hole expected feature to 0, the precise control module comprises the following steps: S31, obtaining the anchor hole real-time feature, specifically comprising: S311, extracting an anchor hole image from the real-time working image, and extracting anchor hole edge points from the anchor hole image; S312, fitting the anchor hole edge points into an ellipse, and taking the ellipse feature as the anchor hole real-time feature; S32, calculating an image Jacobian matrix according to the anchor hole real-time feature, specifically comprising: calculating the image Jacobian matrix according to the real-time anchor hole feature by formula (1): （1）； In equation (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 as ; ; ; ; wherein, S33, establishing a first mapping relationship between the anchor hole real-time feature variation speed and the depth camera motion speed based on the image Jacobian matrix; ( α , β , S34, determining a drill-anchor mechanical arm Jacobian matrix according to the drill-anchor mechanical arm D-H model, and establishing a second mapping relationship between the depth camera motion speed and the drill-anchor mechanical arm joint speed according to the drill-anchor mechanical arm Jacobian matrix; ) are parameters of the plane on which the anchor hole is located in the world coordinate system, the plane on which the anchor hole is located is expressed as , , , ) represent the three-dimensional coordinates of the center point of the anchor hole; S35, determining a third mapping relationship between the anchor hole real-time feature parameter variation speed and the joint speed according to the first mapping relationship and the second mapping relationship; S36, establishing a fourth mapping relationship between an error variation rate and the joint speed according to an anchor hole feature deviation between the anchor hole real-time feature and the anchor hole expected feature; ​ ​ S37, the control error rate is exponential type falling control rate, the visual servo controller is determined according to the exponential type falling control rate and the fourth mapping relationship, and the visual servo controller is used to attenuate the anchor hole feature deviation between the real-time feature of the anchor hole and the expected feature of the anchor hole to 0; The preset threshold is calculated by formula (3): （3）； In formula (3), d is an actual diameter of the anchor hole, f is a focal length of the depth camera, z is a distance from the depth camera to the anchor hole, a resolution of the real-time operation image is WxH, S is an imaging area of the anchor hole, and when S is equal to 0.2xWxH, the distance from the depth camera to the anchor hole calculated by formula (3) is taken as a preset threshold, and W and H represent a pixel length and a width of the depth camera, respectively.

2. The intelligent control system of the drill-anchor robotic arm according to claim 1, wherein, The visual positioning module further includes an edge computer, and the depth camera is electrically connected with the edge computer. S21, the edge computer uses an adaptive histogram equalization algorithm to perform defogging processing on the real-time working image; S22, the edge computer extracts image features of the defogged image by using a YOLOv5s model combined with a self-attention mechanism, and identifies the anchor hole based on the image features to obtain an anchor hole image; S23, the edge computer processes the anchor hole image by morphological operation and Canny edge detection algorithm to obtain an anchor hole edge point set; S24, the edge computer uses a Hough circle detection algorithm to fit the points in the anchor hole edge point set to obtain an anchor hole contour, and determines the pixel coordinates of the anchor hole center point according to the anchor hole contour; S25, the edge computer converts the pixel coordinates of the anchor hole center point into three-dimensional coordinates in the depth camera coordinate system, and calculates the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system according to the relative position of the depth camera and the anchor drilling mechanical arm.

3. The intelligent control system of the drill-anchor robotic arm according to claim 1, wherein, When the coarse control module guides the anchor drilling mechanical arm to approach the anchor hole according to the optimal trajectory, it is specifically used for: acquiring angle data and displacement data collected by a joint angle sensor installed on a rotating joint of the anchor drilling mechanical arm and a position sensor installed on a moving joint in real time, and guiding the anchor drilling mechanical arm to approach the anchor hole by using a PID control algorithm according to the angle data and the displacement data.

4. The intelligent control system of the drill-anchor robotic arm according to claim 1, wherein, When the coarse control module guides the anchor drilling mechanical arm to approach the anchor hole according to the optimal trajectory, it is further used for: judging whether the anchor hole pose depth is less than a preset threshold according to the three-dimensional coordinates of the anchor hole in the mechanical arm coordinate system, and stopping working when the anchor hole pose depth is less than the preset threshold.

5. The intelligent control system of the drill-anchor robotic arm according to claim 1, wherein, The first mapping relationship is represented as: ; wherein, is the real-time feature change speed of the anchor hole, is the motion speed of the depth camera, is the image Jacobian matrix; The second mapping relationship is represented as ; wherein, is a Jacobian matrix of the drill-anchor mechanical arm, is a joint speed; The third mapping relationship is represented as: ; wherein + represents a pseudo-inverse matrix; The fourth mapping relationship is represented as: ; wherein, , A d is an anchor hole expected feature, A is an anchor hole real-time feature; The exponential decrease control rate is expressed as: ; wherein, is a decrease coefficient; The visual servoing controller is represented as .

6. The intelligent control system of the drill-anchor robotic arm according to claim 1, wherein, 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 ellipse center point; the image Jacobian matrix is L [ L E ; L D ] （2）； In equation (2), Z is the depth of the anchor hole.

Images