Raise boring machine mechanical arm control method based on Hermite interpolation algorithm

Abstract

The invention discloses a raise boring machine mechanical arm control method based on a Hermite interpolation algorithm. The Hermite interpolation polynomial of the displacement of a raise boring machine mechanical arm hydraulic cylinder is constructed according to preset key track points of a raise boring machine mechanical arm; according to response characteristics of the raise boring machine mechanical arm hydraulic control system, a control period is determined, a track segment to which the current moment belongs is calculated, and the normalized relative time of the current moment relative to the track segment is solved; substituting the relative time into the Hermite interpolation polynomial of the corresponding track segment to obtain the target displacement of the hydraulic cylinder in the current period; the speed and the acceleration corresponding to the target displacement of the hydraulic cylinder in the current period are verified, and if the speed and the acceleration exceed the physical limit values of the hydraulic cylinder, the target displacement of the hydraulic cylinder in the current period is cut off to be the physical limit values of the hydraulic cylinder; the target displacement of the hydraulic cylinder in the current period serves as a target value of a PID controller; pID closed-loop control is realized in combination with the actual displacement of the hydraulic cylinder collected by the displacement sensor.

Description

Technical Field

[0001] This invention relates to the field of drilling rig robotic arm control technology, and is particularly applicable to a control method for a riser drilling rig robotic arm based on the Hermite interpolation algorithm. Background Technology

[0002] A well drilling rig is a type of mechanical equipment that uses rotary drilling to break rocks and create holes, and can also reverse the hole to enlarge it. Currently, well drilling rigs are widely used in pumped storage projects in my country. The robotic arm, as the core component of the well drilling rig for transporting drill pipes, not only requires high transport speed but also must ensure high positioning accuracy. However, currently, the robotic arm of well drilling rigs is mainly operated manually via a joystick, and the operating speed of its driving hydraulic cylinder is constant. Impacts often occur during start-up and shutdown, leading to a decrease in the robotic arm's transport accuracy after prolonged operation, requiring frequent adjustments of the limit bolts to maintain precision. Therefore, improving the operating speed of the well drilling rig robotic arm while reducing start-up and shutdown impacts and improving its positioning accuracy has become a critical issue that urgently needs to be addressed. Summary of the Invention

[0003] The purpose of this invention is to provide a control method for the robotic arm of a well drilling rig based on the Hermite interpolation algorithm, in order to solve the problem of insufficient positioning accuracy of the robotic arm of a well drilling rig.

[0004] To achieve the above objectives, the hoist robotic arm control method based on the Hermite interpolation algorithm described in this invention includes the following steps: S1, Construct the Hermite interpolation polynomial for the displacement of the hydraulic cylinder of the hoisting arm based on the preset key trajectory points of the hoisting arm. S2, determine the control cycle based on the response characteristics of the hydraulic control system of the hoisting rig, calculate the trajectory segment to which the current moment belongs, and solve the normalized relative time of the current moment relative to the trajectory segment to which it belongs; S3, substitute the relative time into the corresponding trajectory segment Hermite interpolation polynomial to obtain the target displacement of the hydraulic cylinder in the current period; S4, check the velocity and acceleration corresponding to the target displacement of the hydraulic cylinder in the current cycle. If the velocity and acceleration exceed the physical limit value of the hydraulic cylinder, then cut off the target displacement of the hydraulic cylinder in the current cycle to the physical limit value of the hydraulic cylinder. S5 uses the target displacement of the hydraulic cylinder in the current cycle as the target value of the PID controller; combined with the actual displacement of the hydraulic cylinder collected by the displacement sensor, PID closed-loop control is achieved.

[0005] Furthermore, the Hermite interpolation polynomial is the product of the displacement and velocity of the key trajectory points of the hoisting rig's robotic arm and their corresponding basis functions, and then summed.

[0006] Furthermore, the displacement basis functions of the trajectory points are: The velocity basis function of the trajectory point is ;in, , where t is the current interpolation time; The time for key trajectory point i; denoted as , where is the time for other trajectory points, and n is the total number of key trajectory points.

[0007] Furthermore, by measuring the motion characteristics of the hoist's robotic arm under no-load and full-load conditions, the speed of key trajectory points was corrected; ensuring the continuity of the start / end acceleration in the Hermite interpolation polynomial, thus reducing start-stop shock.

[0008] The advantage of this invention lies in the use of a closed-loop control system based on the Hermite interpolation algorithm, which can not only reduce start-stop shocks but also control the robotic arm to transport drill pipes more quickly and accurately, thus achieving automation. Attached Figure Description

[0009] Figure 1 is a flowchart of the method described in this invention. Detailed Implementation

[0010] The technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0011] The flowchart of the hoist robotic arm control method based on the Hermite interpolation algorithm described in this invention is as follows: Figure 1 As shown, the steps include: S1, construct the Hermite interpolation polynomial of the hydraulic cylinder displacement of the hoisting arm based on the preset key trajectory points of the hoisting arm.

[0012] The following explanation uses three key trajectory points as an example. We select three key trajectory points, denoted as the starting point M0, the inflection point M1, and the ending point M2; their corresponding times are t0, t1, and t2, respectively, where t0 is the start time and t2 is the end time. We determine the target displacement s of the hydraulic cylinder at each key trajectory point. i and target velocity v i s represents the target position, v represents the target velocity, and i represents the key trajectory point i. In this embodiment, i is an integer ∈ [0, 2].

[0013] By measuring the motion characteristics of the hoisting arm under no-load and full-load conditions, the displacement and velocity of key trajectory points are corrected, thereby ensuring the continuity of the start-point and end-point accelerations in the obtained Hermite interpolation polynomial and effectively reducing start-stop shocks.

[0014] Define the displacement basis function of key trajectory point i as follows: The velocity basis function of the key trajectory point i is: ;in, Where t is the current interpolation time; The time for key trajectory point i; The time for other trajectory points. n is the total number of key trajectory points, which is 3 in this embodiment.

[0015] Based on the preset key trajectory points of the hoisting arm, Hermite interpolation is used to generate the trajectory polynomial of the hoisting arm, i.e., the Hermite interpolation polynomial. The Hermite interpolation polynomial is the sum of the position value of each key trajectory point multiplied by the displacement basis function corresponding to that key trajectory point and the velocity value of each key trajectory point multiplied by the velocity basis function corresponding to that key trajectory point.

[0016] The Hermite interpolation polynomial obtained in this embodiment is: ,in Let i be the target displacement of the hydraulic cylinder at the key trajectory point i. Let i be the target velocity of the hydraulic cylinder at the critical trajectory point i.

[0017] To reduce real-time computation and improve numerical stability, the Hermite interpolation polynomials described above are substituted. The process of the hoisting rig's robotic arm moving from the starting point to the ending point is divided into n-1 trajectory segments by n key trajectory points. Each trajectory segment needs to be normalized to obtain the Hermite interpolation polynomial for each segment.

[0018] In this embodiment, three key trajectory points divide the process of the hoist's robotic arm moving from the starting point to the ending point into two sub-intervals. Let the total time for each sub-interval be T. In this embodiment, the movement of the hoist's robotic arm from the starting point M0 to the inflection point M1 constitutes the first sub-interval, or the first trajectory segment. The movement from the inflection point M1 to the ending point M2 constitutes the second sub-interval, or the second trajectory segment. The total time for the first sub-interval... The total time of the second sub-interval .

[0019] Define normalized time ,in Let m be the total time for the m-th subinterval. m is an integer in [1, n-1], and n is the total number of key trajectory points.

[0020] Then, by substitution, the Hermite interpolation polynomial for the three key trajectory points in this embodiment can be simplified to a polynomial in τ. Based on the known constraints, the coefficients of the polynomial are calculated using the Hermite algorithm, thus deriving the polynomial function.

[0021] S2, determine the control period T based on the response characteristics of the hydraulic control system of the hoisting rig's robotic arm. c , such as T c =30ms, calculate the current time t. now The trajectory is segmented, and the current time t is determined. now Normalized relative time relative to the trajectory segment .

[0022] when When, it means the current time t now It is in the k-th trajectory segment. And at the current time t now The normalized time for the k-th trajectory segment is ,in, Let k be the total time for the k-th subinterval. k is an integer in the range [1, n-1], and n is the total number of key trajectory points. For key trajectory point M k-1 The corresponding moment.

[0023] S3, relative time Substituting the corresponding Hermite interpolation polynomial, we obtain the target displacement of the hydraulic cylinder in the current period.

[0024] S4: Verify the velocity and acceleration corresponding to the target displacement of the hydraulic cylinder in the current cycle. If the velocity and acceleration exceed the physical limits of the hydraulic cylinder, then truncate the target displacement of the hydraulic cylinder in the current cycle to the physical limits. If the velocity and acceleration do not exceed the physical limits of the hydraulic cylinder, then no processing is required.

[0025] S5 uses the target displacement of the hydraulic cylinder in the current cycle as the target value of the PID controller; based on the actual displacement of the hydraulic cylinder collected by the displacement sensor, PID closed-loop control is achieved. Specifically, the target displacement of the hydraulic cylinder in the current cycle is used as the target value of the PID controller, resulting in a PID control analog output. This output adjusts the proportional valve of the hydraulic cylinder, controlling its extension and retraction. Meanwhile, the hydraulic cylinder extension / retraction displacement sensor collects the actual displacement of the hydraulic cylinder and feeds the displacement data back to the PID controller, comparing it with the target value to achieve closed-loop control of the actual displacement of the hydraulic cylinder.

Claims

1. A control method for a hoist robotic arm based on the Hermite interpolation algorithm, characterized in that, Includes the following steps: S1, Construct the Hermite interpolation polynomial for the displacement of the hydraulic cylinder of the hoisting arm based on the preset key trajectory points of the hoisting arm. S2, determine the control cycle based on the response characteristics of the hydraulic control system of the hoisting rig, calculate the trajectory segment to which the current moment belongs, and solve the normalized relative time of the current moment relative to the trajectory segment to which it belongs; S3, substitute the relative time into the Hermite interpolation polynomial of the corresponding trajectory segment to obtain the target displacement of the hydraulic cylinder in the current period; S4, check the velocity and acceleration corresponding to the target displacement of the hydraulic cylinder in the current cycle. If the velocity and acceleration exceed the physical limit value of the hydraulic cylinder, then cut off the target displacement of the hydraulic cylinder in the current cycle to the physical limit value of the hydraulic cylinder. S5 uses the target displacement of the hydraulic cylinder in the current cycle as the target value of the PID controller; combined with the actual displacement of the hydraulic cylinder collected by the displacement sensor, PID closed-loop control is achieved.

2. The method for controlling the robotic arm of a well drilling rig based on the Hermite interpolation algorithm according to claim 1, characterized in that: The Hermite interpolation polynomial is the sum of the position values ​​of each key trajectory point of the hoisting rig manipulator multiplied by the displacement basis function corresponding to the key trajectory point, and the velocity values ​​of each key trajectory point multiplied by the velocity basis function corresponding to the key trajectory point.

3. The method for controlling the robotic arm of a riser drilling rig based on the Hermite interpolation algorithm according to claim 2, characterized in that: The displacement basis function of the trajectory point is: The velocity basis function of the trajectory point is ;in, , where t is the current interpolation time; The time for key trajectory point i; denoted as , where is the time for other trajectory points, and n is the total number of key trajectory points.

4. The method for controlling the robotic arm of a riser drilling rig based on the Hermite interpolation algorithm according to claim 1, characterized in that: By measuring the motion characteristics of the hoist robotic arm under no-load and full-load conditions, the velocity of key trajectory points is corrected; the start-point and end-point accelerations in the Hermite interpolation polynomial are made continuous, reducing start-stop shock.

Images