A method for inverse kinematics control of a jumbo drill boom

Abstract

A kind of inverse solution control method of rock drilling jumbo drill boom, it is suitable for the first two joints are rotary joint, and the rotary axis of the first two joints is perpendicular, the drill boom is rotated in the angle range of no more than 180 °, the first two joint angle parameters of drill boom are parameterized, and the fast numerical solution mode of the rotation angle of the first two joint angles is given, the inverse solution algorithm structure of fast loop solution combining numerical and analytical solution is established, according to the characteristics of rock drilling jumbo drill boom structure, the first two joints are iteratively solved by numerical method, the middle joint and the end joint are solved by analytical method, the solving accuracy and solving speed of drill boom inverse solution are considered, the initial value of the rotation angle of the two joints is automatically generated by random number, and combined with the mode of multiple trial, the problem of unable to solve caused by improper setting of joint rotation angle initial value can be avoided, which is beneficial to the accurate control of the pose of rock drilling jumbo drill boom.

Description

Technical Field

[0001] This invention relates to the field of drill arm control methods for rock drilling rigs, and more particularly to a reverse solution control method for the drill arm of a rock drilling rig. Background Technology

[0002] In recent years, rock drilling rigs have been continuously evolving towards intelligent and unmanned operation. The inverse kinematics (IK) of the drilling arm is fundamental to realizing its motion control. Currently, IK methods are divided into two categories: numerical methods and analytical methods. Numerical methods include the weighted minimum norm method, the generalized / narrow Jacobi pseudo-inverse method, the gradient projection method, and some heuristic algorithms. These numerical methods are versatile, but their solution time is too long, they are sensitive to initial values, and they also suffer from drawbacks such as local minima and the lack of a global solution. Unlike numerical methods, analytical methods are faster, more accurate, and can avoid many of the drawbacks of numerical methods. However, only robotic arms that satisfy the Pieper criterion can obtain analytical solutions for IK. For redundant robotic arms with multiple degrees of freedom, there are two ways to obtain analytical solutions: one is arm angle parameterization, which constrains the original parameters of the robotic arm by adjusting the arm angle; the other is joint angle parameterization, which treats one joint angle as known and derives its analytical solution. However, the former method only applies to industrial robotic arms where some joints satisfy the Pieper criterion. Rock drilling rigs do not have structures that satisfy the Pieper criterion, and therefore do not meet the conditions for parameterizing the arm's angle. Currently, the analytical solutions for rock drilling rigs are typically obtained by setting a single joint as known or by artificially defining constraints between joints. However, this undoubtedly limits the reachable space of the drilling arm and may involve complex analytical solution derivations. In summary, as a non-standard configuration robotic arm, the rock drilling rig lacks a method for inverse kinematics that is insensitive to initial conditions, has high accuracy, is simple to derive, and provides a fast solution that can meet real-time requirements. Summary of the Invention

[0003] The purpose of this invention is to provide an inverse kinematics control method for the drill arm of a rock drilling rig, which can quickly obtain the inverse kinematics solution of the drill arm of the rock drilling rig through a combination of numerical and analytical methods.

[0004] The technical solution adopted by the present invention to solve the above-mentioned technical problems is: a reverse solution control method for a drilling arm of a rock drilling rig, wherein the drilling arm includes multiple connecting rods, adjacent connecting rods are connected by joints, the starting end of the drilling arm is connected to the frame of the rock drilling rig, and the two joints closest to the starting end according to the sequential connection order of the multiple connecting rods are both rotary joints, one of which is defined as the first rotary joint and the other as the second rotary joint, the rotation axes of the first rotary joint and the second rotary joint are perpendicular to each other, and the rotation angle range of the first rotary joint and the second rotary joint does not exceed 180°.

[0005] The control method is as follows: First, a base coordinate system and a coordinate system for multiple joint positions are established according to the DH method. The base coordinate system is established at the starting end of the drill arm. When the first rotary joint does not rotate, the base coordinate system coincides with the coordinate system of the first rotary joint. The z-axis of the base coordinate system is parallel to the rotation axis of the first rotary joint, and the x-axis of the base coordinate system points to the rotation axis of the second rotary joint.

[0006] Then, based on the interconnection relationship of multiple links, the analytical solution formulas for the remaining joints other than the first and second rotary joints are established, and the expected pose matrix TA of the drill arm is given according to the target position and attitude of the drill arm.

[0007] Then, the rotation angles of the first and second rotary joints are initially assigned in the coordinate system to obtain the initial angle of the first rotary joint. i a0 The initial angle of the second rotational joint i b0 :

[0008] (1);

[0009] (2);

[0010] In equations (1) and (2), lb 1 and ub 1 represents the angle values ​​of the first rotary joint at its extreme rotational positions on both sides. lb 2 and ub 2 represents the angle values ​​of the second rotary joint at its extreme rotational positions on both sides. rand A random value between 0 and 1;

[0011] The initial angle of the first rotational joint i a0 The initial angle of the second rotational joint i b0 As initial variables, the angles of the first and second rotational joints are updated multiple times alternately, and the number of alternating update operations is accumulated sequentially. In each alternating update operation, the updated angle of the first rotational joint is first obtained through the first rotational joint update algorithm. i an The verification was performed, and then the update angle of the second rotational joint was obtained through the second rotational joint update algorithm. i bn And verify it. n This represents the number of alternating update operations currently being performed.

[0012] The first rotational joint update algorithm includes the following steps:

[0013] Step 1: Convert the desired pose matrix TA of the drill arm. i an-1 and i bn-1 Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated respectively, and then based on... i an-1 and i bn-1 The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an-1 / bn-1 Then, the stage pose matrix TB an-1 / bn-1 The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an-1 / bn-1 ;

[0014] Step 2: Calculate the desired pose matrix TA of the drill arm. i an-1 +Δ i and i bn-1 Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated, where Δ i ≤10 -5 *π°, then according to i an-1 +Δ i and i bn-1 The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an-1+Δ Then, the stage pose matrix TB an-1+Δ The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an-1+Δ ;

[0015] Step 3: From the error matrix eT an-1 / bn-1 Select the difference in the y-direction coordinates. e py And recorded as e py ( i an-1 ), from the error matrix eT an-1+Δ Select the difference in the y-direction coordinates. e py And recorded as e py ( i an-1 +Δ i Then, obtain the result through equation (3). e py With Δ i linear ratio between k 1:

[0016] (3);

[0017] Then, by using equation (4), we can obtain... i an :

[0018] (4);

[0019] Step 4: Calculate the desired pose matrix TA of the drill arm. i an and i bn-1 Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated respectively, and then based on... i an and i bn-1 The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an / bn-1 Then, the stage pose matrix TB an / bn-1 The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an / bn-1 ;

[0020] Step 5: For the error matrix eT an / bn-1 Summing the absolute values ​​of the differences between the x, y, and z coordinates, we get... i an and i bn-1 The corresponding total positional error ep an ,when ep an Not greater than the verification threshold ep At 0 o'clock, i an and i bn-1 The obtained joint analytical values, along with the corresponding values ​​for the remaining joints, are used as inverse kinematics output values ​​to control the drill arm to move to the target position. ep an Greater than the verification threshold ep When the value is 0, perform the second rotational joint update algorithm;

[0021] The second rotational joint update algorithm includes the following steps:

[0022] Step 1: Convert the desired pose matrix TA of the drill arm. i an and i bn-1 +Δ i Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated, where Δ i ≤10-5 *π°, then according to i an and i bn-1 +Δ i The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. bn-1+Δ Then, the stage pose matrix TB bn-1+Δ The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. bn-1+Δ ;

[0023] Step 2, from the error matrix eT an / bn-1 Select the difference in z-direction coordinates e pz And recorded as e pz ( i bn-1 ), from the error matrix eT bn-1+Δ Select the difference in the z-direction coordinates. e pz And recorded as e pz ( i bn-1 +Δ i Then, obtain the result using equation (5). e pz With Δ i linear ratio between k 2:

[0024] (5);

[0025] Then, by using equation (6), we can obtain... i bn :

[0026] (6);

[0027] Step 3: Calculate the desired pose matrix TA of the drill arm. i an and i bn Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated respectively, and then based on... i an and i bn The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an / bn Then, the stage pose matrix TB an / bn The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an / bn ;

[0028] Step 4: For the error matrix eT an / bn Summing the absolute values ​​of the differences between the x, y, and z coordinates, we get... i an and i bn The corresponding total positional error ep bn ,when ep bn Not greater than the verification threshold ep At 0 o'clock, i an and i bn The obtained joint analytical values, along with the corresponding values ​​for the remaining joints, are used as inverse kinematics output values ​​to control the drill arm to move to the target position. ep bn Greater than the verification threshold ep At time 0, perform the (n+1)th alternating update operation and set the error matrix eT. an / bn This is the output of step 1 in the (n+1)th first rotational joint update algorithm.

[0029] Preferably, when alternating the number of update operations n The cumulative loop threshold has been reached. n 0, and ep an and ep bn All are greater than the verification threshold ep At time 0, the rotation angles of the first and second rotary joints are re-initialized, and the new initial angles of the first and second rotary joints are used as initial variables. The angles of the first and second rotary joints are then repeatedly updated alternately.

[0030] Preferably, the cycle threshold n 0 is one of 13, 14, 15, or 16.

[0031] According to the above technical solution, the beneficial effects of the present invention are:

[0032] This invention parameterizes the first two joint angles of the drill arm to facilitate the derivation of analytical solutions for the remaining joints. It also provides a fast numerical solution for the rotation angles of the first two joint angles and establishes a fast cyclic solution algorithm structure that combines numerical and analytical solutions. This approach balances the accuracy and speed of the drill arm inverse solution, overcoming many shortcomings of existing inverse kinematics solutions when applied to the drill arm of a rock drilling rig. This facilitates real-time and precise control of the drill arm's motion. Attached Figure Description

[0033] Figure 1A simplified schematic diagram of the drill arm of a rock drilling rig;

[0034] Figure 2 The n-vector error for solving the value;

[0035] Figure 3 The o-vector error for solving the value;

[0036] Figure 4 The error of vector a in solving for the value;

[0037] Figure 5 This represents the end position error of the solution value.

[0038] The markings in the diagram are: 1. First rotational joint, 2. Second rotational joint, 3. Intermediate joint, 4. Translational joint. Detailed Implementation

[0039] This invention provides a reverse engineering control method for a drilling arm of a rock drilling rig. The applicable drilling arm includes multiple connecting rods, adjacent connecting rods are connected by joints, and the starting end of the drilling arm is connected to the frame of the rock drilling rig. According to the sequential connection order of the multiple connecting rods, the two joints closest to the starting end are both rotary joints, one defined as the first rotary joint and the other as the second rotary joint. The rotation axes of the first and second rotary joints are perpendicular to each other, and the rotation angle range of both the first and second rotary joints does not exceed 180°. Figure 1 The diagram shown is a simplified structural diagram of the drill arm. The first rotary joint 1 and the second rotary joint 2 are left-right swing joints and up-down pitch joints, respectively. Multiple connecting rods are then connected segment by segment through other joints such as the intermediate joint 3 and translation joint 4. Finally, the end of the drill arm is connected to the rock drilling rig.

[0040] The control method is as follows: First, a base coordinate system and a coordinate system for multiple joint positions are established according to the DH method. The base coordinate system is established at the starting end of the drill arm. When the first rotary joint does not rotate, the base coordinate system coincides with the coordinate system of the first rotary joint. The z-axis of the base coordinate system is parallel to the rotation axis of the first rotary joint, and the x-axis of the base coordinate system points to the rotation axis of the second rotary joint.

[0041] Then, based on the interconnection of multiple links, analytical solution formulas for the remaining joints other than the first and second rotary joints are established, and the expected pose matrix TA of the drill arm is given according to the target position and attitude of the drill arm.

[0042] Then, the rotation angles of the first and second rotary joints are initially assigned in the coordinate system to obtain the initial angle of the first rotary joint. i a0 The initial angle of the second rotational joint i b0 :

[0043] (1);

[0044] (2);

[0045] In equations (1) and (2), lb 1 and ub 1 represents the angle values ​​of the first rotary joint at its extreme rotational positions on both sides. lb 2 and ub 2 represents the angle values ​​of the second rotary joint at its extreme rotational positions on both sides. rand It is a random value between 0 and 1.

[0046] The initial angle of the first rotational joint i a0 The initial angle of the second rotational joint i b0 As initial variables, the angles of the first and second rotational joints are updated multiple times alternately, and the number of alternating update operations is accumulated sequentially. In each alternating update operation, the updated angle of the first rotational joint is first obtained through the first rotational joint update algorithm. i an The verification was performed, and then the update angle of the second rotational joint was obtained through the second rotational joint update algorithm. i bn And verify it. n This represents the number of alternating update operations currently being performed.

[0047] The first rotational joint update algorithm includes the following steps:

[0048] Step 1: Convert the desired pose matrix TA of the drill arm. i an-1 and i bn-1 Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated respectively, and then based on... i an-1 and i bn-1 The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an-1 / bn-1 Then, the stage pose matrix TB an-1 / bn-1 The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an-1 / bn-1 .

[0049] Step 2: Calculate the desired pose matrix TA of the drill arm. i an-1 +Δ i and ibn-1 Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated, where Δ i ≤10 -5 *π°, then according to i an-1 +Δ i and i bn-1 The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an-1+Δ Then, the stage pose matrix TB an-1+Δ The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an-1+Δ .

[0050] Step 3: From the error matrix eT an-1 / bn-1 Select the difference in the y-direction coordinates. e py And recorded as e py ( i an-1 ), from the error matrix eT an-1+Δ Select the difference in the y-direction coordinates. e py And recorded as e py ( i an-1 +Δ i Then, obtain the result through equation (3). e py With Δ i linear ratio between k 1:

[0051] (3);

[0052] Then, by using equation (4), we can obtain... i an :

[0053] (4);

[0054] By using trigonometric function operations in equation (4), we can avoid the case where the solution value is greater than 2π and constrain the solution value to the range of ±π / 2.

[0055] Step 4: Calculate the desired pose matrix TA of the drill arm. i an and i bn-1 Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated respectively, and then based on... i an and i bn-1 The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an / bn-1 Then, the stage pose matrix TB an / bn-1 The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an / bn-1 .

[0056] Step 5: For the error matrix eT an / bn-1 Summing the absolute values ​​of the differences between the x, y, and z coordinates, we get... i an and i bn-1 The corresponding total positional error ep an ,when ep an Not greater than the verification threshold ep At 0 o'clock, i an and i bn-1 The obtained joint analytical values, along with the corresponding values ​​for the remaining joints, are used as inverse kinematics output values ​​to control the drill arm to move to the target position. ep an Greater than the verification threshold ep When the value is 0, the second rotational joint update algorithm is performed.

[0057] The second rotational joint update algorithm includes the following steps:

[0058] Step 1: Convert the desired pose matrix TA of the drill arm. i an and i bn-1 +Δ i Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated, where Δ i ≤10 -5 *π°, then according to i an and i bn-1 +Δ i The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. bn-1+Δ Then, the stage pose matrix TB bn-1+Δ The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. bn-1+Δ .

[0059] Step 2, from the error matrix eT an / bn-1 Select the difference in the z-direction coordinates. e pz And recorded ase pz ( i bn-1 ), from the error matrix eT bn-1+Δ Select the difference in the z-direction coordinates. e pz And recorded as e pz ( i bn-1 +Δ i Then, obtain the result using equation (5). e pz With Δ i linear ratio between k 2:

[0060] (5);

[0061] Then, by using equation (6), we can obtain... i bn :

[0062] (6);

[0063] By using trigonometric function operations in equation (6), we can avoid the case where the solution value is greater than 2π and constrain the solution value to the range of ±π / 2.

[0064] Step 3: Calculate the desired pose matrix TA of the drill arm. i an and i bn Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated respectively, and then based on... i an and i bn The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an / bn Then, the stage pose matrix TB an / bn The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an / bn .

[0065] Step 4: For the error matrix eT an / bn Summing the absolute values ​​of the differences between the x, y, and z coordinates, we get... i an and i bn The corresponding total positional error ep bn ,when ep bn Not greater than the verification threshold ep At 0 o'clock, ian and i bn The obtained joint analytical values, along with the corresponding values ​​for the remaining joints, are used as inverse kinematics output values ​​to control the drill arm to move to the target position. ep bn Greater than the verification threshold ep At time 0, perform the (n+1)th alternating update operation and set the error matrix eT. an / bn This is the output of step 1 in the (n+1)th first rotational joint update algorithm.

[0066] When alternating update operations n The cumulative loop threshold has been reached. n 0, and ep an and ep bn All are greater than the verification threshold ep At time 0, the rotation angles of the first and second rotary joints are re-initialized, and the new initial angles of the first and second rotary joints are used as initial variables. The angles of the first and second rotary joints are then repeatedly updated alternately, with the loop threshold being calculated. n Setting 0 to one of 13, 14, 15, or 16 can prevent excessive calculations.

[0067] Example: The Q001 type drill arm was solved and verified. The MD-H parameters of the Q001 type drill arm are shown in Table 1.

[0068]

[0069] The drill arm is a 7-DOF drill arm with two translational joints and five rotational joints. Its first joint is a swing joint and its second joint is a pitch joint. The angles of the first two joints are parameterized, and then the analytical solutions of the remaining joint angles are derived. The process is as follows.

[0070] First, derive the pose transformation matrix between the robotic arm's end effector and the base coordinate system, where the transformation of coordinate system i relative to coordinate system i-1 is as follows:

[0071]

[0072] The transformation matrices of each joint coordinate system relative to the previous coordinate system are derived from this formula as follows:

[0073]

[0074] According to the left multiplication rule, by multiplying the homogeneous transformation matrices of each joint of the robotic arm, the transformation matrix of the robotic arm's end effector relative to the base coordinate system can be obtained as follows:

[0075] ;

[0076] ;

[0077] in:

[0078]

[0079] Then, assuming joints 1 and 2 are known, solve for the closed-form solution of the remaining joints:

[0080] (1) Solve i 5

[0081] ;

[0082] ;

[0083] ;

[0084] From -60°≤ i 6≤14°, therefore we know c 6 > 0; therefore c The positive and negative properties of 5 and c 5 c 6 are the same;

[0085] when c 5 c When 6 > 0, c 5 > 0, at this time:

[0086] ;

[0087] when c 5 c When 6 < 0, c 5 < 0, at this time if s If 5 > 0, then:

[0088] ;

[0089] If at this time s If 5 < 0, then:

[0090] ;

[0091] (2) Solve i 4. i 6

[0092] ;

[0093] ;

[0094] ;

[0095] ;

[0096] when At that time, we can obtain:

[0097] ;

[0098] ;

[0099] when At that time, we can obtain:

[0100]

[0101] but: ;

[0102] when At that time, we can obtain:

[0103]

[0104] but: ;

[0105] At this moment, the robotic arm is in a strange state. i 4. i The specific value of 6 can be determined by the solution values ​​of the neighboring points of the point to be solved;

[0106] (4) Solve d 7

[0107] when At that time, we can obtain:

[0108] ;

[0109] at this time:

[0110] ;

[0111] when At that time, we can obtain:

[0112] ;

[0113] ;

[0114] Furthermore, we can obtain:

[0115] ;

[0116] when Simultaneously established, at this time i 5 i 6 = 0, therefore:

[0117] ;

[0118] ;

[0119] At this moment, the robotic arm is in a strange pose. d 3. d The specific value of 7 can be determined by the solution values ​​of the neighboring points of the point to be solved;

[0120] (5) Solve d 3

[0121] make ;

[0122] Because -42°≤ i 1≤42°, -55°≤ i 2≤42°, therefore ,

[0123] We can obtain: .

[0124] Then, an inverse kinematics algorithm was established, randomly generating 30,000 sets of joint variables in the joint space of the Q001 drill arm, generating poses and solving them. The solution took a total of 15 seconds, with an average solution speed of about 0.5ms per point, meeting the real-time control requirements. Five sets of joint vectors were extracted, and the solution results are shown in Table 2.

[0125]

[0126] The pose errors of the five sets of solutions are as follows: Figure 2-5 As shown, the attitude error is controlled to a minimum, with a maximum position error of 0.035mm, which meets the working requirements of the drilling arm of the rock drilling rig.

[0127] This invention parameterizes the first two joint angles of the drill arm and provides a fast numerical solution for the rotation angles of the first two joint angles. It establishes a fast cyclic solution algorithm structure that combines numerical and analytical solutions, while taking into account both the solution accuracy and solution speed of the drill arm inverse solution, which is beneficial for precise control of the position and posture of the drill arm of the rock drilling rig.

Claims

1. A reverse engineering control method for a drilling arm of a rock drilling rig, wherein the drilling arm includes multiple connecting rods, adjacent connecting rods are connected by joints, the starting end of the drilling arm is connected to the frame of the rock drilling rig, and the two joints closest to the starting end according to the sequential connection order of the multiple connecting rods are both rotary joints, one of which is defined as the first rotary joint and the other as the second rotary joint, the rotation axes of the first rotary joint and the second rotary joint are perpendicular to each other, and the rotation angle range of the first rotary joint and the second rotary joint does not exceed 180°. The control method is as follows: First, a base coordinate system and a coordinate system for multiple joint positions are established according to the DH method. The base coordinate system is established at the starting end of the drill arm. When the first rotary joint does not rotate, the base coordinate system coincides with the coordinate system of the first rotary joint. The z-axis of the base coordinate system is parallel to the rotation axis of the first rotary joint, and the x-axis of the base coordinate system points to the rotation axis of the second rotary joint. Then, based on the interconnection relationship of multiple links, the analytical solution formulas for the remaining joints other than the first and second rotary joints are established, and the expected pose matrix TA of the drill arm is given according to the target position and attitude of the drill arm. Its features are: Then, the rotation angles of the first and second rotary joints are initially assigned in the coordinate system to obtain the initial angle of the first rotary joint. θ a0 The initial angle of the second rotational joint θ b0 : （1）； （2）； In equations (1) and (2), lb 1 and ub 1 represents the angle values ​​of the first rotary joint at its extreme rotational positions on both sides. lb 2 and ub 2 represents the angle values ​​of the second rotary joint at its extreme rotational positions on both sides. rand A random value between 0 and 1; The initial angle of the first rotational joint θ a0 The initial angle of the second rotation joint θ b0 As initial variables, the angles of the first and second rotational joints are updated multiple times alternately, and the number of alternating update operations is accumulated sequentially. In each alternating update operation, the updated angle of the first rotational joint is first obtained through the first rotational joint update algorithm. θ an The verification was performed, and then the update angle of the second rotational joint was obtained through the second rotational joint update algorithm. θ bn And verify it. n This represents the number of alternating update operations currently in progress. The first rotational joint update algorithm includes the following steps: Step 1: Convert the desired pose matrix TA of the drill arm. θ an-1 and θ bn-1 Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated respectively, and then based on... θ an-1 and θ bn-1 The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an-1 / bn-1 Then, the stage pose matrix TB an-1 / bn-1 The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an-1 / bn-1 ; Step 2: Calculate the desired pose matrix TA of the drill arm. θ an-1 +Δ θ and θ bn-1 Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated, where Δ θ ≤10 -5 *π°, then according to θ an-1 +Δ θ and θ bn-1 The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an-1+Δ Then, the stage pose matrix TB an-1+Δ The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an-1+Δ ; Step 3: From the error matrix eT an-1 / bn-1 Select the difference in the y-direction coordinates. e py And recorded as e py ( θ an-1 ), from the error matrix eT an-1+Δ Select the difference in the y-direction coordinates. e py And recorded as e py ( θ an-1 +Δ θ Then, obtain the result through equation (3). e py With Δ θ linear ratio between k 1: （3）； Then, by using equation (4), we can obtain... θ an : （4）； Step 4: Calculate the desired pose matrix TA of the drill arm. θ an and θ bn-1 Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated respectively, and then based on... θ an and θ bn-1 The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an / bn-1 Then, the stage pose matrix TB an / bn-1 The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an / bn-1 ; Step 5: For the error matrix eT an / bn-1 Summing the absolute values ​​of the differences between the x, y, and z coordinates, we get... θ an and θ bn-1 The corresponding total positional error ep an ,when ep an Not greater than the verification threshold ep At 0 o'clock, θ an and θ bn-1 The obtained joint analytical values, along with the corresponding values ​​for the remaining joints, are used as inverse kinematics output values ​​to control the drill arm to move to the target position. ep an Greater than the verification threshold ep When the value is 0, perform the second rotational joint update algorithm; The second rotational joint update algorithm includes the following steps: Step 1: Convert the desired pose matrix TA of the drill arm. θ an and θ bn-1 +Δ θ Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated, where Δ θ ≤10 -5 *π°, then according to θ an and θ bn-1 +Δ θ The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. bn-1+Δ Then, the stage pose matrix TB bn-1+Δ The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. bn-1+Δ ; Step 2, from the error matrix eT an / bn-1 Select the difference in z-direction coordinates e pz And recorded as e pz ( θ bn-1 ), from the error matrix eT bn-1+Δ Select the difference in z-direction coordinates e pz And recorded as e pz ( θ bn-1 +Δ θ Then, obtain the result using equation (5). e pz With Δ θ linear ratio between k 2: （5）； Then, by using equation (6), we can obtain... θ bn : （6）； Step 3: Calculate the desired pose matrix TA of the drill arm. θ an and θ bn Substituting the input values ​​into the analytical solution formulas for the remaining joints, the analytical values ​​for the remaining joints are calculated respectively, and then based on... θ an and θ bn The drill arm's stage pose matrix TB is obtained by performing forward kinematics operations on the corresponding joint analytical values ​​and the remaining joint analytical values. an / bn Then, the stage pose matrix TB an / bn The error matrix eT is obtained by performing a difference operation with the desired pose matrix TA. an / bn ; Step 4: For the error matrix eT an / bn Summing the absolute values ​​of the differences between the x, y, and z coordinates, we get... θ an and θ bn The corresponding total positional error ep bn ,when ep bn Not greater than the verification threshold ep At 0 o'clock, θ an and θ bn The obtained joint analytical values, along with the corresponding values ​​for the remaining joints, are used as inverse kinematics output values ​​to control the drill arm to move to the target position. ep bn Greater than the verification threshold ep At time 0, perform the (n+1)th alternating update operation and set the error matrix eT. an / bn This is the output of step 1 in the (n+1)th first rotational joint update algorithm.

2. The inverse kinematics control method for the drill arm of a rock drilling rig according to claim 1, characterized in that: When alternating update operations n The cumulative loop threshold has been reached. n 0, and ep an and ep bn All are greater than the verification threshold ep At time 0, the rotation angles of the first and second rotary joints are re-initialized, and the new initial angles of the first and second rotary joints are used as initial variables. The angles of the first and second rotary joints are then repeatedly updated alternately.

3. The inverse kinematics control method for the drill arm of a rock drilling rig according to claim 2, characterized in that: Cyclic threshold n 0 is one of 13, 14, 15, or 16.

Images