Drill jumbo and mechanical arm control method and device thereof

Abstract

The invention belongs to the technical field of rock drilling control, and particularly relates to a drill jumbo and a mechanical arm control method and device thereof. The method comprises the steps that S1, after a drill jumbo reaches a construction position, the tail end target pose of a mechanical arm of the drill jumbo is determined according to a tunnel face hole arrangement diagram; s2, according to the tail end target pose, any discrete rotation angle and any discrete joint angle, a DH kinematic model is combined to obtain a group of inverse solutions corresponding to the freedom degrees of the discrete rotation angle and the discrete joint angle; s3, the step S2 is repeated until the set number of discrete rotation angles and all discrete joint angles are traversed, and inverse solutions of the corresponding group number are obtained; selecting an optimal solution from the inverse solutions; according to the optimal solution and the discrete rotation angle corresponding to the optimal solution, the mechanical arm is controlled to drill holes; the error between the positive solution corresponding to the optimal solution and the tail end target pose in the height direction is minimum. The technical problems of long time consumption and low solving speed of a control algorithm in the prior art are solved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of rock drilling control, and particularly relates to a rock drilling jumbo and a mechanical arm control method and device thereof. BACKGROUND

[0002] The motion control of a mechanical arm, especially the control of a multi-degree-of-freedom redundant mechanical arm, has always been a key and difficult problem in the field of scientific research. With the rapid development of society, higher standards are put forward for the response speed and autonomous decision-making ability of robot control. In recent years, with the increasing in-depth research of tunnel construction unmanned, automation and intelligentization, especially the application of full-computer rock drilling jumbo in drill-and-blast construction. Compared with the last generation of semi-computer rock drilling jumbo, although the number of operators is reduced and safety is improved, the intelligent control still has problems such as weak environmental adaptability and low operation efficiency. Therefore, developing a more efficient intelligent construction method has become a key issue to be solved at present.

[0003] The inverse solution algorithm of a redundant mechanical arm mainly includes an analytical method and an iterative method. The analytical method has problems of difficult solution and solution possibly not in the working limit interval of the arm support; the solution process of the iterative method depends on the iteration parameters and initial value, and only one solution is obtained by single iteration, which has problems of slow solution speed, dependence on iteration parameters, only one solution obtained by single iteration and easy to fall into local minimum value, and complex parameter adjustment.

[0004] The Chinese invention patent application publication with the application publication number CN118481598A and the application publication date of August 13, 2024 discloses a drilling control method of a rock drilling jumbo. In the scheme, the corresponding relationship between each degree-of-freedom combination of the mechanical arm and the end pose of the mechanical arm is established before construction and stored in the database; after the target pose of the end of the mechanical arm is determined during the construction stage, the corresponding degree-of-freedom combination can be determined according to the closeness between the target pose and each end pose in the corresponding relationship. SUMMARY

[0005] The purpose of the application is to provide a rock drilling jumbo and a mechanical arm control method and device thereof, so as to solve the technical problems of long time consumption and slow solution speed of the control algorithm in the prior art.

[0006] To solve the above technical problems, the technical scheme of a mechanical arm control method of a rock drilling jumbo provided by the application is as follows: a mechanical arm control method of a rock drilling jumbo, the method comprising:

[0007] S1, after the rock drilling jumbo arrives at the construction position, determining the end target pose of the mechanical arm of the rock drilling jumbo according to the hole layout diagram of the working face;

[0008] S2. Based on the end target pose, any discrete rotation angle and any discrete joint angle, combine the DH kinematic model to obtain a set of inverse solutions corresponding to the degrees of freedom of the discrete rotation angle and the discrete joint angle.

[0009] The discrete rotation angle is obtained by discretizing the rotation angle in the end-effector pose of the robotic arm according to a first interval angle; the discrete joint is obtained by discretizing the working range of one degree of freedom of the robotic arm according to a second interval angle.

[0010] S3. Repeat S2 until a set number of discrete rotation angles and all discrete joint angles are traversed to obtain the corresponding number of inverse solutions; select the optimal solution from the inverse solutions; control the robot arm to drill according to the optimal solution and its corresponding discrete rotation angle.

[0011] The error between the optimal solution and the correct solution in the height direction is the smallest.

[0012] The beneficial effects of the above technical solution are as follows: In the process of solving the robotic arm problem, this invention, combined with the actual application scenario of the rock drilling rig (drilling operation at the working face), discretizes the rotation angle, which is not closely related to the drilling operation at the working face, and discretizes one degree of freedom of the robotic arm. Based on the discretization of the two parameters, the robotic arm problem is solved. On the one hand, the inverse solution calculation time is shortened by discretization; on the other hand, the accuracy of the solution is guaranteed by analytical methods, so that the accuracy is not significantly affected. This invention solves the technical problems of long control algorithm consumption and slow solution speed in the prior art.

[0013] Furthermore, the robotic arm is a seven-degree-of-freedom robotic arm, including a boom connected to a rock drilling rig, a propulsion beam connected to one end of the boom, and the end of the boom being located at the end of the propulsion beam connected to the drill bit. The seven degrees of freedom are, in order: boom swing joint, boom pitch joint, boom telescopic joint, propulsion beam rotation joint, propulsion beam pitch joint, propulsion beam swing joint, and propulsion beam telescopic joint.

[0014] The discrete joint angle is obtained by discretizing the oscillating joint of the propulsion beam.

[0015] Furthermore, a set of inverse solutions is obtained as follows:

[0016] θ6(i)=(i×δ+(θ60-δ))×pi / 180;

[0017] θ1(i)=atan2d(-d5×F(i)+B(i),A(i)-d5×E(i));

[0018] θ2(i)=atan2d(-1×abs(sin(θ6(i))×oz - cos(θ6(i))×nz),(E(i)×cosd(rand1(i))+F(i)×sind(θ1(i))));

[0019] θ4(i)=asind(-(sin(θ6(i))×oz- cos(θ6(i))×nz) / sind(θ2(i)));

[0020] θ5(i)=atan2d(N(i),-M(i))-atan2d(cosd(θ2(i)),sqrt(N(i)^2+M(i)^2-cosd(θ2(i))^2));

[0021] d3(i)=((a0+cosd(θ1(i))×a1-d5×(cosd(θ4(i))×sind(θ1(i))-cosd(θ1(i))×cosd(θ2(i))×sind(θ4(i))))+cosd(θ1(i))×cosd(θ2(i))×a2-(px-ax×d6-a6×ox-d7×nx-cos(rand6(i))×a5×ox-sin(θ6(i))×a5×nx))) / (sind(θ2(i))×cosd(θ1(i)));

[0022] A(i)=px - ax×d6 - a6×ox - d7×nx -cos(θ6(i))×a5×ox - sin(θ6(i))×a5×nx-a0;

[0023] B(i)=py - ay×d6 - a6×oy - d7×ny -cos(θ6(i))×a5×oy - sin(θ6(i))×a5×ny;

[0024] E(i)=sin(rand6(i))×ox -cos(θ6(i))×nx;

[0025] F(i)=sin(rand6(i))×oy -cos(θ6(i))×ny;

[0026] M(i)=-(cos(θ6(i))×oz + sin(θ6(i))×nz);

[0027] N(i)=az;

[0028] Where (θ1(i), θ2(i), d3(i), θ4(i), θ5(i), θ6(i)) is a set of inverse solutions; θ6(i) is the i-th discrete joint angle; δ is the second interval angle; θ60 is the lower limit of the working range of the propulsion beam swing joint; px and py are the x and y coordinates in the position coordinates of the target pose, respectively; nx, ny, and nz are the first column of the attitude rotation matrix with discrete rotation angles as rotation angles; ox, oy, and oz are the second column of the attitude rotation matrix; ax, ay, and az are the third column of the attitude rotation matrix; abs is the absolute value function; sqrt is the square root function; a0 is the distance of the common perpendicular from joint axis 0 to joint axis 1 along the x0 axis direction; a1 is the distance of the common perpendicular from joint axis 1 to joint axis 2 along the x1 axis direction; d5 is the distance along the x0 axis direction; d5 is the distance along the x0 axis direction; d6(i) ... Joint axis 5, the distance from x4 to x5; a2, the distance along the x2 axis from joint axis 2 to joint axis 3; d6, the distance from x5 to x6 along joint axis 6; a6, the distance along the x6 axis from joint axis 6 to joint axis 7; d7, the distance from x6 to x7 along joint axis 7; a5, the distance along the x5 axis from joint axis 5 to joint axis 6; atan2d, the arctangent function in the four quadrants; cosd, the cosine function with the input angle; sind, the sine function with the input angle; asind, the arcsine function with the output angle; rand1(i), the value of the first joint angle under the current discrete condition; rand6(i), the value of the sixth joint angle under the current discrete condition.

[0029] Furthermore, the DH kinematic model is a simplified six-degree-of-freedom model that simplifies the telescopic joint of the propulsion beam to a fixed value.

[0030] Furthermore, the first interval angle is 0.1 degrees.

[0031] Furthermore, the second interval angle is 0.1 degrees.

[0032] The present invention also provides a technical solution for a robotic arm control device for a rock drilling rig: a robotic arm control device for a rock drilling rig includes a processor, the processor being used to execute a computer program to implement the steps of the robotic arm control method for the rock drilling rig described below:

[0033] S1. After the rock drilling rig arrives at the construction position, determine the end target pose of the rock drilling rig's robotic arm according to the hole layout diagram at the working face.

[0034] S2. Based on the end target pose, any discrete rotation angle and any discrete joint angle, combine the DH kinematic model to obtain a set of inverse solutions corresponding to the degrees of freedom of the discrete rotation angle and the discrete joint angle.

[0035] The discrete rotation angle is obtained by discretizing the rotation angle in the end-effector pose of the robotic arm according to a first interval angle; the discrete joint is obtained by discretizing the working range of one degree of freedom of the robotic arm according to a second interval angle.

[0036] S3. Repeat S2 until a set number of discrete rotation angles and all discrete joint angles are traversed to obtain the corresponding number of inverse solutions; select the optimal solution from the inverse solutions; control the robot arm to drill according to the optimal solution and its corresponding discrete rotation angle.

[0037] The error between the optimal solution and the correct solution in the height direction is the smallest.

[0038] The beneficial effects of the above technical solution are as follows: In the process of solving the robotic arm problem, this invention, combined with the actual application scenario of the rock drilling rig (drilling operation at the working face), discretizes the rotation angle, which is not closely related to the drilling operation at the working face, and discretizes one degree of freedom of the robotic arm. Based on the discretization of the two parameters, the robotic arm problem is solved. On the one hand, the inverse solution calculation time is shortened by discretization; on the other hand, the accuracy of the solution is guaranteed by analytical methods, so that the accuracy is not significantly affected. This invention solves the technical problems of long control algorithm consumption and slow solution speed in the prior art.

[0039] Furthermore, the robotic arm is a seven-degree-of-freedom robotic arm, including a boom connected to a rock drilling rig, a propulsion beam connected to one end of the boom, and the end of the boom being located at the end of the propulsion beam connected to the drill bit. The seven degrees of freedom are, in order: boom swing joint, boom pitch joint, boom telescopic joint, propulsion beam rotation joint, propulsion beam pitch joint, propulsion beam swing joint, and propulsion beam telescopic joint.

[0040] The discrete joint angle is obtained by discretizing the oscillating joint of the propulsion beam.

[0041] Furthermore, a set of inverse solutions is obtained as follows:

[0042] θ6(i)=(i×δ+(θ60-δ))×pi / 180;

[0043] θ1(i)=atan2d(-d5×F(i)+B(i),A(i)-d5×E(i));

[0044] θ2(i)=atan2d(-1×abs(sin(θ6(i))×oz - cos(θ6(i))×nz),(E(i)×cosd(rand1(i))+F(i)×sind(θ1(i))));

[0045] θ4(i)=asind(-(sin(θ6(i))×oz- cos(θ6(i))×nz) / sind(θ2(i)));

[0046] θ5(i)=atan2d(N(i),-M(i))-atan2d(cosd(θ2(i)),sqrt(N(i)^2+M(i)^2-cosd(θ2(i))^2));

[0047] d3(i)=((a0+cosd(θ1(i))×a1-d5×(cosd(θ4(i))×sind(θ1(i))-cosd(θ1(i))×cosd(θ2(i))×sind(θ4(i))))+cosd(θ1(i))×cosd(θ2(i))×a2-(px-ax×d6-a6×ox-d7×nx-cos(rand6(i))×a5×ox-sin(θ6(i))×a5×nx))) / (sind(θ2(i))×cosd(θ1(i)));

[0048] A(i)=px - ax×d6 - a6×ox - d7×nx -cos(θ6(i))×a5×ox - sin(θ6(i))×a5×nx-a0;

[0049] B(i)=py - ay×d6 - a6×oy - d7×ny -cos(θ6(i))×a5×oy - sin(θ6(i))×a5×ny;

[0050] E(i)=sin(rand6(i))×ox -cos(θ6(i))×nx;

[0051] F(i)=sin(rand6(i))×oy -cos(θ6(i))×ny;

[0052] M(i)=-(cos(θ6(i))×oz + sin(θ6(i))×nz);

[0053] N(i)=az;

[0054] Where (θ1(i), θ2(i), d3(i), θ4(i), θ5(i), θ6(i)) is a set of inverse solutions; θ6(i) is the i-th discrete joint angle; δ is the second interval angle; θ60 is the lower limit of the working range of the propulsion beam swing joint; px and py are the x and y coordinates in the position coordinates of the target pose, respectively; nx, ny, and nz are the first column of the attitude rotation matrix with discrete rotation angles as rotation angles; ox, oy, and oz are the second column of the attitude rotation matrix; ax, ay, and az are the third column of the attitude rotation matrix; abs is the absolute value function; sqrt is the square root function; a0 is the distance of the common perpendicular from joint axis 0 to joint axis 1 along the x0 axis direction; a1 is the distance of the common perpendicular from joint axis 1 to joint axis 2 along the x1 axis direction; d5 is the distance along the x0 axis direction; d5 is the distance along the x0 axis direction; d6(i) ... Joint axis 5, the distance from x4 to x5; a2, the distance along the x2 axis from joint axis 2 to joint axis 3; d6, the distance from x5 to x6 along joint axis 6; a6, the distance along the x6 axis from joint axis 6 to joint axis 7; d7, the distance from x6 to x7 along joint axis 7; a5, the distance along the x5 axis from joint axis 5 to joint axis 6; atan2d, the arctangent function in the four quadrants; cosd, the cosine function with the input angle; sind, the sine function with the input angle; asind, the arcsine function with the output angle; rand1(i), the value of the first joint angle under the current discrete condition; rand6(i), the value of the sixth joint angle under the current discrete condition.

[0055] Furthermore, the DH kinematic model is a simplified six-degree-of-freedom model that simplifies the telescopic joint of the propulsion beam to a fixed value.

[0056] Furthermore, the first interval angle is 0.1 degrees.

[0057] Furthermore, the second interval angle is 0.1 degrees.

[0058] The present invention also provides a technical solution for a rock drilling rig: a rock drilling rig includes a robotic arm for drilling operations and a robotic arm controller, wherein the robotic arm controller includes a processor, the processor being used to execute a computer program to implement the following steps of the robotic arm control method for the rock drilling rig:

[0059] S1. After the rock drilling rig arrives at the construction position, determine the end target pose of the rock drilling rig's robotic arm according to the hole layout diagram at the working face.

[0060] S2. Based on the end target pose, any discrete rotation angle and any discrete joint angle, combine the DH kinematic model to obtain a set of inverse solutions corresponding to the degrees of freedom of the discrete rotation angle and the discrete joint angle.

[0061] The discrete rotation angle is obtained by discretizing the rotation angle in the end-effector pose of the robotic arm according to a first interval angle; the discrete joint is obtained by discretizing the working range of one degree of freedom of the robotic arm according to a second interval angle.

[0062] S3. Repeat S2 until a set number of discrete rotation angles and all discrete joint angles are traversed to obtain the corresponding number of inverse solutions; select the optimal solution from the inverse solutions; control the robot arm to drill according to the optimal solution and its corresponding discrete rotation angle.

[0063] The error between the optimal solution and the correct solution in the height direction is the smallest.

[0064] The beneficial effects of the above technical solution are as follows: In the process of solving the robotic arm problem, this invention, combined with the actual application scenario of the rock drilling rig (drilling operation at the working face), discretizes the rotation angle, which is not closely related to the drilling operation at the working face, and discretizes one degree of freedom of the robotic arm. Based on the discretization of the two parameters, the robotic arm problem is solved. On the one hand, the inverse solution calculation time is shortened by discretization; on the other hand, the accuracy of the solution is guaranteed by analytical methods, so that the accuracy is not significantly affected. This invention solves the technical problems of long control algorithm consumption and slow solution speed in the prior art.

[0065] Furthermore, the robotic arm is a seven-degree-of-freedom robotic arm, including a boom connected to a rock drilling rig, a propulsion beam connected to one end of the boom, and the end of the boom being located at the end of the propulsion beam connected to the drill bit. The seven degrees of freedom are, in order: boom swing joint, boom pitch joint, boom telescopic joint, propulsion beam rotation joint, propulsion beam pitch joint, propulsion beam swing joint, and propulsion beam telescopic joint.

[0066] The discrete joint angle is obtained by discretizing the oscillating joint of the propulsion beam.

[0067] Furthermore, a set of inverse solutions is obtained as follows:

[0068] θ6(i)=(i×δ+(θ60-δ))×pi / 180;

[0069] θ1(i)=atan2d(-d5×F(i)+B(i),A(i)-d5×E(i));

[0070] θ2(i)=atan2d(-1×abs(sin(θ6(i))×oz - cos(θ6(i))×nz),(E(i)×cosd(rand1(i))+F(i)×sind(θ1(i))));

[0071] θ4(i)=asind(-(sin(θ6(i))×oz- cos(θ6(i))×nz) / sind(θ2(i)));

[0072] θ5(i)=atan2d(N(i),-M(i))-atan2d(cosd(θ2(i)),sqrt(N(i)^2+M(i)^2-cosd(θ2(i))^2));

[0073] d3(i)=((a0+cosd(θ1(i))×a1-d5×(cosd(θ4(i))×sind(θ1(i))-cosd(θ1(i))×cosd(θ2(i))×sind(θ4(i))))+cosd(θ1(i))×cosd(θ2(i))×a2-(px-ax×d6-a6×ox-d7×nx-cos(rand6(i))×a5×ox-sin(θ6(i))×a5×nx))) / (sind(θ2(i))×cosd(θ1(i)));

[0074] A(i)=px - ax×d6 - a6×ox - d7×nx -cos(θ6(i))×a5×ox - sin(θ6(i))×a5×nx-a0;

[0075] B(i)=py - ay×d6 - a6×oy - d7×ny -cos(θ6(i))×a5×oy - sin(θ6(i))×a5×ny;

[0076] E(i)=sin(rand6(i))×ox -cos(θ6(i))×nx;

[0077] F(i)=sin(rand6(i))×oy -cos(θ6(i))×ny;

[0078] M(i)=-(cos(θ6(i))×oz + sin(θ6(i))×nz);

[0079] N(i)=az;

[0080] Where (θ1(i), θ2(i), d3(i), θ4(i), θ5(i), θ6(i)) is a set of inverse solutions; θ6(i) is the i-th discrete joint angle; δ is the second interval angle; θ60 is the lower limit of the working range of the propulsion beam swing joint; px and py are the x and y coordinates in the position coordinates of the target pose, respectively; nx, ny, and nz are the first column of the attitude rotation matrix with discrete rotation angles as rotation angles; ox, oy, and oz are the second column of the attitude rotation matrix; ax, ay, and az are the third column of the attitude rotation matrix; abs is the absolute value function; sqrt is the square root function; a0 is the distance of the common perpendicular from joint axis 0 to joint axis 1 along the x0 axis direction; a1 is the distance of the common perpendicular from joint axis 1 to joint axis 2 along the x1 axis direction; d5 is the distance along the x0 axis direction; d5 is the distance along the x0 axis direction; d6(i) ... Joint axis 5, the distance from x4 to x5; a2, the distance along the x2 axis from joint axis 2 to joint axis 3; d6, the distance from x5 to x6 along joint axis 6; a6, the distance along the x6 axis from joint axis 6 to joint axis 7; d7, the distance from x6 to x7 along joint axis 7; a5, the distance along the x5 axis from joint axis 5 to joint axis 6; atan2d, the arctangent function in the four quadrants; cosd, the cosine function with the input angle; sind, the sine function with the input angle; asind, the arcsine function with the output angle; rand1(i), the value of the first joint angle under the current discrete condition; rand6(i), the value of the sixth joint angle under the current discrete condition.

[0081] Furthermore, the DH kinematic model is a simplified six-degree-of-freedom model that simplifies the telescopic joint of the propulsion beam to a fixed value.

[0082] Furthermore, the first interval angle is 0.1 degrees.

[0083] Furthermore, the second interval angle is 0.1 degrees. Attached Figure Description

[0084] Figure 1 This is a schematic diagram of the construction of the rock drilling rig in the embodiment of the present invention.

[0085] Figure 2 This is a flowchart of the control method in the embodiment of the rock drilling rig of the present invention. Detailed Implementation

[0086] In solving the problem of the robotic arm, this invention, considering the actual application scenario of the rock drilling rig (drilling operation at the working face), discretizes the rotation angle, which is not closely related to the drilling operation, and discretizes one degree of freedom of the robotic arm. Based on the discretization of two parameters, the robotic arm solution is performed. On the one hand, the discretization method shortens the inverse solution calculation time; on the other hand, the analytical method ensures the accuracy of the solution, so that the accuracy is not significantly affected. This invention solves the technical problems of long control algorithm consumption and slow solution speed in the prior art.

[0087] Implementation method of rock drilling rig:

[0088] A type of rock drilling rig, such as Figure 1 As shown, the system includes a rock drilling rig body, with two 8-DOF redundant robotic arms, one 7-DOF redundant robotic arm, and one auxiliary suspended platform arm positioned at the front. The auxiliary suspended platform arm allows workers to mark the drilling face from the suspended platform for easier drilling. The 8-DOF and 7-DOF redundant robotic arms are used for fully automated drilling operations on the drilling face. During drilling, each robotic arm operates independently and without interference.

[0089] The seven-DOF redundant robotic arm has seven degrees of freedom: boom swing joint, boom pitch joint, boom telescopic joint, propeller beam rotation joint, propeller beam pitch joint, propeller beam swing joint, and propeller beam telescopic joint. The boom swing joint is mounted on a base at the front of the drilling rig. The boom pitch joint is controlled by a hydraulic cylinder to adjust the boom's pitch angle; the boom telescopic joint is controlled by a hydraulic cylinder to adjust the position of the propeller beam at the boom's end along the boom's extension direction; and the boom swing joint is controlled by a hydraulic cylinder to adjust the boom's angle on the horizontal plane. After adjusting the robotic arm's end-effector pose to the target pose using the various degrees of freedom, the rock drill (i.e., the drill bit) at the end of the robotic arm is controlled to perform drilling operations.

[0090] The eight-degree-of-freedom redundant robotic arm has eight degrees of freedom. Compared to the seven-degree-of-freedom redundant robotic arm, an eagle-type lifting arm is added at the end of the boom swing joint near the rock drilling rig. That is, the boom swing joint of the eight-degree-of-freedom redundant robotic arm is connected to the base through the eagle-type lifting arm.

[0091] Each joint of each robotic arm is equipped with rotation angle and wire displacement sensors for real-time monitoring of the boom's posture. The rock drilling rig also includes dual-axis tilt sensors, dual prisms, and a host computer intelligent control terminal; the dual-axis tilt sensors are used for leveling the rock drilling rig within the tunnel; two dual-axis tilt sensors, one on each side at the rear of the rock drilling rig, are used for navigation and positioning within the tunnel; the robotic arm controller (i.e....) Figure 1The upper-level intelligent control terminal (the system) calculates the hole position and pose information in real time and controls the movement of the robotic arm. The robotic arm controller includes a processor, which executes computer programs to implement the robotic arm control method of the rock drilling rig.

[0092] After the trolley enters the tunnel, the dual-axis tilt sensor is used to level the trolley both front to back and left to right. The prism on the trolley body is then used to navigate and position it within the tunnel. The boom (i.e., the robotic arm) then touches the tunnel face to obtain the position and orientation information of the face relative to the trolley. Finally, based on the hole position and orientation information on the hole layout diagram, the joint angle values ​​of each joint of the boom are calculated using the robotic arm control method of the rock drilling trolley and sent to the lower computer to control the boom to move to the designated position and orientation. Finally, automatic drilling operation is started.

[0093] like Figure 2 As shown, the following section uses a seven-degree-of-freedom redundant robotic arm as an example to describe in detail the robotic arm control method of the rock drilling rig in this embodiment:

[0094] First, a kinematic model of the boom is established using the Denavit-Hartenberg (DH) method based on the boom's own structure:

[0095]

[0096] In the above equation, the left side represents the end-effector pose, p x p y and p z R, P, and Y represent the position coordinates in the end-effector pose; R, P, and Y represent the roll, pitch, and yaw angles in the end-effector target pose, respectively. The right side of the above equation represents the values ​​of each degree of freedom of the boom, where q1~q6 represent the swing angle θ1 of the boom swing joint, the pitch angle θ2 of the boom pitch joint, the extension / retraction amount d3 of the boom telescopic joint, the rotation angle θ4 of the propeller beam slewing joint, the pitch angle θ5 of the propeller beam pitch joint, the swing angle θ6 of the propeller beam swing joint, and the extension / retraction amount d7 of the propeller beam telescopic joint, respectively.

[0097] During drilling, the target pose required for the robotic arm to perform the drilling operation is determined based on the hole layout diagram at the drilling face. Then, according to the aforementioned kinematic model, the target pose is... The kinematics are then converted into values ​​for each degree of freedom of the robotic arm. .

[0098] Specifically, in this embodiment, since the telescopic joint of the propulsion beam is only used to move the drill arm, the telescopic amount d7 of the propulsion beam telescopic joint can be taken as a fixed value when performing kinematic modeling, and only the degrees of freedom of the first 6 joints can be determined. In actual control, after the degrees of freedom of the first 6 joints are determined, d7 can move along its axis according to the actual drilling requirements.

[0099] Furthermore, generally speaking, in this embodiment, the rotation angle in the target pose is discretized in increments of 0.1 degrees between [0, 360] degrees, resulting in 3600 discrete rotation angles within the workspace of the rotation angle. From these 3600 discrete rotation angles, a set number m of discrete rotation angles are randomly selected.

[0100] The swing angle θ6 of the propulsion beam swing joint is also discretized at 0.1-degree intervals within its working range, resulting in n discrete swing joint angles within the working space of θ6. In this embodiment, assuming the minimum value of θ6 is 30 degrees, the i-th discrete swing joint angle is:

[0101]

[0102] Substitute each discrete swing joint angle and the selected discrete rotation angle into the established DH kinematic model and solve to obtain the solutions for the joint variables of the remaining 5 degrees of freedom. Combined with the corresponding discrete swing joint angles, we can obtain (q1,q2,q3,q4,q5,q6)=(θ1, θ2, d3, θ4, θ5, θ6).

[0103] In this embodiment, the solution can be obtained using the following formula:

[0104] θ1(i)=atan2d(-d5×F(i)+B(i),A(i)-d5×E(i));

[0105] θ2(i)=atan2d(-1×abs(sin(θ6(i))×oz - cos(θ6(i))×nz),(E(i)×cosd(rand1(i))+F(i)×sind(θ1(i))));

[0106] θ4(i)=asind(-(sin(θ6(i))×oz - cos(θ6(i))×nz) / sind(θ2(i)));

[0107] θ5(i)=atan2d(N(i),-M(i))-atan2d(cosd(θ2(i)),sqrt(N(i)^2+M(i)^2-cosd(θ2(i))^2));

[0108] d3(i)= ((a0 +cosd(θ1(i))×a1 - d5×(cosd(θ4(i))×sind(θ1(i)) - cosd(θ1(i))×cosd(θ2(i))×sind(θ4(i)))) + cosd(θ1(i))×cosd(θ2(i))×a2-(px - ax×d6- a6×ox - d7×nx - cos(rand6(i))×a5×ox - sin(θ6(i))×a5×nx))) / (sind(θ2(i))×cosd(θ1(i)));

[0109] A(i)=px - ax×d6 - a6×ox - d7×nx -cos(θ6(i))×a5×ox - sin(θ6(i))×a5×nx-a0;

[0110] B(i)= py - ay×d6 - a6×oy - d7×ny -cos(θ6(i))×a5×oy - sin(θ6(i))×a5×ny;

[0111] E(i)=sin(rand6(i))×ox -cos(θ6(i))×nx;

[0112] F(i)=sin(rand6(i))×oy -cos(θ6(i))×ny;

[0113] M(i)=-(cos(θ6(i))×oz + sin(θ6(i))×nz);

[0114] N(i)=az;

[0115] Where (θ1(i), θ2(i), d3(i), θ4(i), θ5(i), θ6(i)) is a set of inverse solutions; θ6(i) is the i-th discrete joint angle; δ is the second interval angle; θ60 is the lower limit of the working range of the propulsion beam swing joint; px and py are the x and y coordinates in the position coordinates of the target pose, respectively; nx, ny, and nz are the first column of the attitude rotation matrix with discrete rotation angles as rotation angles; ox, oy, and oz are the second column of the attitude rotation matrix; ax, ay, and az are the third column of the attitude rotation matrix; abs is the absolute value function; sqrt is the square root function; a0 is the distance of the common perpendicular from joint axis 0 to joint axis 1 along the x0 axis direction; a1 is the distance of the common perpendicular from joint axis 1 to joint axis 2 along the x1 axis direction; d5 is the distance along the x0 axis direction; d5 is the distance along the x0 axis direction; d6(i) ... Joint axis 5, the distance from x4 to x5; a2, the distance along the x2 axis from joint axis 2 to joint axis 3; d6, the distance from x5 to x6 along joint axis 6; a6, the distance along the x6 axis from joint axis 6 to joint axis 7; d7, the distance from x6 to x7 along joint axis 7; a5, the distance along the x5 axis from joint axis 5 to joint axis 6; atan2d, the arctangent function in the four quadrants; cosd, the cosine function with the input angle; sind, the sine function with the input angle; asind, the arcsine function with the output angle; rand1(i), the value of the first joint angle under the current discrete condition; rand6(i), the value of the sixth joint angle under the current discrete condition.

[0116] The parameter a above i-1 Let be the link length, representing the distance along the common perpendicular from joint axis i-1 to joint axis i (along the x-axis). i-1 (axis direction); d i Let x be the link offset, representing the distance from x along joint axis i. i-1 To x i The distance. These are all common parameters in the DH-dynamic model.

[0117] Based on the above solution process, a total of m×n solutions (θ1, θ2, d3, θ4, θ5, θ6) can be obtained. Substituting these solutions into the forward kinematics model, the pose vectors corresponding to these solutions are obtained. These pose vectors are compared with the target pose, and the solution corresponding to the pose vector with the smallest error in the z-direction (i.e., the height direction) of the target pose is found as the optimal solution and its corresponding rotation angle.

[0118] Based on the optimal solution and its corresponding rotation angle, control the drill arm. After the first six degrees of freedom have reached their positions, adjust the extension / retraction (d7) of the propulsion beam telescopic joint along its axis to the appropriate position. Compared to the seven-degree-of-freedom redundant manipulator, the eight-degree-of-freedom redundant manipulator only adds an eagle-type lifting arm at the end near the drilling rig. Therefore, when solving the problem, the above method can still be used: first determine the lifting / retraction of the eagle-type lifting arm and the extension / retraction of the propulsion beam telescopic joint, and then adjust the lifting / retraction of the eagle-type lifting arm and the extension / retraction of the propulsion beam telescopic joint according to the actual drilling requirements.

[0119] Implementation method of the robotic arm control device for rock drilling rigs:

[0120] A robotic arm control device for a rock drilling rig includes a processor for executing a computer program to implement the steps of the robotic arm control method for the rock drilling rig as described above. The specific robotic arm control method for the rock drilling rig has been described in sufficient detail in the above-described rock drilling rig embodiments and will not be repeated here.

[0121] like Figure 2 As shown, the following section uses a seven-degree-of-freedom redundant robotic arm as an example to describe in detail the robotic arm control method of the rock drilling rig in this embodiment:

[0122] First, a kinematic model of the boom is established using the Denavit-Hartenberg (DH) method based on the boom's own structure:

[0123]

[0124] In the above equation, the left side represents the end-effector pose, p x p y and p z R, P, and Y represent the position coordinates in the end-effector pose; R, P, and Y represent the roll, pitch, and yaw angles in the end-effector target pose, respectively. The right side of the above equation represents the values ​​of each degree of freedom of the boom, where q1~q6 represent the swing angle θ1 of the boom swing joint, the pitch angle θ2 of the boom pitch joint, the extension / retraction amount d3 of the boom telescopic joint, the rotation angle θ4 of the propeller beam slewing joint, the pitch angle θ5 of the propeller beam pitch joint, the swing angle θ6 of the propeller beam swing joint, and the extension / retraction amount d7 of the propeller beam telescopic joint, respectively.

[0125] During drilling, the target pose required for the robotic arm to perform the drilling operation is determined based on the hole layout diagram at the drilling face. Then, according to the aforementioned kinematic model, the target pose is... The kinematics are then converted into values ​​for each degree of freedom of the robotic arm. .

[0126] Specifically, in this embodiment, since the telescopic joint of the propulsion beam is only used to move the drill arm, the telescopic amount d7 of the propulsion beam telescopic joint can be taken as a fixed value when performing kinematic modeling, and only the degrees of freedom of the first 6 joints can be determined. In actual control, after the degrees of freedom of the first 6 joints are determined, d7 can move along its axis according to the actual drilling requirements.

[0127] Furthermore, generally speaking, in this embodiment, the rotation angle in the target pose is discretized in increments of 0.1 degrees between [0, 360] degrees, resulting in 3600 discrete rotation angles within the workspace of the rotation angle. From these 3600 discrete rotation angles, a set number m of discrete rotation angles are randomly selected.

[0128] The swing angle θ6 of the propulsion beam swing joint is also discretized at 0.1-degree intervals within its working range, resulting in n discrete swing joint angles within the working space of θ6. In this embodiment, assuming the minimum value of θ6 is 30 degrees, the i-th discrete swing joint angle is:

[0129]

[0130] Substitute each discrete swing joint angle and the selected discrete rotation angle into the established DH kinematic model and solve to obtain the solutions for the joint variables of the remaining 5 degrees of freedom. Combined with the corresponding discrete swing joint angles, we can obtain (q1,q2,q3,q4,q5,q6)=(θ1, θ2, d3, θ4, θ5, θ6).

[0131] In this embodiment, the solution can be obtained using the following formula:

[0132] θ1(i)=atan2d(-d5×F(i)+B(i),A(i)-d5×E(i));

[0133] θ2(i)=atan2d(-1×abs(sin(θ6(i))×oz - cos(θ6(i))×nz),(E(i)×cosd(rand1(i))+F(i)×sind(θ1(i))));

[0134] θ4(i)=asind(-(sin(θ6(i))×oz - cos(θ6(i))×nz) / sind(θ2(i)));

[0135] θ5(i)=atan2d(N(i),-M(i))-atan2d(cosd(θ2(i)),sqrt(N(i)^2+M(i)^2-cosd(θ2(i))^2));

[0136] d3(i)= ((a0 +cosd(θ1(i))×a1 - d5×(cosd(θ4(i))×sind(θ1(i)) - cosd(θ1(i))×cosd(θ2(i))×sind(θ4(i)))) + cosd(θ1(i))×cosd(θ2(i))×a2-(px - ax×d6- a6×ox - d7×nx - cos(rand6(i))×a5×ox - sin(θ6(i))×a5×nx))) / (sind(θ2(i))×cosd(θ1(i)));

[0137] A(i)=px - ax×d6 - a6×ox - d7×nx -cos(θ6(i))×a5×ox - sin(θ6(i))×a5×nx-a0;

[0138] B(i)= py - ay×d6 - a6×oy - d7×ny -cos(θ6(i))×a5×oy - sin(θ6(i))×a5×ny;

[0139] E(i)=sin(rand6(i))×ox -cos(θ6(i))×nx;

[0140] F(i)=sin(rand6(i))×oy -cos(θ6(i))×ny;

[0141] M(i)=-(cos(θ6(i))×oz + sin(θ6(i))×nz);

[0142] N(i)=az;

[0143] Where (θ1(i), θ2(i), d3(i), θ4(i), θ5(i), θ6(i)) is a set of inverse solutions; θ6(i) is the i-th discrete joint angle; δ is the second interval angle; θ60 is the lower limit of the working range of the propulsion beam swing joint; px and py are the x and y coordinates in the position coordinates of the target pose, respectively; nx, ny, and nz are the first column of the attitude rotation matrix with discrete rotation angles as rotation angles; ox, oy, and oz are the second column of the attitude rotation matrix; ax, ay, and az are the third column of the attitude rotation matrix; abs is the absolute value function; sqrt is the square root function; a0 is the distance of the common perpendicular from joint axis 0 to joint axis 1 along the x0 axis direction; a1 is the distance of the common perpendicular from joint axis 1 to joint axis 2 along the x1 axis direction; d5 is the distance along the x0 axis direction; d5 is the distance along the x0 axis direction; d6(i) ... Joint axis 5, the distance from x4 to x5; a2, the distance along the x2 axis from joint axis 2 to joint axis 3; d6, the distance from x5 to x6 along joint axis 6; a6, the distance along the x6 axis from joint axis 6 to joint axis 7; d7, the distance from x6 to x7 along joint axis 7; a5, the distance along the x5 axis from joint axis 5 to joint axis 6; atan2d, the arctangent function in the four quadrants; cosd, the cosine function with the input angle; sind, the sine function with the input angle; asind, the arcsine function with the output angle; rand1(i), the value of the first joint angle under the current discrete condition; rand6(i), the value of the sixth joint angle under the current discrete condition.

[0144] Based on the above solution process, a total of m×n solutions (θ1, θ2, d3, θ4, θ5, θ6) can be obtained. Substituting these solutions into the forward kinematics model, the pose vectors corresponding to these solutions are obtained. These pose vectors are compared with the target pose, and the solution corresponding to the pose vector with the smallest error in the z-direction (i.e., the height direction) of the target pose is found as the optimal solution and its corresponding rotation angle.

[0145] Based on the optimal solution and its corresponding rotation angle, control the drill arm. After the first six degrees of freedom have reached their positions, adjust the extension / retraction (d7) of the propulsion beam telescopic joint along its axis to the appropriate position. Compared to the seven-degree-of-freedom redundant manipulator, the eight-degree-of-freedom redundant manipulator only adds an eagle-type lifting arm at the end near the drilling rig. Therefore, when solving the problem, the above method can still be used: first determine the lifting / retraction of the eagle-type lifting arm and the extension / retraction of the propulsion beam telescopic joint, and then adjust the lifting / retraction of the eagle-type lifting arm and the extension / retraction of the propulsion beam telescopic joint according to the actual drilling requirements.

[0146] Specifically, the processor can be a CPU, or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor. The processor can also be a processor supporting the Advanced Reduced Instruction Set Machine (ARM) architecture.

[0147] Implementation method of controlling the robotic arm of a rock drilling rig:

[0148] A method for controlling the robotic arm of a rock drilling rig, the specific method of which has been described in sufficient detail in the above-described rock drilling rig embodiments. The method includes:

[0149] S1. After the rock drilling rig arrives at the construction position, determine the end target pose of the rock drilling rig's robotic arm according to the hole layout diagram at the working face.

[0150] S2. Based on the end target pose, any discrete rotation angle and any discrete joint angle, combine the DH kinematic model to obtain a set of inverse solutions corresponding to the degrees of freedom of the discrete rotation angle and the discrete joint angle.

[0151] The discrete rotation angle is obtained by discretizing the rotation angle in the end-effector pose of the robotic arm according to a first interval angle; the discrete joint is obtained by discretizing the working range of one degree of freedom of the robotic arm according to a second interval angle.

[0152] S3. Repeat S2 until a set number of discrete rotation angles and all discrete joint angles are traversed to obtain the corresponding number of inverse solutions; select the optimal solution from the inverse solutions; control the robot arm to drill according to the optimal solution and its corresponding discrete rotation angle.

[0153] The error between the optimal solution and the correct solution in the height direction is the smallest.

[0154] Furthermore, the robotic arm is a seven-degree-of-freedom robotic arm, including a boom connected to a rock drilling rig and a propulsion beam connected to one end of the boom. The end of the boom is located at the end of the propulsion beam connected to the drill bit. The seven degrees of freedom are, in order: boom swing joint, boom pitch joint, boom telescopic joint, propulsion beam rotation joint, propulsion beam pitch joint, propulsion beam swing joint, and propulsion beam telescopic joint; the discretized joint angles are obtained by discretizing the propulsion beam swing joint.

[0155] Furthermore, a set of inverse solutions is obtained according to the following method:

[0156] θ6(i)=(i×δ+(θ60 - δ))×pi / 180;

[0157] θ1(i)=atan2d(-d5×F(i)+B(i),A(i)-d5×E(i));

[0158] θ2(i)=atan2d(-1×abs(sin(θ6(i))×oz - cos(θ6(i))×nz),(E(i)×cosd(rand1(i))+F(i)×sind(θ1(i))));

[0159] θ4(i)=asind(-(sin(θ6(i))×oz - cos(θ6(i))×nz) / sind(θ2(i)));

[0160] θ5(i)=atan2d(N(i),-M(i)) - atan2d(cosd(θ2(i)),sqrt(N(i)^2+M(i)^2 - cosd(θ2(i))^2));

[0161] d3(i)=((a0+cosd(θ1(i))×a1 - d5×(cosd(θ4(i))×sind(θ1(i)) - cosd(θ1(i))×cosd(θ2(i))×sind(θ4(i)))+cosd(θ1(i))×cosd(θ2(i))×a2-(px - ax×d6 - a6×ox - d7×nx - cos(rand6(i))×a5×ox - sin(θ6(i))×a5×nx))) / (sind(θ2(i))×cosd(θ1(i)));

[0162] A(i)=px - ax×d6 - a6×ox - d7×nx - cos(θ6(i))×a5×ox - sin(θ6(i))×a5×nx - a0;

[0163] B(i)=py - ay×d6 - a6×oy - d7×ny - cos(θ6(i))×a5×oy - sin(θ6(i))×a5×ny;

[0164] E(i)=sin(rand6(i))×ox - cos(θ6(i))×nx;

[0165] F(i)=sin(rand6(i))×oy -cos(θ6(i))×ny;

[0166] M(i)=-(cos(θ6(i))×oz + sin(θ6(i))×nz);

[0167] N(i) = az;

[0168] Where (θ1(i), θ2(i), d3(i), θ4(i), θ5(i), θ6(i)) is a set of inverse solutions; θ6(i) is the i-th discrete joint angle; δ is the second interval angle; θ60 is the lower limit of the working range of the propulsion beam swing joint; px and py are the x and y coordinates in the position coordinates of the target pose, respectively; nx, ny, and nz are the first column of the attitude rotation matrix with discrete rotation angles as rotation angles; ox, oy, and oz are the second column of the attitude rotation matrix; ax, ay, and az are the third column of the attitude rotation matrix; abs is the absolute value function; sqrt is the square root function; a0 is the distance of the common perpendicular from joint axis 0 to joint axis 1 along the x0 axis direction; a1 is the distance of the common perpendicular from joint axis 1 to joint axis 2 along the x1 axis direction; d5 is the distance along the x0 axis direction; d5 is the distance along the x0 axis direction; d6(i) ... Joint axis 5, the distance from x4 to x5; a2, the distance along the x2 axis from joint axis 2 to joint axis 3; d6, the distance from x5 to x6 along joint axis 6; a6, the distance along the x6 axis from joint axis 6 to joint axis 7; d7, the distance from x6 to x7 along joint axis 7; a5, the distance along the x5 axis from joint axis 5 to joint axis 6; atan2d, the arctangent function in the four quadrants; cosd, the cosine function with the input angle; sind, the sine function with the input angle; asind, the arcsine function with the output angle; rand1(i), the value of the first joint angle under the current discrete condition; rand6(i), the value of the sixth joint angle under the current discrete condition.

[0169] Furthermore, the DH kinematic model is a simplified six-degree-of-freedom model that simplifies the telescopic joint of the propulsion beam to a fixed value.

[0170] Furthermore, the first interval angle is 0.1 degrees.

[0171] Furthermore, the second interval angle is 0.1 degrees.

[0172] This invention has the following characteristics:

[0173] This invention proposes a fully computerized intelligent drilling control method and device for rock drilling rigs. It solves the problems of existing multi-degree-of-freedom redundant robotic arm inverse kinematics control methods, such as long processing time, solution range exceeding the structural limits of the arm itself, low efficiency, only one solution per operation, large error, and getting stuck in local minima. Furthermore, it can achieve accurate solutions through discretization analytical methods, shortening the inverse kinematics calculation time, quickly providing key joint angle information for arm movement, and improving the intelligence level and working efficiency of the robotic arm.

[0174] The mainstream inverse kinematics algorithms for redundant robotic arms include analytical and iterative methods. However, iterative methods suffer from slow solution speed, reliance on iteration parameters, yielding only one solution per iteration and prone to getting trapped in local minima, and complex parameter tuning. Analytical methods, on the other hand, impose too many constraints on the boom joints, reducing the reachable space of the inverse kinematics. This invention proposes a discretized analytical method, which better addresses the problems of iterative methods (resulting in single-valued solutions, slow speed, and low efficiency) and analytical methods (resulting in excessive constraints and reduced workspace), thereby improving control efficiency.

[0175] Finally, it should be noted that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still make modifications to the technical solutions described in the foregoing embodiments without creative effort, or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for controlling the robotic arm of a rock drilling rig, characterized in that, The method includes: S1. After the rock drilling rig arrives at the construction position, determine the end target pose of the rock drilling rig's robotic arm according to the hole layout diagram at the working face. S2. Based on the end target pose, any discrete rotation angle and any discrete joint angle, combine the DH kinematic model to obtain a set of inverse solutions corresponding to the degrees of freedom of the discrete rotation angle and the discrete joint angle. The discrete rotation angle is obtained by discretizing the rotation angle in the end-effector pose of the robotic arm according to a first interval angle; the discrete joint angle is obtained by discretizing the working range of one degree of freedom of the robotic arm according to a second interval angle. S3. Repeat S2 until a set number of discrete rotation angles and all discrete joint angles are traversed to obtain the corresponding number of inverse solutions; select the optimal solution from the inverse solutions; control the robot arm to drill according to the optimal solution and its corresponding discrete rotation angle. The optimal solution corresponds to the correct solution with the smallest error in the height direction compared to the end target pose.

2. The robotic arm control method for a rock drilling rig according to claim 1, characterized in that, The robotic arm is a seven-degree-of-freedom robotic arm, including a boom connected to a rock drilling rig and a push beam connected to one end of the boom. The end of the boom is located at the end of the push beam that is connected to the drill bit. The seven degrees of freedom are, in order: boom swing joint, boom pitch joint, boom telescopic joint, push beam rotation joint, push beam pitch joint, push beam swing joint, and push beam telescopic joint. The discrete joint angle is obtained by discretizing the oscillating joint of the propulsion beam.

3. The robotic arm control method for a rock drilling rig according to claim 2, characterized in that, A set of inverse solutions is obtained using the following method: θ6(i)=(i×δ+(θ60-δ))×pi / 180; θ1(i)=atan2d(-d5×F(i)+B(i),A(i)-d5×E(i)); θ2(i)=atan2d(-1×abs(sin(θ6(i))×oz - cos(θ6(i))×nz),(E(i)×cosd(rand1(i))+F(i)×sind(θ1(i)))); θ4(i)=asind(-(sin(θ6(i))×oz- cos(θ6(i))×nz) / sind(θ2(i))); θ5(i)=atan2d(N(i),-M(i))-atan2d(cosd(θ2(i)),sqrt(N(i)^2+M(i)^2-cosd(θ2(i))^2)); d3(i)=((a0+cosd(θ1(i))×a1-d5×(cosd(θ4(i))×sind(θ1(i))-cosd(θ1(i))×cosd(θ2(i))×sind(θ4(i)))+cosd(θ1(i))× cosd(θ2(i))×a2-(px-ax×d6-a6×ox-d7×nx-cos(rand6(i))×a5×ox-sin(θ6(i))×a5×nx))) / (sind(θ2(i))×cosd(θ1(i))); A(i)=px - ax×d6 - a6×ox - d7×nx -cos(θ6(i))×a5×ox - sin(θ6(i))×a5×nx-a0; B(i)=py - ay×d6 - a6×oy - d7×ny -cos(θ6(i))×a5×oy - sin(θ6(i))×a5×ny; E(i)=sin(rand6(i))×ox -cos(θ6(i))×nx; F(i)=sin(rand6(i))×oy -cos(θ6(i))×ny; M(i)=-(cos(θ6(i))×oz + sin(θ6(i))×nz); N(i) = az; Where (θ1(i), θ2(i), d3(i), θ4(i), θ5(i), θ6(i)) is a set of inverse solutions; θ6(i) is the i-th discrete joint angle; δ is the second interval angle; θ60 is the lower limit of the working range of the propulsion beam swing joint; px and py are the position coordinates in the target pose, respectively. x coordinates and y Coordinates; nx, ny, and nz are the first columns of the attitude rotation matrix with discrete rotation angles as rotation angles; ox, oy, and oz are the second columns of the attitude rotation matrix; ax, ay, and az are the third columns of the attitude rotation matrix; abs is the absolute value function; sqrt is the square root function; a0 is the perpendicular distance from joint axis 0 to joint axis 1 along the x0 axis; a1 is the perpendicular distance from joint axis 1 to joint axis 2 along the x1 axis; d5 is the distance from x4 to x5 along joint axis 5; a2 is the perpendicular distance from joint axis 2 to joint axis 3 along the x2 axis. d6 is the distance from x5 to x6 along joint axis 6; a6 is the distance along the common perpendicular line from joint axis 6 to joint axis 7 along the x6 axis; d7 is the distance from x6 to x7 along joint axis 7; a5 is the distance along the common perpendicular line from joint axis 5 to joint axis 6 along the x5 axis; atan2d is the arctangent function in the four quadrants; cosd is the cosine function with the input angle; sind is the sine function with the input angle; asind is the arcsine function with the output angle; rand1(i) is the value of the first joint angle under the current discrete condition; rand6(i) is the value of the sixth joint angle under the current discrete condition.

4. The robotic arm control method for a rock drilling rig according to claim 2, characterized in that, The DH kinematic model is a simplified six-degree-of-freedom model that simplifies the telescopic joint of the propulsion beam to a fixed value.

5. The robotic arm control method for a rock drilling rig according to claim 1, characterized in that, The first interval angle is 0.1 degrees.

6. The robotic arm control method for a rock drilling rig according to claim 1, characterized in that, The second interval angle is 0.1 degrees.

7. A robotic arm control device for a rock drilling rig, comprising a processor, characterized in that, The processor is used to execute a computer program to implement the steps of the robotic arm control method for a rock drilling rig as described in any one of claims 1 to 6.

8. A rock drilling rig, comprising a robotic arm for drilling operations and a robotic arm controller, the robotic arm controller including a processor, characterized in that, The processor is used to execute a computer program to implement the steps of the robotic arm control method for a rock drilling rig as described in any one of claims 1 to 6.

Images