A servo control system for a multi-axis robotic arm used in a coal mine drilling rig

Abstract

The present disclosure provides a servo control system for a multi-axis robotic arm used in a coal mine drill rig, belonging to the technical field of robotic arm control. The servo control system for the multi-axis robotic arm used in the coal mine drill rig includes: a strategy determination module, configured to determine a working mode based on construction requirements, where there are multiple operation strategies for the same working mode; and further configured to select, from the multiple operation strategies, an operation strategy that matches the complexity of the working content of the multi-axis robotic arm as the target operation strategy; the operation strategy includes the operation parameters of the multi-axis robotic arm; a distance determination module, configured to determine a target distance based on the working mode, where the target distance is the distance between the multi-axis robotic arm and the target drill pipe; and a servo control module, configured to control a plurality of servo motors based on the target operation strategy and the target distance, and each servo motor corresponds to a part of the multi-axis robotic arm. The present disclosure can solve the problem of low servo control work efficiency.

Description

Technical Field

[0001] The present disclosure relates to the technical field of robotic arm control, and particularly to a servo control system for a multi-axis robotic arm used in a coal mine drill rig. Background Art

[0002] During the coal mining process, the efficient and precise operation of the drill rig is crucial for improving the mining efficiency and ensuring safe production. As an important actuator of the drill rig, the multi-axis robotic arm can achieve complex motion trajectories to adapt to different drilling requirements. However, the traditional servo control system has deficiencies in the aspect of work mode switching, and cannot achieve automatic switching of different work modes at any position. It relies on manual startup, which reduces the work efficiency and stability of the robotic arm.

[0003] Therefore, there is an urgent need for a servo control system for a multi-axis robotic arm used in a coal mine drill rig that is efficient and stable. Summary of the Invention

[0004] Embodiments of the present disclosure provide a servo control system for a multi-axis robotic arm used in a coal mine drill rig to solve the problem of low servo control work efficiency.

[0005] Embodiments of the present disclosure provide a servo control system for a multi-axis robotic arm used in a coal mine drill rig, including:

[0006] A strategy determination module, configured to determine a work mode based on construction requirements, where there are multiple operation strategies for the same work mode; and further configured to select an operation strategy that matches the complexity of the work content of the multi-axis robotic arm from the multiple operation strategies as the target operation strategy; the operation strategy includes the operation parameters of the multi-axis robotic arm;

[0007] A distance determination module, configured to determine a target distance based on the work mode, where the target distance is the distance between the multi-axis robotic arm and the target drill pipe;

[0008] A servo control module, configured to control multiple servo motors based on the target operation strategy and the target distance, where each servo motor corresponds to a part of the multi-axis robotic arm;

[0009] The strategy determination module and the distance determination module are both connected to the servo control module.

[0010] In an exemplary embodiment of the present disclosure, the work content includes operation steps;

[0011] The strategy determination module is specifically configured to:

[0012] In response to the complexity of the operation steps being greater than or equal to a first complexity, take the first operation strategy as the target operation strategy;

[0013] In response to the complexity of the operation step being less than the first complexity, the second operation strategy is taken as the target operation strategy;

[0014] Among them, the working efficiency of the first operation strategy is greater than that of the second operation strategy, and the accuracy of the first operation strategy is less than that of the second operation strategy.

[0015] In an exemplary embodiment of the present disclosure, the operation parameters include the speed, acceleration, and torque of each motion axis of the multi-axis robotic arm;

[0016] The strategy determination module is specifically further configured to:

[0017] Calculate the speed of each motion axis of the multi-axis robotic arm based on the working efficiency, accuracy, and the target working content corresponding to each operation step;

[0018] Calculate the acceleration of each motion axis of the multi-axis robotic arm based on the working efficiency, accuracy, and the target working content corresponding to each operation step;

[0019] Calculate the torque of each motion axis of the multi-axis robotic arm based on the target working content corresponding to each operation step.

[0020] In an exemplary embodiment of the present disclosure, a servo control system for a multi-axis robotic arm used in a coal mine drilling rig further includes:

[0021] An attitude detection module, configured to determine the attitude characteristics of the multi-axis robotic arm based on an encoder and send the attitude characteristics to the servo control module.

[0022] In an exemplary embodiment of the present disclosure, a servo control system for a multi-axis robotic arm used in a coal mine drilling rig further includes:

[0023] A path planning module, configured to process the target distance and attitude characteristics based on a path planning algorithm to obtain the target path for the multi-axis robotic arm to operate;

[0024] The input end of the path planning module is respectively connected to the strategy determination module and the attitude detection module;

[0025] The output end of the path planning module is connected to the servo control module.

[0026] In an exemplary embodiment of the present disclosure, the path planning module is specifically configured to:

[0027] Determine the starting position point, actual position point, and target position point of the multi-axis robotic arm;

[0028] Determine the actual cost function of the path planning algorithm based on the starting position point and the actual position point;

[0029] Determine the heuristic function of the path planning algorithm based on the actual position point and the target position point;

[0030] Determine the evaluation function by adding the actual cost function and the heuristic function;

[0031] Input the actual position point into the evaluation function to obtain the target path for the multi-axis robotic arm to operate.

[0032] In an exemplary embodiment of the present disclosure, the working modes include a drilling working mode and a drill unloading working mode;

[0033] The path planning module is specifically further configured to:

[0034] Determine the initial adjustment coefficient of the actual cost function;

[0035] In response to the drilling working mode being in the first stage, increase the initial adjustment coefficient according to the first step length to obtain the first adjustment coefficient;

[0036] In response to the drilling working mode being in the second stage, decrease the first adjustment coefficient according to the second step length to obtain the second adjustment coefficient;

[0037] In response to the drill unloading working mode being in the first stage, increase the initial adjustment coefficient according to the third step length to obtain the third adjustment coefficient;

[0038] In response to the drill unloading working mode being in the second stage, decrease the third adjustment coefficient according to the fourth step length to obtain the fourth adjustment coefficient;

[0039] Wherein, the first adjustment coefficient is greater than the second adjustment coefficient, and the third adjustment coefficient is greater than the fourth adjustment coefficient.

[0040] In an exemplary embodiment of the present disclosure, the path planning module is specifically further configured to:

[0041] Determine the initial weight of the heuristic function;

[0042] In response to the target distance being greater than or equal to the first distance threshold, decrease the initial weight according to the fifth step length to obtain the first weight;

[0043] In response to the target distance being less than the first distance threshold, increase the initial weight according to the sixth step length to obtain the second weight.

[0044] In an exemplary embodiment of the present disclosure, a servo control system for a multi-axis robotic arm of a coal mine drill further includes:

[0045] A tightness detection module, configured to obtain the first force by detecting the pressure of the robotic arm gripper according to the first pressure sensor; and obtain the second force by detecting the pressure of the drill main machine gripper according to the second pressure sensor;

[0046] Send the first force and the second force to the policy determination module to instruct the policy determination module to adjust the operation policy based on the first force and the second force.

[0047] In an exemplary embodiment of the present disclosure, a servo control system for a multi-axis robotic arm of a coal mine drill rig further includes:

[0048] A gripper control module, configured to control the robotic arm gripper to release the drill pipe in response to the second force being greater than or equal to the second force threshold;

[0049] Control the robotic arm gripper to grab the drill pipe in response to the second force being less than the second force threshold;

[0050] Wherein, if the first force is less than the first force threshold, the speed of each motion axis of the multi-axis robotic arm is reduced.

[0051] The beneficial effects of the servo control system for the multi-axis robotic arm of the coal mine drill rig provided by the embodiments of the present disclosure are as follows:

[0052] Through the policy determination module, the present disclosure can flexibly adapt to different construction requirements, automatically select the optimal operation policy to complete the working mode, and improve the work efficiency. At the same time, the distance determination module can accurately calculate the distance between the multi-axis robotic arm and the target drill pipe, and accurately position the multi-axis robotic arm. Subsequently, the servo control module can achieve precise control of the servo motor based on the target operation policy and the target distance, thereby realizing the coordinated work of each part of the multi-axis robotic arm and greatly improving the work efficiency. Therefore, the present disclosure can solve the problem of low servo control work efficiency. Description of the Drawings

[0053] In order to more clearly illustrate the technical solutions in the embodiments of the present disclosure, the following will briefly introduce the drawings required for use in the embodiments or the description of the prior art. Obviously, the following drawings are only some embodiments of the present disclosure. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0054] Figure 1 It is a schematic structural diagram of a servo control system for a multi-axis robotic arm of a coal mine drill rig provided by an embodiment of the present disclosure;

[0055] Figure 2 It is a schematic structural diagram of another servo control system for a multi-axis robotic arm of a coal mine drill rig provided by an embodiment of the present disclosure;

[0056] Figure 3 It is a side view of the servo robotic arm and the drill pipe box provided by an embodiment of the present disclosure;

[0057] Figure 4It is the front view of the servo robotic arm provided by an embodiment of the present disclosure;

[0058] Figure 5 It is the position where the servo robotic arm provided by an embodiment of the present disclosure grabs the drill pipe in the drill pipe box;

[0059] Figure 6 It is the position of the drill pipe of the servo robotic arm provided by an embodiment of the present disclosure at the gripper of the drilling rig mainframe. Detailed implementation manners

[0060] To enable those skilled in the art to better understand this solution, the following will clearly describe the technical solutions in the embodiments of this solution with reference to the accompanying drawings in the embodiments of this solution. Obviously, the described embodiments are some, but not all, of the embodiments of this solution. Based on the embodiments in this solution, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the scope of protection of this solution.

[0061] The term "including" in the specification, claims and above-mentioned accompanying drawings of this solution, as well as any other deformation, means "including but not limited to", and is intended to cover non-exclusive inclusion, not limited only to the examples listed in the text. In addition, terms such as "first" and "second" are used to distinguish different objects, rather than to describe a specific order.

[0062] The following will describe the implementation of the present disclosure in detail with reference to specific accompanying drawings:

[0063] Figure 1 It is the structural schematic diagram of a servo control system of a multi-axis robotic arm for a coal mine drilling rig provided by an embodiment of the present disclosure. Refer to Figure 1 , this servo control system of a multi-axis robotic arm for a coal mine drilling rig includes:

[0064] A strategy determination module, configured to determine a working mode based on construction requirements, and there are multiple operation strategies for the same working mode; it is also configured to select an operation strategy that matches the complexity of the work content of the multi-axis robotic arm from multiple operation strategies as the target operation strategy; the operation strategy includes the operation parameters of the multi-axis robotic arm;

[0065] A distance determination module, configured to determine a target distance based on the working mode, and the target distance is the distance between the multi-axis robotic arm and the target drill pipe;

[0066] A servo control module, configured to control multiple servo motors based on the target operation strategy and the target distance, and each servo motor corresponds to a part of the multi-axis robotic arm;

[0067] The strategy determination module and the distance determination module are both connected to the servo control module.

[0068] In this embodiment, the construction requirement is whether the current coal mining has started or ended. If the construction requirement is that the current coal mining has started, the working mode is the drilling working mode of the servo control system; if the construction requirement is that the current coal mining has ended, the working mode is the drill unloading working mode of the servo control system. Even in the same working mode, there will be different operation strategies (i.e., multiple operation strategies), and different operation strategies have different considerations for working efficiency or working precision.

[0069] In each working mode, multiple operation strategies can be pre-formulated according to the complexity of the work content of the multi-axis robotic arm, that is, the similarity between the complexity of the work content of the target multi-axis robotic arm and the complexity of the work content of the standard multi-axis robotic arm can be calculated to determine the target operation strategy. The complexity of the work content of the target multi-axis robotic arm is the complexity corresponding to the current work content, and the complexity of the work content of the standard multi-axis robotic arm is the complexity corresponding to the pre-planned work content.

[0070] Determining the first similarity based on the similarity between the complexity of the work content of the target multi-axis robotic arm and the complexity of the work content of the standard multi-axis robotic arm includes:

[0071] Determining the number of operation steps for the target multi-axis robotic arm to complete the construction requirement ;

[0072] Calculating the operation step similarity between the number of operation steps for the target multi-axis robotic arm to complete the construction requirement and the number of operation steps for the standard multi-axis robotic arm to complete the construction requirement based on the first formula ;

[0073] Each construction requirement corresponds to a number of operation steps, and the number of operation steps corresponding to the construction requirement can be used as ;

[0074] The first formula is ;

[0075] Where is the number of operation steps for the standard multi-axis robotic arm to complete the construction requirement, and is a preset value;

[0076] Let the set of action types be where are different action types, , and the action types can be translation, rotation, grasping, etc.;

[0077] Determining the action type vector of the target multi-axis robotic arm ; where is the number of times the action type appears in the work content of the target multi-axis robotic arm;

[0078] Determine the action type vector of the standard multi-axis robotic arm ; where is the number of occurrences of the action type in the work content of the standard multi-axis robotic arm;

[0079] Calculate the action type similarity between the action type vector of the target multi-axis robotic arm and the action type vector of the standard multi-axis robotic arm based on the second formula ;

[0080] The second formula is ;

[0081] ;

[0082] ;

[0083] ;

[0084] Perform weighted calculation based on the operation step similarity and the action type similarity to obtain the first similarity.

[0085] Each operation strategy corresponds to a work content of the standard multi-axis robotic arm. Therefore, if the first similarity is greater than or equal to the first similarity threshold, the complexity of the work content of the target multi-robotic arm matches the complexity of the work content of the standard multi-axis robotic arm, that is, select this operation strategy as the target operation strategy. Among them, the first similarity threshold is a preset reference value, which can be set according to experience.

[0086] Performing weighted calculation based on the operation step similarity and the action type similarity to obtain the first similarity includes:

[0087] Determine the weight of the operation step similarity , ;

[0088] Determine the weight of the action type similarity , ;

[0089] The first similarity is:

[0090] , and .

[0091] The operation strategy is to set the operation parameters of the multi-axis robotic arm, and the parameters can include the movement speed, acceleration, and torque of each motion axis, etc. For example, if the selected operation strategy is a fast movement strategy to improve work efficiency, then the corresponding operation parameter adjustment can be to increase the maximum speed and acceleration of each axis.

[0092] Different working modes determine different working relationships between the multi-axis robotic arm and the target drill pipe, and thus correspond to different target distances. The target drill pipe can be the drill pipe in the drill pipe bin or the drill pipe at the gripper of the main drill rig. For example, in the working mode of preparing to grasp the drill pipe, the target distance can be the distance between the end effector of the robotic arm and the drill pipe to be grasped. The distance determination module will accurately calculate or determine the target distance between the multi-axis robotic arm and the target drill pipe according to the current working mode. This distance information is crucial for the precise control of the subsequent servo motors and provides a key position reference data for the servo control module.

[0093] The servo control module receives the target operation strategy (i.e., the determined operation parameters) transmitted by the strategy determination module and the target distance information given by the distance determination module. Then, it precisely controls multiple servo motors according to the above information. Since each servo motor corresponds to a part of the multi-axis robotic arm, such as the rotation of a certain joint or the linear movement of a certain axis, the servo control module will send corresponding control signals to each servo motor according to the parameter requirements such as speed, acceleration, and torque in the target operation strategy, as well as the position information determined by the target distance. For example, in order to make the end of the robotic arm reach the target distance position, according to the speed parameter in the target operation strategy, control the corresponding servo motor to rotate at an appropriate speed, drive the corresponding part of the robotic arm to move, and ensure that the robotic arm accurately completes the task according to the predetermined operation strategy.

[0094] It can be concluded from the above that through the strategy determination module, the present disclosure can flexibly adapt to different construction requirements, automatically select the optimal operation strategy to complete the working mode, and improve work efficiency. At the same time, the distance determination module can accurately calculate the distance between the multi-axis robotic arm and the target drill pipe and accurately locate the position of the multi-axis robotic arm. Then, the servo control module can achieve precise control of the servo motors based on the target operation strategy and the target distance, thereby realizing the coordinated work of each part of the multi-axis robotic arm and greatly improving work efficiency. Therefore, the present disclosure can solve the problem of low work efficiency of servo control.

[0095] In an embodiment of the present disclosure, referring to Figure 2 , the work content includes operation steps;

[0096] The strategy determination module is specifically used for:

[0097] In response to the complexity of the operation steps being greater than or equal to the first complexity, taking the first operation strategy as the target operation strategy;

[0098] In response to the complexity of the operation steps being less than the first complexity, taking the second operation strategy as the target operation strategy;

[0099] Among them, the working efficiency of the first operation strategy is greater than that of the second operation strategy, and the accuracy of the first operation strategy is less than that of the second operation strategy.

[0100] In this embodiment, the operation steps are the sequence of specific actions performed by the multi-axis robotic arm to complete the construction requirements. For example, how many times is the transfer work of the drill pipe from the drill pipe bin to the main drill rig required, and whether the multi-axis robotic arm needs to move forward and backward, etc.

[0101] The complexity of the operation steps is used to measure the difficulty level of the operation steps when the multi-axis robotic arm completes the construction requirements, comprehensively considering factors such as the number of operation steps, the logical relationship between steps, the working efficiency of completing all steps, and the accuracy requirements.

[0102] The first complexity is a pre-set threshold, used as a standard to distinguish the high or low complexity of the operation steps, which can be set according to experience and is used for judgment in the strategy determination module. The first operation strategy is the operation mode of the multi-axis robotic arm formulated for the case of higher complexity of the operation steps, which features emphasizing working efficiency, that is, it can complete the work task in a shorter time, but is relatively weak in terms of accuracy. The second operation strategy is the operation mode of the multi-axis robotic arm formulated for the case of lower complexity of the operation steps, which features emphasizing accuracy and is relatively lower in working efficiency than the first operation strategy.

[0103] From the above, it can be concluded that this embodiment can intelligently identify the complexity of the operation steps and accordingly select an operation strategy that balances working efficiency and accuracy. For high-complexity operations, this embodiment adopts the first operation strategy that is efficient but slightly lower in accuracy to ensure the operation progress; while for simple operations, it adopts the second operation strategy with higher accuracy to ensure the operation quality. The above dynamic adjustment strategy not only improves the overall operation efficiency but also ensures satisfactory accuracy in different operation scenarios, achieving a dual optimization of efficiency and quality.

[0104] In an embodiment of the present disclosure, referring to Figure 2 , the operating parameters include the speed, acceleration, and torque of each moving axis of the multi-axis robotic arm;

[0105] The strategy determination module is specifically further configured to:

[0106] Calculate the speed of each moving axis of the multi-axis robotic arm based on the working efficiency, accuracy, and the target work content corresponding to each operation step;

[0107] Calculate the acceleration of each moving axis of the multi-axis robotic arm based on the working efficiency, accuracy, and the target work content corresponding to each operation step;

[0108] Calculate the torque of each moving axis of the multi-axis robotic arm based on the target work content corresponding to each operation step.

[0109] In this embodiment, the operating parameters are the parameters describing the operating state of the multi-axis robotic arm. Among them, the speed determines how fast each moving axis of the multi-axis robotic arm moves; the acceleration controls how fast the speed of the moving axis changes and affects the starting, stopping, and acceleration / deceleration processes of the robotic arm; the torque reflects the magnitude of the rotational force that the moving axis can output.

[0110] The work efficiency is the amount of work completed by the multi-axis robotic arm per unit time and can reflect how fast the multi-axis robotic arm completes tasks. The accuracy is the degree of compliance with the target requirements when the multi-axis robotic arm completes the work task and reflects the precision of the multi-axis robotic arm's operation. The target work content is the specific work requirement expected to be achieved for each operation step. Different operation steps correspond to different target work contents.

[0111] The strategy determination module comprehensively considers the requirements of work efficiency, the standards of accuracy, and the specific goals to be achieved for each operation step to determine the moving speed of each moving axis of the multi-axis robotic arm. If the work efficiency requirement is high and the accuracy requirement is relatively low, and the target work content allows for faster operation, then the speed of the moving axis can be increased; conversely, if the accuracy requirement is extremely high and the target work content requires precise operation, the speed can be decreased.

[0112] The strategy determination module considers starting from work efficiency, accuracy, and the target work content of each operation step to determine how fast the speed of the moving axis changes. If the work efficiency requirement is high, a larger acceleration may be needed to quickly bring the robotic arm to the working speed; however, if the accuracy requirement is high, to avoid impact on the operation accuracy due to too fast speed change, the acceleration can be reduced.

[0113] The strategy determination module determines the magnitude of the rotational force that the moving axis needs to output based on the target work content expected to be achieved for each operation step. Different operation steps have different torque requirements due to different loads they bear.

[0114] Calculate the speed of each moving axis of the multi-axis robotic arm based on the third formula ;

[0115] The third formula is ;

[0116] where is the speed of the moving axis for the th operation step, is the time allowed for the th operation step, , is the number of operation steps required to complete a complete work task, The total time allowed to complete a work task , is the work efficiency, i.e., the amount of tasks completed per unit time; is the accuracy, which can be determined by the allowable range of position error, and the value range is , and the smaller the value, the higher the accuracy, is the basic accuracy; is the distance that the moving axis needs to move in the th operation step, is the difficulty coefficient of the target work content corresponding to the

[0117] th operation step. The value range is , and the larger the value, the more difficult it is to complete this step, and it can be set according to experience. is the adjustment coefficient considering the accuracy requirement. When the accuracy is greater than the basic accuracy , this coefficient is greater than 1, and the speed can be reduced accordingly to ensure the accuracy of the operation; conversely, when the accuracy is less than the basic accuracy , the coefficient is less than 1, and the speed can be increased accordingly; is the adjustment coefficient considering the difficulty of the target work content. The larger the difficulty coefficient of the target work content, the smaller this coefficient, and the speed will decrease to handle more complex operations; the smaller the difficulty coefficient, the speed can be relatively increased.

[0118] Calculate the acceleration of each moving axis of the multi-axis robotic arm based on the fourth formula ;

[0119] The fourth formula is ;

[0120] Among them, is the speed of the moving axis at the end of the th operation step, is the time of the th operation step,

[0121] is the time ratio coefficient in the speed change transition stage, ;

[0122] The fifth formula is ;

[0123] Among them, is the torque required for the moving axis in the th operation step, is the load force that the moving axis needs to overcome in the th operation step, is the distance from the moving axis to the load acting point, is the friction force received by the moving axis in the th operation step, is a correction coefficient related to the difficulty of the target work content ( , the greater the difficulty, the greater the value).

[0124] It can be seen from the above that this embodiment not only ensures the high efficiency and accuracy of the multi-axis robotic arm when performing tasks, but also greatly improves the stability and reliability of the operation. In addition, by flexibly adjusting the motion parameters of each axis for different operation steps, the robotic arm can better adapt to diverse work tasks, improving the overall flexibility and adaptability.

[0125] In an embodiment of the present disclosure, referring to Figure 2 , a servo control system for a multi-axis robotic arm used in a coal mine drill also includes:

[0126] An attitude detection module, configured to determine the attitude characteristics of the multi-axis robotic arm based on an encoder and send the attitude characteristics to the servo control module.

[0127] A path planning module, configured to process the target distance and attitude characteristics based on a path planning algorithm to obtain the target path for the multi-axis robotic arm to run;

[0128] The input end of the path planning module is respectively connected to the policy determination module and the attitude detection module;

[0129] The output end of the path planning module is connected to the servo control module.

[0130] In this embodiment, the attitude characteristics include the position characteristics, angle characteristics, and direction characteristics of the multi-axis robotic arm. The encoder is a sensor capable of measuring the rotation or linear displacement of mechanical components. In the multi-axis robotic arm, the encoder can be installed at each joint to measure the rotation position, angle, and direction of the joint, thereby determining the attitude of the multi-axis robotic arm. The attitude detection module uses the data measured by the encoder, through calculation and analysis, to determine the attitude characteristics of the multi-axis robotic arm, and then sends the above attitude characteristic information to the servo control module so that the servo control module can perform subsequent control operations according to the current attitude of the robotic arm.

[0131] The path planning module plans the best operation path for the multi-axis robotic arm from the current position to the target position according to certain algorithms and input information. The path planning algorithm in this embodiment can be the A* algorithm. The target distance is the distance information between the target position that the multi-axis robotic arm needs to reach and the current position, which is provided by the distance determination module. The target path is the operation trajectory that the multi-axis robotic arm needs to follow, calculated by the path planning module to complete the work task.

[0132] The path planning module receives the operation strategy-related information (such as operation parameters like speed and acceleration) from the strategy determination module and the pose feature information from the pose detection module, combines the target distance, and uses the path planning algorithm for processing to calculate the target path for the multi-axis robotic arm to move from the current pose to the target position.

[0133] The input end of the path planning module is connected to the strategy determination module and the pose detection module for obtaining the necessary input information; the output end is connected to the servo control module to send the planned target path to the servo control module so that it can control the multi-axis robotic arm to run according to this path.

[0134] It can be concluded from the above that this embodiment can obtain the pose characteristics of the robotic arm in real time and accurately. This embodiment can intelligently plan the optimal path for the robotic arm to run according to the target distance and pose characteristics, greatly improving the operation efficiency and accuracy.

[0135] In an embodiment of the present disclosure, referring to Figure 2 , the path planning module is specifically used for:

[0136] Determine the starting position point, actual position point, and target position point of the multi-axis robotic arm;

[0137] Determine the actual cost function of the path planning algorithm based on the starting position point and the actual position point;

[0138] Determine the heuristic function of the path planning algorithm based on the actual position point and the target position point;

[0139] Determine the evaluation function by adding the actual cost function and the heuristic function;

[0140] Input the actual position point into the evaluation function to obtain the target path for the multi-axis robotic arm to run.

[0141] In this embodiment, the starting position point is the spatial position where the multi-axis robotic arm starts to execute the work task and is the starting point of path planning. The actual position point is the spatial position where the multi-axis robotic arm is currently located during the path planning process and will change as the multi-axis robotic arm moves. The target position point is the final spatial position that the multi-axis robotic arm needs to reach and is the goal to be achieved by path planning, that is, the drill pipe bin or the drilling rig mainframe.

[0142] The actual cost function is a function that measures the actual cost incurred from the starting position point to the current actual position point in path planning and is related to factors such as the distance traveled, time, and energy consumption. The actual cost function is , that is, to calculate the distance from the starting position point D to the actual position point N.

[0143] The heuristic function is an estimation function for the cost required to reach the target position point from the current actual position point, which can guide the search in the direction of the target and improve the search efficiency. The heuristic function is , that is, to calculate the distance from the actual position point N to the target position point G. The above two distance formulas can be calculated by the Euclidean distance or Manhattan distance formula.

[0144] The evaluation function combines the actual cost function and the heuristic function and is used to evaluate the advantages and disadvantages of each possible path node to determine the next search direction. The evaluation function is , where is the target adjustment coefficient of the actual cost function, is the target weight of the heuristic function. In this embodiment, by comparing the evaluation function values of different nodes, the path planning algorithm can select the optimal node for expansion, thereby finding the target path faster.

[0145] The target adjustment coefficient may include a first adjustment coefficient, a second adjustment coefficient, a third adjustment coefficient, and a fourth adjustment coefficient.

[0146] In an embodiment of the present disclosure, referring to Figure 2 , the working modes include a drilling working mode and a drill unloading working mode;

[0147] The path planning module is specifically further configured to:

[0148] Determine the initial adjustment coefficient of the actual cost function;

[0149] In response to the drilling working mode being the first stage, increase the initial adjustment coefficient by the first step length to obtain the first adjustment coefficient;

[0150] In response to the drilling working mode being the second stage, decrease the first adjustment coefficient by the second step length to obtain the second adjustment coefficient;

[0151] In response to the drill unloading working mode being the first stage, increase the initial adjustment coefficient by the third step length to obtain the third adjustment coefficient;

[0152] In response to the drill unloading working mode being the second stage, decrease the third adjustment coefficient by the fourth step length to obtain the fourth adjustment coefficient;

[0153] Among them, the first adjustment coefficient is greater than the second adjustment coefficient, and the third adjustment coefficient is greater than the fourth adjustment coefficient.

[0154] In this embodiment, the initial adjustment coefficient is the starting coefficient for adjusting the actual cost function, and is used to dynamically adjust the actual cost function according to different working modes and stages in the subsequent process. The adjustment coefficient is used to adjust the value of the actual cost function. By changing the adjustment coefficient, the evaluation of different paths during path planning can be changed, thereby affecting the finally planned path.

[0155] Both the first step length and the second step length are used to increase or decrease the value of the adjustment coefficient at different stages of the drilling operation mode, and can be set according to experience; both the third step length and the fourth step length are used to increase or decrease the value of the adjustment coefficient at different stages of the drill unloading operation mode, and can be set according to experience.

[0156] When the multi-axis robotic arm is in the first stage of the drilling operation mode, the path planning module adds the first step length to the initial adjustment coefficient to obtain the first adjustment coefficient. The first stage is the initial stage, and more cautious path planning is required. Increasing the adjustment coefficient can relatively increase the cost of some paths, thereby guiding the robotic arm to select a safer and more appropriate path.

[0157] In the second stage of the drilling operation mode, the path planning module subtracts the second step length from the first adjustment coefficient to obtain the second adjustment coefficient. The second stage is the stage after the initial stage. Because in this stage, the multi-axis robotic arm has completed some preliminary preparations, or the working environment has become relatively stable. Reducing the adjustment coefficient can make the path planning more flexible and improve work efficiency.

[0158] When the robotic arm is in the first stage of the drill unloading operation mode, the path planning module adds the third step length to the initial adjustment coefficient to obtain the third adjustment coefficient. Special care is required in the first stage of drill unloading to avoid damaging the drill pipe. Increasing the adjustment coefficient can make the path selection more conservative.

[0159] In the second stage of the drill unloading operation mode, the path planning module subtracts the fourth step length from the third adjustment coefficient to obtain the fourth adjustment coefficient. At this time, most of the drill unloading operations have been completed. Reducing the adjustment coefficient can speed up the movement speed of the robotic arm and improve work efficiency.

[0160] In the first stage of the drilling and drill unloading operation modes, in order to ensure the safety and accuracy of the work, it is necessary to increase the adjustment coefficient of the actual cost function; while in the second stage, in order to improve work efficiency, the adjustment coefficient can be appropriately reduced.

[0161] The target weights can include a first weight and a second weight.

[0162] In an embodiment of the present disclosure, refer toFigure 2 , the path planning module is further specifically configured to:

[0163] Determine the initial weight of the heuristic function;

[0164] In response to the target distance being greater than or equal to the first distance threshold, reduce the initial weight by the fifth step size to obtain the first weight;

[0165] In response to the target distance being less than the first distance threshold, increase the initial weight by the sixth step size to obtain the second weight.

[0166] In this embodiment, the initial weight is the initial set value of the proportion of the heuristic function in the evaluation function. The evaluation function is usually the sum of the actual cost function and the heuristic function after weight adjustment. The initial weight determines the influence degree of the heuristic function on path evaluation at the beginning.

[0167] The first distance threshold is a preset distance value, serving as the boundary for judging the length of the target distance.

[0168] The fifth step size is the value used to reduce the weight of the heuristic function when the target distance is greater than or equal to the first distance threshold. The sixth step size is the value used to increase the weight of the heuristic function when the target distance is less than the first distance threshold.

[0169] The first weight is the new weight of the heuristic function obtained after reducing the initial weight when the target distance is greater than or equal to the first distance threshold. The second weight is the new weight of the heuristic function obtained after increasing the initial weight when the target distance is less than the first distance threshold.

[0170] When the target distance of the multi-axis robotic arm is greater than or equal to the preset first distance threshold, it indicates that the target position is relatively far. At this time, the path planning module will subtract the fifth step size from the initial weight to obtain the first weight. Reducing the weight of the heuristic function means that in the evaluation function, the influence of the actual cost function relatively increases, and the algorithm will pay more attention to the actual cost from the starting point to the current point rather than simply relying on the estimate of the target point by the heuristic function. This can make the path planning more robust and avoid large deviations in the search direction caused by the estimation error of the heuristic function.

[0171] When the target distance is less than the first distance threshold, it indicates that the target position is relatively close. The path planning module will add the sixth step size to the initial weight to obtain the second weight. Increasing the weight of the heuristic function can make the algorithm more rely on the estimate of the target point by the heuristic function, speed up the search speed, and find the target path faster because the estimate of the heuristic function is relatively more accurate when the target is close.

[0172] The target path is the optimal motion path of the multi-axis robotic arm from the starting position point to the target position point calculated by the path planning algorithm. The path planning module continuously searches and expands nodes until a path from the starting position point to the target position point is found. During the search process, the node with the minimum evaluation function value is always selected for expansion, and finally the target path for the operation of the multi-axis robotic arm is obtained.

[0173] As can be seen from the above, in this embodiment, by accurately determining the starting position point, the actual position point, and the target position point, and combining the actual cost function and the heuristic function to optimize the path planning, the efficiency and accuracy of the multi-axis robotic arm during operation are ensured. In addition, the path planning module also dynamically adjusts the adjustment coefficient of the actual cost function according to different stages of the working mode, thereby further optimizing the path planning. The above dynamic adjustment strategy enables the robotic arm to maintain the best working state in different stages of drilling and drill unloading, improving the stability and reliability of the overall operation. At the same time, the weight of the heuristic function is also dynamically adjusted according to the change of the target distance, making the path planning more flexible and intelligent. This design not only improves the response speed of the multi-axis robotic arm but also enhances its ability to adapt to complex environments.

[0174] In an embodiment of the present disclosure, referring to Figure 2 , a servo control system for a multi-axis robotic arm of a coal mine drill also includes:

[0175] A tightness detection module, configured to detect the pressure of the robotic arm gripper according to the first pressure sensor to obtain the first force; detect the pressure of the drill rig main machine gripper according to the second pressure sensor to obtain the second force; and send the first force and the second force to the policy determination module to instruct the policy determination module to adjust the operation policy based on the first force and the second force.

[0176] A gripper control module, configured to control the robotic arm gripper to release the drill pipe in response to the second force being greater than or equal to the second force threshold; control the robotic arm gripper to grab the drill pipe in response to the second force being less than the second force threshold; wherein, if the first force is less than the first force threshold, the speed of each motion axis of the multi-axis robotic arm is reduced.

[0177] In this embodiment, the tightness detection module is used to detect the pressure conditions of relevant components to judge the tightness between the multi-axis robotic arm and the drill pipe, and between the drill rig main machine gripper and the drill pipe, that is, whether the drill pipe is firmly grasped.

[0178] The first pressure sensor is installed on the robotic arm gripper and is used to detect the pressure exerted by the robotic arm gripper on the drill pipe. The second pressure sensor is installed on the drill rig mainframe gripper and is used to detect the pressure exerted by the drill rig mainframe gripper on the drill pipe. The first force is the characteristic information obtained by detecting with the first pressure sensor, reflecting the magnitude of the pressure exerted by the robotic arm gripper on the drill pipe. The second force is the characteristic information obtained by detecting with the second pressure sensor, reflecting the magnitude of the pressure exerted by the drill rig mainframe gripper on the drill pipe.

[0179] The gripper control module controls the grasping or releasing action of the robotic arm gripper on the drill pipe according to the magnitude of the second force. The first force threshold is a preset pressure value, which is used to determine whether the grasping force of the robotic arm gripper on the drill pipe is sufficient. The second force threshold is another preset pressure value, which is used to determine whether the clamping force of the drill rig mainframe gripper on the drill pipe is sufficient, so as to determine the action of the robotic arm gripper.

[0180] The gripper control module controls the action of the robotic arm gripper according to the comparison result between the second force and the second force threshold. When the second force is greater than or equal to the second force threshold, it indicates that the drill rig mainframe gripper has firmly clamped the drill pipe. At this time, the robotic arm gripper is controlled to release the drill pipe; when the second force is less than the second force threshold, it indicates that the clamping force of the drill rig mainframe gripper on the drill pipe is insufficient, and the robotic arm gripper needs to continue to grasp the drill pipe.

[0181] If the first force is less than the first force threshold, it indicates that the grasping force of the robotic arm gripper on the drill pipe is insufficient. At this time, in order to prevent the drill pipe from falling during movement, the strategy determination module will reduce the speed of each motion axis of the multi-axis robotic arm to ensure the safety of the operation.

[0182] Reduce the speed of each motion axis of the multi-axis robotic arm to obtain the updated speed V;

[0183] That is, the updated speed V is calculated according to the sixth formula;

[0184] The sixth formula is ;

[0185] Among them, is the first force, that is, the pressure exerted by the robotic arm gripper on the drill pipe; is the first force threshold, that is, the preset pressure value for determining whether the grasping force of the robotic arm gripper is sufficient; is the initial speed of each motion axis of the multi-axis robotic arm, that is, the motion speed when the first force is greater than or equal to the first force threshold; is to reduce the speed of each motion axis of the multi-axis robotic arm, that is, the actual speed of each motion axis of the multi-axis robotic arm after adjustment; is the adjustment coefficient for speed update, and the value range is , which is used to control the degree of speed reduction and can be set according to the actual situation.

[0186] From the above, it can be concluded that this embodiment significantly improves the safety and efficiency of the operation. The tightness detection module can real-time monitor the force characteristics of the robotic arm gripper and the drill rig mainframe gripper, ensuring the firm clamping of the drill pipe and avoiding safety accidents caused by the drill pipe falling off during the operation. At the same time, the gripper control module intelligently controls the actions of the robotic arm gripper according to the force characteristics, realizing the automatic grasping and releasing of the drill pipe and improving the automation level of the operation.

[0187] Reference Figure 3 , Figure 3 Figure 3 is a side view of the servo robotic arm and the drill pipe box. The drill pipes are neatly arranged in rows and columns in the drill pipe box 1 (i.e., the drill pipe bin) of the drill rig. The servo robotic arm 2 (i.e., the multi-axis robotic arm) is driven by 5 servo motors and can move back and forth in the drill pipe box 1 of the drill rig to pick up and place the drill pipes.

[0188] Reference Figure 4 , Figure 4 Figure 4 is a front view of the servo robotic arm. The robotic arm servo control system for automatically adding and removing drill pipes of the fully automatic mine drill rig includes 5 actions: moving back and forth, swinging back and forth, telescoping left and right, swinging left and right, and telescoping long and short.

[0189] Reference Figure 5 and Figure 6 , Figure 5 Figure 5 is the position where the servo robotic arm grabs the drill pipe in the drill pipe box, Figure 6 Figure 6 is the position where the servo robotic arm holds the drill pipe at the drill rig mainframe gripper. The robotic arm (i.e., the servo robotic arm 2) for automatically adding and removing drill pipes of the fully automatic mine drill rig is located between the drill pipe box 1 of the drill rig and the drill rig mainframe 3.

[0190] The automatic drilling process includes:

[0191] The servo robotic arm 2 is responsible for grabbing the drill pipes neatly arranged in rows and columns from the drill pipe box 1 of the fully automatic drill rig; transporting the grabbed drill pipe to the position directly above the rotary center line of the drill rig power head along the established path trajectory, and waiting for the gripper of the fully automatic drill rig mainframe 3 to clamp the transported drill pipe; after the gripper clamps, the servo robotic arm 2 releases the drill pipe, completing the transfer work of the drill pipe from the drill pipe box 1 to the drill rig mainframe 3. Then continue to grab the next drill pipe from the drill pipe box 1 and perform the transfer from the drill pipe box 1 to the drill rig mainframe 3, and so on in a cycle.

[0192] The automatic drill pipe removal process includes:

[0193] The servo manipulator 2 is responsible for gripping the drill pipe from the position of the center line of the rotary motion of the drill power head directly above the main guide rail of the full-automatic drill rig, and waiting for the gripper of the main drill rig 3 to release the gripped drill pipe; after the gripper releases, the manipulator transports the gripped drill pipe to the drill pipe box 1 of the full-automatic drill rig along the established path trajectory, and arranges it neatly in rows and columns, completing the transfer of the drill pipe from the main drill rig 3 to the drill pipe box 1; then it continues to grasp the next drill pipe at the specified position of the main drill rig 3 and perform the transfer from the main drill rig 3 to the drill pipe box 1, and so on in a cycle.

[0194] In this embodiment, the start, stop and switching of different working modes of automatic drilling and automatic drill unloading can be realized at any attitude position.

[0195] The above embodiments are only used to illustrate the technical solutions of the present disclosure, rather than to limit them; although the present disclosure has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the present disclosure in each embodiment.

Claims

1. A servo control system for a multi-axis mechanical arm for a coal mine drilling rig, characterized in that: include: A strategy determination module is used to determine a working mode based on construction requirements, and the same working mode has multiple operation strategies; it is also used to determine a first similarity based on the similarity between the complexity of the work content of the target multi-axis robot arm and the complexity of the work content of the standard multi-axis robot arm; if the first similarity is greater than or equal to a first similarity threshold, the complexity of the work content of the target multi-axis robot arm matches the complexity of the work content of the standard multi-axis robot arm, and the target operation strategy is determined based on the first similarity; each operation strategy corresponds to a standard multi-axis robot arm work content; the operation strategy includes the operation parameters of the multi-axis robot arm; A distance determination module, used to determine a target distance based on the working mode, wherein the target distance is the distance between the multi-axis mechanical arm and the target drill rod; A servo control module, used to control a plurality of servo motors based on the target operation strategy and the target distance, each servo motor corresponding to a part of the multi-axis robotic arm; The strategy determination module and the distance determination module are both connected to the servo control module; Wherein, when the strategy determination module determines the first similarity based on the similarity between the complexity of the work content of the target multi-axis robotic arm and the complexity of the work content of the standard multi-axis robotic arm, it is specifically used to: Determine the number of operation steps required by the target multi-axis robot to complete the construction requirements ; The similarity between the number of operation steps required to complete the construction by the target multi-axis robot and the number of operation steps required to complete the construction by the standard multi-axis robot is calculated based on the first formula. ; Each construction requirement corresponds to a number of operation steps. The number of operation steps corresponding to the construction requirement is taken as ; The first formula is ; in, The number of operation steps required for a standard multi-axis robot to complete the construction requirements is a preset value; The action type set is ,in, For different action types, ; Determine the motion type vector of the target multi-axis robot ;in, The action type in the target multi-axis robot work content Number of occurrences; Determine the motion type vector of a standard multi-axis robot ;in, For the action type in the standard multi-axis robot work content Number of occurrences; Calculate the similarity between the action type vector of the target multi-axis robot and the action type vector of the standard multi-axis robot based on the second formula ; The second formula is ; ； ； ； A weighted calculation is performed based on the operation step similarity and the action type similarity to obtain a first similarity, including: Determine the weight of the similarity of operation steps , ; Determine the weight of action type similarity , ; The first similarity is: ,and .

2. A servo control system for a multi-axis mechanical arm for a coal mine drill according to claim 1, characterized in that: The work content includes operation steps; The strategy determination module is specifically used to: In response to the complexity of the operation step being greater than or equal to the first complexity, taking the first operation strategy as the target operation strategy; In response to the complexity of the operation step being less than the first complexity, using the second operation strategy as the target operation strategy; The working efficiency of the first operation strategy is greater than the working efficiency of the second operation strategy, and the accuracy of the first operation strategy is less than the accuracy of the second operation strategy.

3. A servo control system for a multi-axis mechanical arm for a coal mine drill according to claim 2, characterized in that: The operating parameters include the speed, acceleration and torque of each motion axis of the multi-axis robot; The strategy determination module is also used to: Calculating the speed of each motion axis of the multi-axis robot arm based on the work efficiency, the accuracy, and the target work content corresponding to each operation step; Calculating the acceleration of each motion axis of the multi-axis robotic arm based on the work efficiency, the accuracy, and the target work content corresponding to each operation step; The torque of each motion axis of the multi-axis robot arm is calculated based on the target work content corresponding to each operation step.

4. The servo control system of a multi-axis mechanical arm for a coal mine drill according to claim 1, characterized in that: Also includes: The posture detection module is used to determine the posture characteristics of the multi-axis robot arm based on the encoder and send the posture characteristics to the servo control module.

5. A servo control system for a multi-axis mechanical arm for a coal mine drill according to claim 4, characterized in that: Also includes: A path planning module, used to process the target distance and the posture feature based on a path planning algorithm to obtain a target path for the multi-axis robot arm to operate; The input end of the path planning module is connected to the strategy determination module and the posture detection module respectively; The output end of the path planning module is connected to the servo control module.

6. A servo control system for a multi-axis mechanical arm for a coal mine drill according to claim 5, characterized in that: The path planning module is specifically used for: Determining a starting position point, an actual position point, and a target position point of the multi-axis robotic arm; Determining an actual cost function of the path planning algorithm based on the starting position point and the actual position point; Determine a heuristic function of the path planning algorithm based on the actual position point and the target position point; Determine an evaluation function based on the addition of the actual cost function and the heuristic function; The actual position point is input into the evaluation function to obtain the target path of the multi-axis robot arm.

7. A servo control system for a multi-axis mechanical arm for a coal mine drill according to claim 6, characterized in that: The working modes include a drilling working mode and a drill unloading working mode; The path planning module is further specifically used for: Determining an initial adjustment coefficient of the actual cost function; In response to the drilling operation mode being the first stage, increasing the initial adjustment coefficient according to the first step length to obtain a first adjustment coefficient; In response to the drilling operation mode being in the second stage, reducing the first adjustment coefficient according to a second step length to obtain a second adjustment coefficient; In response to the drill unloading working mode being the first stage, increasing the initial adjustment coefficient according to a third step length to obtain a third adjustment coefficient; In response to the drill unloading working mode being the second stage, reducing the third adjustment coefficient according to a fourth step length to obtain a fourth adjustment coefficient; The first adjustment coefficient is greater than the second adjustment coefficient, and the third adjustment coefficient is greater than the fourth adjustment coefficient.

8. The servo control system of a multi-axis mechanical arm for a coal mine drill according to claim 6, characterized in that: The path planning module is further specifically used for: Determining an initial weight of the heuristic function; In response to the target distance being greater than or equal to a first distance threshold, reducing the initial weight according to a fifth step length to obtain a first weight; In response to the target distance being less than a first distance threshold, the initial weight is increased according to a sixth step to obtain a second weight.

9. The servo control system of a multi-axis mechanical arm for a coal mine drill according to claim 1, characterized in that: Also includes: A tightness detection module, used for detecting the pressure of the gripper of the robot arm according to the first pressure sensor to obtain a first force; The second pressure sensor detects the pressure of the drill host clamp to obtain a second force; the first force and the second force are sent to the strategy determination module to instruct the strategy determination module to adjust the operation strategy based on the first force and the second force.

10. A servo control system for a multi-axis mechanical arm for a coal mine drill according to claim 9, characterized in that: Also includes: A gripper control module, configured to control the mechanical arm gripper to release the drill rod in response to the second force being greater than or equal to a second force threshold; and to control the mechanical arm gripper to grab the drill rod in response to the second force being less than the second force threshold; If the first force is less than a first force threshold, the speed of each movement axis of the multi-axis robotic arm is reduced.

Images