Servo control system of multi-shaft mechanical arm for coal mine drilling machine

Abstract

The invention provides a servo control system of a multi-shaft mechanical arm for a coal mine drilling machine, and belongs to the technical field of mechanical arm control. The servo control system of the multi-shaft mechanical arm for the coal mine drilling machine comprises a strategy determining module used for determining a working mode based on construction requirements, and the same working mode has a plurality of operation strategies; the selection module is further used for selecting an operation strategy matched with the complexity of the working content of the multi-axis mechanical arm from the multiple operation strategies as a target operation strategy; the operation strategy comprises operation parameters of the multi-axis mechanical arm; the distance determining module is used for determining a target distance based on the working mode, and the target distance is the distance between the multi-axis mechanical arm and the target drill rod; and the servo control module is used for controlling a plurality of servo motors based on the target operation strategy and the target distance, and each servo motor corresponds to one part of the multi-axis mechanical arm. The problem of low working efficiency of servo control can be solved.

Description

Technical Field

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

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

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

[0004] The disclosed embodiment provides a servo control system for a multi-axis mechanical arm for a coal mine drill to solve the problem of low working efficiency of the servo control.

[0005] The embodiment of the present disclosure provides a servo control system for a multi-axis mechanical arm for a coal mine drilling rig, comprising: A strategy determination module is used to determine the working mode based on the construction requirements, and the same working mode has multiple operation strategies; it is also used to select an operation strategy that matches the complexity of the work content of the multi-axis robot arm from the multiple operation strategies as the target operation strategy; the operation strategy includes the operation parameters of the multi-axis robot arm; A distance determination module is used to determine a target distance based on a working mode, where the target distance is the distance between the multi-axis manipulator and the target drill rod; A servo control module is used to control multiple servo motors based on a target operation strategy and a target distance, each servo motor corresponding to a part of the multi-axis robot arm; The strategy determination module and the distance determination module are both connected to the servo control module.

[0006] In an exemplary embodiment of the present disclosure, 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, taking the second operation strategy as the target operation strategy; Among them, 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.

[0007] In one exemplary embodiment of the present disclosure, the operating parameters include the speed, acceleration, and torque of each axis of motion of the multi-axis robot; The strategy determination module is also used to: Calculate the speed of each motion axis of the multi-axis robot based on work efficiency, accuracy, and target work content corresponding to each operation step; Calculate the acceleration of each motion axis of the multi-axis robot based on work efficiency, accuracy, and 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.

[0008] In an exemplary embodiment of the present disclosure, a servo control system for a multi-axis mechanical arm for a coal mine drill further 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.

[0009] In an exemplary embodiment of the present disclosure, a servo control system for a multi-axis mechanical arm for a coal mine drill further includes: The path planning module is used to process the target distance and posture characteristics based on the path planning algorithm to obtain the 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.

[0010] In an exemplary embodiment of the present disclosure, the path planning module is specifically used to: Determine the starting position point, actual position point and target position point of the multi-axis robot arm; Determine the actual cost function of the path planning algorithm based on the starting position point and the actual position point; Determine the heuristic function of the path planning algorithm based on the actual position point and the target position point; Determine the 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.

[0011] In an exemplary embodiment of the present disclosure, the working mode includes a drilling working mode and a drill unloading working mode; The path planning module is also used for: Determine the initial adjustment coefficient of the actual cost function; In response to the drilling working 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 working mode being 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; 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.

[0012] In an exemplary embodiment of the present disclosure, the path planning module is further configured to: Determine the initial weight of the heuristic function; In response to the target distance being greater than or equal to the 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 the first distance threshold, the initial weight is increased according to a sixth step length to obtain a second weight.

[0013] In an exemplary embodiment of the present disclosure, a servo control system for a multi-axis mechanical arm for a coal mine drill further includes: The tightness detection module is used to detect the pressure of the gripper of the mechanical arm according to the first pressure sensor to obtain the first force; and detect the pressure of the clamp of the drilling rig mainframe according to the second pressure sensor to obtain the 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.

[0014] In an exemplary embodiment of the present disclosure, a servo control system for a multi-axis mechanical arm for a coal mine drill further includes: a gripper control module, configured to control the gripper of the mechanical arm to release the drill rod in response to the second force being greater than or equal to a second force threshold; In response to the second force being less than a second force threshold, controlling the mechanical arm gripper to grab the drill rod; 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.

[0015] The beneficial effects of a servo control system for a multi-axis mechanical arm for a coal mine drill provided by the embodiment of the present disclosure are: The present disclosure can flexibly adapt to different construction requirements through the strategy determination module, 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 robot arm and the target drill rod, and accurately locate the position of the multi-axis robot arm. Afterwards, the servo control module can achieve precise control of the servo motor based on the target operation strategy and the target distance, thereby achieving coordinated work of various parts of the multi-axis robot arm, greatly improving work efficiency. Therefore, the present disclosure can solve the problem of low work efficiency of servo control. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] In order to more clearly illustrate the technical solutions in the embodiments of the present disclosure, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present disclosure. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0017] Figure 1 It is a structural schematic diagram of a servo control system of a multi-axis mechanical arm for a coal mine drilling rig provided by an embodiment of the present disclosure; Figure 2 It is a structural schematic diagram of another servo control system of a multi-axis mechanical arm for a coal mine drilling rig provided by an embodiment of the present disclosure; Figure 3 is a side view of a servo mechanical arm and a drill rod box provided by an embodiment of the present disclosure; Figure 4 is a front view of a servo mechanical arm provided by an embodiment of the present disclosure; Figure 5 The servo mechanical arm provided in the embodiment of the present disclosure grabs the drill rod position in the drill rod box; Figure 6 The servo mechanical arm provided by the embodiment of the present disclosure is in the drill rod position of the drilling rig mainframe clamp. DETAILED DESCRIPTION

[0018] In order to enable people in the technical field to better understand the present solution, the technical solution in the embodiment of the present solution will be clearly described below in conjunction with the drawings in the embodiment of the present solution. Obviously, the described embodiment is an embodiment of a part of the present solution, not all of the embodiments. Based on the embodiments in the present solution, all other embodiments obtained by ordinary technicians in the field without creative work should fall within the scope of protection of the present solution.

[0019] The term "including" and any other variations in the specification and claims of this solution and the above drawings mean "including but not limited to", and is intended to cover non-exclusive inclusions and is not limited to the examples listed in the text. In addition, the terms "first" and "second" are used to distinguish different objects, not to describe a specific order.

[0020] The following is a detailed description of the implementation of the present disclosure in conjunction with the specific drawings: Figure 1 This is a schematic diagram of the structure of a servo control system for a multi-axis mechanical arm for a coal mine drill provided by an embodiment of the present disclosure. Figure 1 The servo control system of a multi-axis mechanical arm for a coal mine drilling rig comprises: A strategy determination module is used to determine the working mode based on the construction requirements, and the same working mode has multiple operation strategies; it is also used to select an operation strategy that matches the complexity of the work content of the multi-axis robot arm from the multiple operation strategies as the target operation strategy; the operation strategy includes the operation parameters of the multi-axis robot arm; A distance determination module is used to determine a target distance based on a working mode, where the target distance is the distance between the multi-axis manipulator and the target drill rod; A servo control module is used to control multiple servo motors based on a target operation strategy and a target distance, each servo motor corresponding to a part of the multi-axis robot arm; The strategy determination module and the distance determination module are both connected to the servo control module.

[0021] In this embodiment, the construction requirement is whether the current coal mining is started or ended. If the construction requirement is the start of the current coal mining, the working mode is the drilling working mode of the servo control system; if the construction requirement is the end of the current coal mining, the working mode is the unloading drilling 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 work efficiency or work accuracy.

[0022] In each working mode, multiple operation strategies can be formulated in advance according to the complexity of the work content of the multi-axis robot arm, that is, 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 can be calculated to determine the target operation strategy. The complexity of the work content of the target multi-axis robot arm is the complexity corresponding to the current work content, and the complexity of the work content of the standard multi-axis robot arm is the complexity corresponding to the pre-planned work content.

[0023] Determining 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 includes: 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 can be used 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; Suppose the action type set is ,in, For different action types, , the action type can be translation, rotation, grabbing, etc.; 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.

[0024] Each operation strategy corresponds to a standard multi-axis robot arm work content, so if the first similarity is greater than or equal to the first similarity threshold, the complexity of the target multi-axis robot arm work content matches the complexity of the standard multi-axis robot arm work content, that is, the operation strategy is selected as the target operation strategy. Among them, the first similarity threshold is a pre-set reference value, which can be set according to experience.

[0025] 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 .

[0026] The operation strategy is to set the operation parameters of the multi-axis robot arm, which may include the movement speed, acceleration and torque of each movement axis. For example, if the selected operation strategy is a fast movement strategy to improve work efficiency, the corresponding operation parameter adjustment may be to increase the maximum speed and acceleration of each axis.

[0027] Different working modes determine different operating relationships between the multi-axis robot arm and the target drill rod, which in turn correspond to different target distances. The target drill rod can be a drill rod in the drill rod bin or a drill rod at the drill rig mainframe clamp. For example, in the working mode of preparing to grab the drill rod, the target distance can be the distance between the robot arm end effector and the drill rod to be grabbed. The distance determination module will accurately calculate or determine the target distance between the multi-axis robot arm and the target drill rod based on the current working mode. This distance information is crucial for the precise control of the subsequent servo motor, and it provides a key position reference data for the servo control module.

[0028] The servo control module receives the target operation strategy (i.e., the determined operation parameters) from the strategy determination module and the target distance information from the distance determination module. Then, multiple servo motors are precisely controlled based on the above information. Since each servo motor corresponds to a part of the multi-axis robotic arm, such as the rotation of a joint or the linear motion of an axis, the servo control module will send corresponding control signals to each servo motor based on the speed, acceleration, torque and other parameter requirements 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 parameters in the target operation strategy, the corresponding servo motor is controlled to rotate at an appropriate speed to drive the corresponding part of the robotic arm to move, ensuring that the robotic arm accurately completes the task according to the predetermined operation strategy.

[0029] It can be concluded from the above that the present disclosure can flexibly adapt to different construction requirements through the strategy determination module, 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 robot arm and the target drill rod, and accurately locate the position of the multi-axis robot arm. Afterwards, the servo control module can achieve precise control of the servo motor based on the target operation strategy and target distance, thereby achieving coordinated work of various parts of the multi-axis robot arm, greatly improving work efficiency. Therefore, the present disclosure can solve the problem of low work efficiency of servo control.

[0030] In one embodiment of the present disclosure, reference Figure 2 , 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, taking the second operation strategy as the target operation strategy; Among them, 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.

[0031] In this embodiment, the operation steps are the sequence of specific actions performed by the multi-axis robot to complete the construction requirements, for example, how many times the drill rod needs to be transferred from the drill rod bin to the drilling rig mainframe, whether the multi-axis robot needs to move forward and backward, etc.

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

[0033] The first complexity is a pre-set threshold value, which is used as a standard to distinguish the complexity of the operation steps. It can be set based on experience and used for judgment in the strategy determination module. The first operation strategy is a multi-axis robot arm operation mode formulated for the case of high complexity of the operation steps. Its characteristic is that it focuses on work efficiency, that is, it can complete the work task in a shorter time, but it is relatively weak in accuracy. The second operation strategy is a multi-axis robot arm operation mode formulated for the case of low complexity of the operation steps. Its characteristic is that it emphasizes accuracy, and its work efficiency is lower than that of the first operation strategy.

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

[0035] In one embodiment of the present disclosure, reference Figure 2 ,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: Calculate the speed of each motion axis of the multi-axis robot based on work efficiency, accuracy, and target work content corresponding to each operation step; Calculate the acceleration of each motion axis of the multi-axis robot based on work efficiency, accuracy, and 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.

[0036] In this embodiment, the operating parameters are parameters that describe the operating status of the multi-axis robot. Among them, the speed determines the speed of each motion axis of the multi-axis robot; the acceleration controls the speed of the motion axis speed change, affecting the robot arm start, stop and acceleration and deceleration process; the torque reflects the magnitude of the rotational force that the motion axis can output.

[0037] Work efficiency is the amount of work completed by the multi-axis robot in a unit of time, which can reflect the speed at which the multi-axis robot completes the task. Accuracy is the degree of compliance with the target requirements when the multi-axis robot completes the task, which reflects the accuracy of the multi-axis robot operation. The target work content is the specific work requirements expected to be achieved for each operation step. Different operation steps correspond to different target work contents.

[0038] The strategy determination module will comprehensively consider the work efficiency requirements, accuracy standards, and the specific goals to be achieved in each operation step to determine the moving speed of each motion axis of the multi-axis robot. If the work efficiency requirements are high and the accuracy requirements are relatively low, and the target work content allows faster operation, then the speed of the motion axis can be increased; conversely, if the accuracy requirements are extremely high and the target work content requires delicate operation, the speed can be reduced.

[0039] The strategy determination module considers work efficiency, accuracy, and the target work content of each operation step to determine the speed of the motion axis change. If the work efficiency requirement is high, a larger acceleration may be required to enable the robot arm to quickly reach the working speed; but if the accuracy requirement is high, in order to avoid the impact caused by too fast speed change that affects the operation accuracy, the acceleration can be reduced.

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

[0041] Calculate the speed of each motion axis of the multi-axis robot based on the third formula ; The third formula is ; in, For the The speed of the moving axis in each operation step, For the The time allowed for each operation step, , The number of steps required to complete a task. The total time allowed to complete a work task, , is work efficiency, that is, the amount of tasks completed per unit time; The accuracy can be determined by the allowable range of position error, and the value range is , the smaller the value, the higher the accuracy. As the basic accuracy; For the The distance the motion axis needs to move in each operation step. For the The difficulty coefficient of the target work content corresponding to the operation steps is in the range of The larger the value, the more difficult it is to complete the step. It can be set based on experience.

[0042] In order to meet the required time without considering the accuracy and difficulty of the target work content, Distance completed The basic speed required for the movement axis; To consider the adjustment factor of accuracy requirements, when the accuracy Greater than basic accuracy When the coefficient is greater than 1, the speed can be reduced accordingly to ensure the accuracy of the operation; on the contrary, when the accuracy Less than basic accuracy When the coefficient is less than 1, the speed can be increased accordingly; To consider the adjustment coefficient of the difficulty of the target work content, the target work content difficulty coefficient The larger the difficulty is, the smaller the coefficient is, and the speed will be reduced to cope with more complex operations; the smaller the difficulty coefficient is, the speed can be relatively improved.

[0043] Calculate the acceleration of each motion axis of the multi-axis robot based on the fourth formula ; The fourth formula is ; in, For the The speed of the moving axis at the end of each operation step, For the The time for each operation step, is the time proportion coefficient of the speed change transition stage, .

[0044] Calculate the torque of each motion axis of the multi-axis robot based on the fifth formula ; The fifth formula is ; in, For the The torque required for the moving axis in each operation step, For the The load force that the moving axis needs to overcome in each operation step, is the distance from the motion axis to the load application point, For the The friction force on the moving axis during each operation step, is the correction factor related to the difficulty of the target work content ( , the greater the difficulty, the larger the value).

[0045] It can be concluded from the above that this embodiment not only ensures the efficiency and accuracy of the multi-axis robot arm when performing tasks, but also greatly improves the stability and reliability of the operation. In addition, the motion parameters of each axis are flexibly adjusted according to different operation steps, so that the robot arm can better adapt to various work tasks, improving the overall flexibility and adaptability.

[0046] In one embodiment of the present disclosure, reference Figure 2 , a servo control system for a multi-axis mechanical arm for a coal mine drilling rig, further comprising: 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.

[0047] The path planning module is used to process the target distance and posture characteristics based on the path planning algorithm to obtain the 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.

[0048] In this embodiment, the posture characteristics include the position characteristics, angle characteristics and direction characteristics of the multi-axis robot. The encoder is a sensor that can measure the rotation or linear displacement of a mechanical part. In the multi-axis robot, the encoder can be installed at each joint to measure the rotation position, angle and direction of the joint, so as to determine the posture of the multi-axis robot. The posture detection module uses the data measured by the encoder to determine the posture characteristics of the multi-axis robot through calculation and analysis, and then sends the above posture characteristic information to the servo control module so that the servo control module performs subsequent control operations according to the current posture of the robot.

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

[0050] The path planning module receives the operation strategy related information (such as speed, acceleration and other operation parameters) from the strategy determination module and the posture feature information from the posture detection module, combines it with the target distance, and uses the path planning algorithm to process it to calculate the target path of the multi-axis robot arm from the current posture to the target position.

[0051] The input end of the path planning module is connected to the strategy determination module and the posture detection module to obtain 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 robot arm to run according to the path.

[0052] It can be concluded from the above that the present embodiment can obtain the posture characteristics of the robot arm in real time and accurately. The present embodiment can intelligently plan the optimal path for the robot arm to operate according to the target distance and posture characteristics, greatly improving the operation efficiency and accuracy.

[0053] In one embodiment of the present disclosure, reference Figure 2 , path planning module, specifically used for: Determine the starting position point, actual position point and target position point of the multi-axis robot arm; Determine the actual cost function of the path planning algorithm based on the starting position point and the actual position point; Determine the heuristic function of the path planning algorithm based on the actual position point and the target position point; Determine the 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.

[0054] In this embodiment, the starting position point is the spatial position where the multi-axis robot arm starts to perform the work task, which is the starting point of the path planning. The actual position point is the spatial position where the multi-axis robot arm is currently located during the path planning process, which will change with the movement of the multi-axis robot arm. The target position point is the final spatial position that the multi-axis robot arm needs to reach, which is the goal to be achieved by the path planning, that is, the drill rod bin or the drilling rig host.

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

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

[0057] The evaluation function combines the actual cost function and the heuristic function to evaluate the pros and cons of each possible path node to determine the next search direction. The evaluation function is ,in, 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 more quickly.

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

[0059] In one embodiment of the present disclosure, reference Figure 2 , the working modes include drilling working mode and drill unloading working mode; The path planning module is also used for: Determine the initial adjustment coefficient of the actual cost function; In response to the drilling working 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 working mode being 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; 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.

[0060] 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. 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 final planned path.

[0061] 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 working mode, and can be set according to experience; 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 unloading drill working mode, and can be set according to experience.

[0062] When the multi-axis robot is in the first stage of the drilling working mode, the path planning module adds the initial adjustment coefficient to the first step length to obtain the first adjustment coefficient. The first stage is the initial stage, and the path needs to be planned more carefully. Increasing the adjustment coefficient can make the cost of some paths relatively higher, thereby guiding the robot to choose a safer and more appropriate path.

[0063] In the second stage of the drilling work 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 robot has completed some preliminary preparations, or the working environment has become relatively stable. Reducing the adjustment coefficient can make path planning more flexible and improve work efficiency.

[0064] When the robot arm is in the first stage of the drill unloading mode, the path planning module adds the third step length to the initial adjustment coefficient to obtain the third adjustment coefficient. The first stage of drill unloading requires special care to avoid damaging the drill rod. Increasing the adjustment coefficient can make the path selection more conservative.

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

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

[0067] The target weight may include a first weight and a second weight.

[0068] In one embodiment of the present disclosure, reference Figure 2 , the path planning module is also used for: Determine the initial weight of the heuristic function; In response to the target distance being greater than or equal to the 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 the first distance threshold, the initial weight is increased according to a sixth step length to obtain a second weight.

[0069] In this embodiment, the initial weight is the initial setting 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 of the heuristic function on the path evaluation at the beginning.

[0070] The first distance threshold is a pre-set distance value, which serves as a limit for determining the distance to the target.

[0071] The fifth step length is a 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 length is a value used to increase the weight of the heuristic function when the target distance is less than the first distance threshold.

[0072] The first weight is the new weight of the heuristic function obtained by 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 by increasing the initial weight when the target distance is less than the first distance threshold.

[0073] When the target distance of the multi-axis robot is greater than or equal to the preset first distance threshold, it means that the target position is relatively far away. At this time, the path planning module will subtract the fifth step length 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 is relatively increased. The algorithm will pay more attention to the actual cost from the starting point to the current point, rather than simply relying on the heuristic function's estimate of the target point. This can make path planning more robust and avoid excessive deviation in the search direction due to the estimation error of the heuristic function.

[0074] 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 length to the initial weight to obtain the second weight. Increasing the weight of the heuristic function allows the algorithm to rely more on the heuristic function's estimate of the target point, speeding up the search and finding the target path faster, because when the target is closer, the heuristic function's estimate is relatively more accurate.

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

[0076] It can be concluded from the above that this embodiment ensures the efficiency and accuracy of the multi-axis robot arm during operation by accurately determining the starting position point, the actual position point and the target position point, and optimizing the path planning by combining the actual cost function and the heuristic function. In addition, the path planning module dynamically adjusts the adjustment coefficient of the actual cost function according to the different stages of the working mode, thereby further optimizing the path planning. The above dynamic adjustment strategy enables the robot arm to maintain the best working state in different stages of drilling and unloading, thereby 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 robot arm, but also enhances its ability to adapt to complex environments.

[0077] In one embodiment of the present disclosure, reference Figure 2 , a servo control system for a multi-axis mechanical arm for a coal mine drilling rig, further comprising: The tightness detection module is used to detect the pressure of the robot arm gripper according to the first pressure sensor to obtain a first force; detect the pressure of the drilling rig mainframe clamp according to the second pressure sensor to obtain a second force; send the first force and the second force 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.

[0078] The clamp control module is used to control the robotic arm gripper to release the drill rod in response to the second force being greater than or equal to the second force threshold; and to control the robotic arm gripper to grab the drill rod 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 movement axis of the multi-axis robotic arm is reduced.

[0079] In this embodiment, the tightness detection module is used to detect the pressure of related components to determine the tightness between the multi-axis robot arm and the drill rod, and between the drilling rig mainframe clamp and the drill rod, that is, whether the drill rod is gripped tightly.

[0080] The first pressure sensor is installed on the gripper of the manipulator arm, and is used to detect the pressure applied by the gripper of the manipulator arm to the drill rod. The second pressure sensor is installed on the mainframe clamp of the drilling rig, and is used to detect the pressure applied by the mainframe clamp of the drilling rig to the drill rod. The first force is detected by the first pressure sensor, and reflects the characteristic information of the pressure applied by the gripper of the manipulator arm to the drill rod. The second force is detected by the second pressure sensor, and reflects the characteristic information of the pressure applied by the mainframe clamp of the drilling rig to the drill rod.

[0081] The gripper control module controls the gripping or releasing action of the manipulator gripper on the drill rod according to the magnitude of the second force. The first force threshold is a pre-set pressure value, which is used to determine whether the gripping force of the manipulator gripper on the drill rod is sufficient. The second force threshold is another pre-set pressure value, which is used to determine whether the gripping force of the drilling rig mainframe gripper on the drill rod is sufficient, thereby determining the action of the manipulator gripper.

[0082] The gripper control module controls the action of the mechanical 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 gripped the drill rod, and the mechanical arm gripper is controlled to release the drill rod; when the second force is less than the second force threshold, it indicates that the gripper of the drill rig mainframe is not strong enough to grip the drill rod, and the mechanical arm gripper needs to continue to grip the drill rod.

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

[0084] Reduce the speed of each motion axis of the multi-axis robot to obtain the update speed V; That is, the update speed V is calculated according to the sixth formula; The sixth formula is ; in, is the first force, i.e., the pressure exerted by the gripper of the robotic arm on the drill pipe; is the first force threshold, i.e., a pre-set pressure value for judging whether the gripping force of the robot arm gripper is sufficient; is the initial speed of each motion axis of the multi-axis robot arm, that is, the motion speed when the first force is greater than or equal to the first force threshold; To reduce the speed of each moving axis of the multi-axis robot arm, that is, the actual speed of each moving axis of the multi-axis robot arm after adjustment; is the adjustment coefficient of speed update, the value range is , used to control the degree of speed reduction, which can be set according to actual conditions.

[0085] It can be concluded from the above that this embodiment significantly improves the safety and efficiency of the operation. The tightness detection module can monitor the force characteristics of the mechanical arm gripper and the drill rig mainframe clamp in real time to ensure that the drill rod is firmly clamped and avoid safety accidents caused by the drill rod falling off during the operation. At the same time, the clamp control module intelligently controls the movement of the mechanical arm gripper according to the force characteristics, realizes the automatic grasping and release of the drill rod, and improves the level of operation automation.

[0086] refer to Figure 3 , Figure 3 The figure is a side view of the servo robot arm and the drill rod box. The drill rods are neatly arranged in rows and columns in the drill rod box 1 (i.e., the drill rod bin) of the drilling rig. The servo robot arm 2 (i.e., the multi-axis robot arm) is driven by five servo motors and can move back and forth in the drill rod box 1 of the drilling rig to pick up and put down the drill rods.

[0087] refer to Figure 4 , Figure 4 This is the front view of the servo mechanical arm. The servo control system of the mechanical arm for automatic loading and unloading of drill rods for fully automatic mining drill rigs includes five actions: forward and backward movement, forward and backward swing, left and right extension, left and right swing, and length extension.

[0088] refer to Figure 5 and Figure 6 , Figure 5 The servo robot arm grabs the drill rod position in the drill rod box. Figure 6 The servo mechanical arm is in the drill rod position of the drilling rig mainframe clamp. The mechanical arm for automatically loading and unloading drill rods for the fully automatic mining drilling rig (i.e., the servo mechanical arm 2) is located between the drilling rig drill rod box 1 and the drilling rig mainframe 3.

[0089] The automatic drilling process includes: The servo mechanical arm 2 is responsible for grabbing the drill rods neatly arranged in rows and columns from the drill rod box 1 of the fully automatic drilling rig; it transports the grabbed drill rods to the rotary centerline position of the drilling rig power head just above the mainframe guide rail of the fully automatic drilling rig according to the predetermined path trajectory, waiting for the clamp of the mainframe 3 of the fully automatic drilling rig to clamp the transported drill rod; after the clamp is clamped, the servo mechanical arm 2 releases the drill rod, completing the transfer of the drill rod from the drill rod box 1 to the mainframe 3 of the drilling rig. Then it continues to grab the next drill rod from the drill rod box 1 of the drilling rig, and transfers it from the drill rod box 1 to the mainframe 3 of the drilling rig, and the work is cyclical.

[0090] The automatic drill unloading process includes: The servo robot arm 2 is responsible for grasping the drill rod from the rotation center line position of the drill rig power head just above the main machine guide rail of the fully automatic drilling rig, and waiting for the clamp of the drilling rig main machine 3 to release the grasped drill rod; after the clamp is released, the robot arm transports the grasped drill rod to the drilling rig drill rod box 1 of the fully automatic drilling rig according to the established path trajectory, and arranges it neatly in rows and columns, completing the transfer of the drill rod from the drilling rig main machine 3 to the drilling rig drill rod box 1; then continues to grasp the next drill rod at the specified position of the drilling rig main machine 3, and transfers it from the drilling rig main machine 3 to the drilling rig drill rod box 1, and repeats the cycle.

[0091] This embodiment can realize the start, stop and switching of different working modes of automatic drilling and automatic drill unloading at any posture position.

[0092] 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 aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present disclosure.

Claims

1. A servo control system for a multi-axis mechanical arm for a coal mine drilling rig, characterized in that: include: Strategy determination module, used to determine the working mode based on construction requirements. The same working mode has multiple operation strategies; It is also used to select an operation strategy that matches the complexity of the work content of the multi-axis robot arm from multiple operation strategies as a target operation strategy; the operation strategy includes 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.

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