Automatic rotary positioning control system of rotary drilling rig
By using data fusion of angle sensors, acceleration sensors, gyroscopes and Kalman filters in rotary drilling rigs, the best control strategy is generated, and the problems of insufficient positioning accuracy and unstable operation of traditional rotary drilling rigs are solved, achieving higher rotation angle control accuracy and system stability.
Patent Information
- Application Number
- CN202411248700.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-06
- Publication Date
- 2024-11-08
AI Technical Summary
Traditional rotary drilling rigs have problems such as insufficient positioning accuracy, unstable operation and slow rotation angle adjustment in drilling operations. The existing automated control systems have insufficient data fusion accuracy and real-time feedback capabilities in data processing, control strategy formulation and feedback adjustment. Poor control strategy is not optimized enough.
A rotary drilling rig automatic rotation positioning control system is designed, including angle sensor, acceleration sensor, gyroscope, central control unit and electro-hydraulic proportional valve control module. The sensor data is fused through a Kalman filter to generate the optimal rotation angle. Estimates and control strategies use wireless networks to monitor and adjust control signals in real time to ensure accurate control of the rotation angle of the rotary drilling rig.
It improves the accuracy and robustness of the rotation angle estimation value, improves the stability and reliability of the system, achieves more stable and accurate drilling operations, and enhances the system's response speed and operation stability.
Smart Images

Figure CN118911663A_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of rotary drilling rigs, and in particular relates to an automatic rotation positioning control system for a rotary drilling rig. Background Art
[0002] Rotary drilling rigs are widely used in engineering construction, especially in the fields of foundation engineering and underground engineering. They are mainly used for drilling operations, and their efficient and precise operating performance has a vital impact on engineering quality and construction efficiency. Traditional rotary drilling rigs usually rely on manual operation and mechanical feedback to control the rotation angle of the drilling rig during drilling operations. However, due to the complexity and variability of the drilling rig operating environment, traditional control methods often face problems such as insufficient positioning accuracy, unstable operation, and slow rotation angle adjustment.
[0003] With the development of technology, automated control systems are gradually being used in rotary drilling rigs. These systems are designed to improve the positioning accuracy and operational stability of the drilling rig through real-time data acquisition and intelligent control. Modern automatic rotary positioning control systems usually include angle sensors, acceleration sensors and gyroscopes, which can collect the angle, acceleration and angular velocity data of the drilling rig in real time. These data are crucial for the dynamic adjustment and precise control of the system. However, the prior art still has certain challenges in data processing, control strategy formulation and feedback adjustment, such as insufficient data fusion accuracy, poor real-time feedback capability, and insufficient optimization of control strategies. Based on this, the present invention hopes to provide an automatic rotary positioning control system for a rotary drilling rig. Summary of the invention
[0004] The object of the present invention is to provide an automatic rotation positioning control system for a rotary drilling rig to solve the problems raised in the above-mentioned background technology.
[0005] To achieve the above object, the present invention provides the following technical solutions:
[0006] A rotary drilling rig automatic rotation positioning control system comprises: an angle sensor, an acceleration sensor, a gyroscope, a central control unit, a target rotation angle setting module and an electro-hydraulic proportional valve control module; the angle sensor, the acceleration sensor and the gyroscope are used to collect the angle, acceleration and angular velocity data of the rotary drilling rig in real time, and transmit the data to the central control unit; the central control unit calculates the estimated value of the optimal rotation angle of the rotary drilling rig based on the data obtained from the angle sensor, the acceleration sensor and the gyroscope, and estimates the current posture and motion state of the rotary drilling rig using the data fusion result, so as to provide accurate input data for the control system; the target rotation angle setting module sets the target rotation angle of the rotary drilling rig according to the operation requirements or the preset operation plan, and generates the optimal control strategy from the current angle to the target angle; the electro-hydraulic proportional valve control module converts the generated control strategy into the control signal of the electro-hydraulic proportional valve, so as to accurately control the rotation action of the rotary drilling rig.
[0007] Preferably, the angle sensor, acceleration sensor and gyroscope are respectively installed on the rotating part, center of gravity and rotating axis of the rotary drilling rig to ensure the accuracy and real-time performance of the data.
[0008] Preferably, the sensor data is transmitted to the central control unit via a wireless network.
[0009] Preferably, the central control unit is used to filter the collected data to make the system more stable during actual operation.
[0010] Preferably, the central control unit uses a Kalman filter to fuse data from the angle sensor, the acceleration sensor and the gyroscope to generate a more accurate and robust rotation angle estimation value;
[0011] Settings: N represents the number of sensors. The sensor data fusion part calculates the overall calibration value and measurement error by summing the data of N sensors; M represents the number of samples of angular velocity data or the number of angular velocity sensors; θ t is the estimated value of the rotation angle at time t; α t is the angular acceleration at time t; ω t is the angular velocity at time t;
[0012] The specific steps are as follows:
[0013] S1: Determine the noise filter function of the weighted sum of angular velocities as g(x), which is in the form of:
[0014] In the above formula, the noise filter function performs exponential weighted summation on different angular velocity data to ensure that the filtered angular velocity data is more accurate and stable;
[0015] represents the exponential decay of the j-th angular velocity, with the weight being λ j Decide;
[0016] λ j : Weight parameter, which controls the influence of different angular velocity data in noise filtering. j sets the serial number of the sample or angular velocity sensor used to index the angular velocity data. In the noise filtering part, by processing each angular velocity data sample or sensor data, combined with the weight parameter λ j Realize weighted average of different angular velocity data;
[0017] S2: Determine the correction and weighted sum of the sensor measurement values; correct the measurement value of each sensor, and the corrected value is: z i,t -k i ·P i,t ;
[0018] In the above formula, z i,t is the measurement value of the i-th sensor at time t; P i,t : is the covariance matrix, which is used to measure the distribution of the estimation error and is dynamically updated over time; k i : Kalman gain, which reflects the impact of the current measurement value on the estimated value and is dynamically adjusted through the Kalman filter algorithm; k i ·P i,t represents the calibration deviation of the i-th sensor;
[0019] The modified values are then weighted summed:
[0020] In the above formula, h i (x) is a sensor calibration function based on feature x, which is used to correct the inaccuracy of sensor readings, using a function based on linear or nonlinear regression, such as h i (x) = β i x+γ i , where β i and γ i is the calibration parameter;
[0021] S3: Attenuation term of combined acceleration:
[0022] Consider the acceleration term that decays over time: This formula is used to correct the angular velocity in combination with the time integral, taking into account the effect of acceleration decay over time;
[0023] In this formula, α t ·e -αt It represents the acceleration that decays with time; It represents the integral of the angular velocity from time 0 to time t, representing the change in the rotation angle; α: represents the attenuation coefficient, which controls the degree to which the influence of acceleration on the estimated value decays over time; dt: represents a small time increment. During the integration process, dt is used to discretize time, convert the continuous time integral into the discrete time sum, and calculate the estimated value of the rotation angle from the initial time to the current time.
[0024] Preferably, the estimated value of the rotation angle from the initial time to the current time is calculated by time integration, and the calculation formula is as follows:
[0025]
[0026] Among them, θ t : The final calculated rotation angle estimate is a dynamically changing quantity whose value range depends on the range of the sensor measurement value and the working state of the system, and usually varies between [-360°, 360°].
[0027] In the above formula, the integral function: calculates the change in rotation angle from the initial time to the current time by integrating the time t;
[0028] Complex summation function: Through the summation function, the data of different sensors are integrated, while taking into account the impact of sensor calibration and Kalman gain on the data;
[0029] Noise filtering: Use Gaussian filter to filter the angular velocity data to ensure the accuracy and stability of the data;
[0030] Exponential function: Use the exponential function to weight the angular velocity data to enhance the response to sudden changes in a short period of time;
[0031] Fractional structure: The effects of acceleration and the integral of angular velocity are combined through the fractional structure to form an accurate calculation of the estimated value of the rotation angle;
[0032] This formula achieves accurate estimation of the rotation angle of the rotary drilling rig by integrating multi-sensor data fusion and complex mathematical functions, while taking into account data noise filtering and sensor calibration issues to ensure the accuracy and robustness of the estimated value.
[0033] Preferably, in the target rotation angle setting module, the method for generating the best control strategy from the current angle to the target angle is: calculating the control signal according to the angle difference, assuming that the proportional control k is used p , then the control signal = k p ×Angle difference.
[0034] As a preferred choice, the optimal control strategy achieves precise speed control by controlling the current and voltage of the motor by: monitoring the pressure changes of the hydraulic system to ensure the stability and response speed of the system; this control strategy dynamically adjusts the motor current and hydraulic system pressure, and updates the control signal in real time to ensure precise control of the rotation angle of the rotary drilling rig.
[0035] Preferably, in the electro-hydraulic proportional valve control module, after converting the generated optimal control strategy into the control signal of the electro-hydraulic proportional valve, the central control unit accurately adjusts the motor and hydraulic system through the control signal to realize the rotation action of the rotary drilling rig; during the rotation process, the angle, acceleration and angular velocity data are monitored in real time, and then the control signal is continuously and dynamically adjusted in the above manner to ensure precise control of the rotation angle.
[0036] Preferably, the posture of the rotary drilling rig represents the rotation angle of the rotary drilling rig, and the motion state specifically refers to the angular velocity and angular acceleration.
[0037] Compared with the prior art, the present invention has the following beneficial effects:
[0038] (1) The automatic rotation positioning control system of a rotary drilling rig provided by the present invention uses a Kalman filter to fuse the data of an angle sensor, an acceleration sensor and a gyroscope, thereby improving the accuracy and robustness of the rotation angle estimation value. By filtering and removing noise from the data, the consistency of the data at different time points is ensured, thereby improving the stability and reliability of the system;
[0039] (2) The present invention provides an automatic rotation positioning control system for a rotary drilling rig, in which a central control unit calculates an optimal rotation angle estimate based on real-time data and generates an optimal control strategy from a current angle to a target angle. This strategy can accurately control the rotation action of the rotary drilling rig to achieve a more stable and accurate drilling operation.
[0040] (3) The present invention provides an automatic rotation positioning control system for a rotary drilling rig. The system can monitor the angle, acceleration and angular velocity data of the drilling rig in real time, and dynamically adjust the control signal to ensure that the rotation angle of the drilling rig is accurately controlled according to preset requirements. By dynamically adjusting the motor current and hydraulic system pressure, the response speed and operational stability of the system are further improved. BRIEF DESCRIPTION OF THE DRAWINGS
[0041] Figure 1 It is a schematic diagram of the overall structure of the present invention.
[0042] Figure 2 It is the work flow chart of the present invention. DETAILED DESCRIPTION
[0043] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0044] Example 1
[0045] Reference Figures 1-2 , which is the first embodiment of the present invention, provides an automatic rotation positioning control system for a rotary drilling rig, including an angle sensor, an acceleration sensor and a gyroscope, which are used to collect the angle, acceleration and angular velocity data of the rotary drilling rig in real time and transmit the data to a central control unit; the angle sensor, the acceleration sensor and the gyroscope are respectively installed on the rotating part, the center of gravity and the rotating axis of the rotary drilling rig to ensure the accuracy and real-time nature of the data; the sensor data is transmitted to the central control unit via a wireless network.
[0046] The central control unit calculates the optimal rotation angle estimate of the rotary drilling rig based on the data obtained from the angle sensor, acceleration sensor and gyroscope, and uses the data fusion result to estimate the current posture and motion state of the rotary drilling rig (the posture of the rotary drilling rig represents the rotation angle of the rotary drilling rig, and the motion state specifically refers to the angular velocity and angular acceleration), providing accurate input data for the control system.
[0047] The central control unit is used to filter the collected data to make the system more stable during actual operation; specifically: the central control unit uses the Kalman filter to fuse the data from the angle sensor, acceleration sensor and gyroscope to generate a more accurate and robust rotation angle estimate.
[0048] Let N represent the number of sensors. The sensor data fusion part calculates the overall calibration value and measurement error by summing the data of N sensors. M represents the number of samples of angular velocity data or the number of angular velocity sensors. θ t is the estimated value of the rotation angle at time t; α t is the angular acceleration at time t; ω t is the angular velocity at time t.
[0049] The specific steps are as follows:
[0050] S1: Determine the noise filter function of the weighted sum of angular velocities as g(x), which is in the form of:
[0051] In the above formula, the noise filter function performs exponential weighted summation on different angular velocity data to ensure that the filtered angular velocity data is more accurate and stable;
[0052] represents the exponential decay of the j-th angular velocity, with the weight being λ j Decide;
[0053] λ j : Weight parameter, which controls the influence of different angular velocity data in noise filtering. j sets the serial number of the sample or angular velocity sensor used to index the angular velocity data. In the noise filtering part, by processing each angular velocity data sample or sensor data, combined with the weight parameter λ j Realize the weighted average of different angular velocity data.
[0054] S2: Determine the correction and weighted sum of the sensor measurement values: Correct the measurement value of each sensor to obtain the corrected value: z i,t -k i ·P i,t .
[0055] In the above formula, z i,t is the measurement value of the i-th sensor at time t; P i,t : is the covariance matrix, which is used to measure the distribution of the estimation error and is dynamically updated over time; k i : Kalman gain, which reflects the impact of the current measurement value on the estimated value and can be dynamically adjusted through the Kalman filter algorithm; k i ·P i,t represents the calibration deviation of the i-th sensor;
[0056] These corrections are then weighted summed:
[0057] In the above formula, h i (x) is a sensor calibration function based on feature x, which is used to correct the inaccuracy of sensor readings. It can use a function based on linear or nonlinear regression, such as h i (x) = β i x+γ i , where β i and γ i is the calibration parameter.
[0058] S3: Attenuation term of combined acceleration.
[0059] Consider the acceleration term that decays over time:
[0060] This formula is used to correct the angular velocity in combination with the time integral, taking into account the effect of acceleration decay over time.
[0061] In this formula, α t ·e -αt It represents the acceleration that decays with time; It represents the integral of the angular velocity from time 0 to time t, representing the change in the rotation angle. α: represents the attenuation coefficient, which controls the degree to which the influence of acceleration on the estimated value decays over time. dt: represents a small time increment. During the integration process, dt is used to discretize time, convert the continuous time integral into a discrete time sum, and calculate the estimated value of the rotation angle from the initial time to the current time.
[0062] The estimated value of the rotation angle from the initial time to the current time is calculated by time integration. The calculation formula is as follows:
[0063]
[0064] θ t : The final calculated rotation angle estimate is a dynamically changing quantity whose value range depends on the range of the sensor measurement value and the working state of the system, and usually varies between [-360°, 360°].
[0065] In the above formula, the integral function: calculates the change in rotation angle from the initial time to the current time by integrating the time t.
[0066] Complex summation function: The data from different sensors are combined through the summation function, while taking into account the impact of sensor calibration and Kalman gain on the data.
[0067] Noise filtering: Use Gaussian filter to filter the noise of angular velocity data to ensure the accuracy and stability of the data.
[0068] Exponential function: Use the exponential function to weight the angular velocity data to enhance the response to sudden changes in a short period of time.
[0069] Fractional structure: The effects of acceleration are combined with the effects of the integral of angular velocity through the fractional structure to form an accurate calculation of the estimated value of the rotation angle.
[0070] This formula achieves accurate estimation of the rotation angle of the rotary drilling rig by integrating multi-sensor data fusion and complex mathematical functions, while taking into account data noise filtering and sensor calibration issues to ensure the accuracy and robustness of the estimated value.
[0071] The target rotation angle setting module sets the target rotation angle of the rotary drilling rig according to operation requirements or preset operation plans, and generates the optimal control strategy from the current angle to the target angle.
[0072] In the target rotation angle setting module, the method for generating the optimal control strategy from the current angle to the target angle is: calculate the control signal according to the angle difference, assuming that the proportional control k p , then the control signal = k p ×Angle difference.
[0073] The electro-hydraulic proportional valve control module converts the generated control strategy into the control signal of the electro-hydraulic proportional valve, thereby accurately controlling the rotary action of the rotary drilling rig.
[0074] The optimal control strategy achieves precise speed control by controlling the motor current and voltage. It monitors the pressure changes of the hydraulic system to ensure the stability and response speed of the system. This control strategy dynamically adjusts the motor current and hydraulic system pressure, and updates the control signal in real time to ensure precise control of the rotary drilling rig's rotation angle.
[0075] In the electro-hydraulic proportional valve control module, after the generated optimal control strategy is converted into the control signal of the electro-hydraulic proportional valve, the central control unit accurately adjusts the motor and hydraulic system through the control signal to realize the rotation action of the rotary drilling rig; during the rotation process, the angle, acceleration and angular velocity data are monitored in real time, and then the control signal is continuously and dynamically adjusted in the above manner to ensure precise control of the rotation angle.
[0076] Example 2
[0077] This is the second embodiment of the present invention, which is different from the first embodiment in that: In order to verify the effect of the present invention, we designed a comparative experiment, the purpose of which is to evaluate the performance improvement of the system in terms of rotation angle control accuracy and response speed. The experimental equipment includes a rotary drilling rig, an angle sensor, an acceleration sensor, a gyroscope, a central control unit, an electro-hydraulic proportional valve, etc.
[0078] Step S1: Data collection
[0079] Angle sensors, acceleration sensors and gyroscopes are installed on the rotary drilling rig. The sensors are installed at the rotating part, the center of gravity and the rotating axis to ensure the accuracy and real-time nature of the data. After the sensor is installed, it is initialized and calibrated to ensure the measurement accuracy of the sensor.
[0080] Start the data acquisition system to collect angle, acceleration and angular velocity data in real time. The sensor data is transmitted to the central control unit via the wireless network.
[0081] Step S2: Data fusion and state estimation
[0082] The Kalman filter is used to fuse the data from the angle sensor, acceleration sensor, and gyroscope to generate a more accurate and robust rotation angle estimate. The fusion algorithm is calculated according to the following formula:
[0083]
[0084] The parameters in the formula are set as follows: N = 3 (number of sensors), M = 3 (number of angular velocity samples), λ j =0.1, α=0.01, dt=0.1 seconds. Through real-time calculation and updating, the central control unit obtains the current posture and motion state of the rotary drilling rig and generates accurate input data.
[0085] When time t = 1 second, the specific real-time calculation and update steps are as follows:
[0086] Assume that the data obtained from the sensor is as follows:
[0087] Angle sensor: measured angle z 1,1 =10°
[0088] Acceleration sensor: angular acceleration α1 = 0.48 rad / s 2
[0089] Gyroscope: angular velocity ω 1,1 =0.5rad / s
[0090] Calculate the fusion data at the current moment:
[0091] Set the weight h i (x) = 1 (assuming all sensors have the same weight)
[0092] Calculate the noise filter function g(x):
[0093]
[0094] Assumptions but:
[0095] Compute the angle estimate:
[0096] Assume k i ·P i,t ≈0, then:
[0097]
[0098] Current posture: The rotation angle of the rotary drilling rig is: 18.086 degrees;
[0099] Current motion state: angular velocity is 0.5rad / s, angular acceleration is 0.48rad / s 2 .
[0100] Step S3: Generate optimal control strategy
[0101] Set the target rotation angle of the rotary drilling rig according to operational requirements or preset work plans.
[0102] For example, the target rotation angle is set to 90 degrees. According to the above fusion algorithm formula, the current angle is calculated to be 18.086 degrees, and the difference is 71.914 degrees. According to the difference between the current angle and the target angle, the optimization algorithm is used to generate the best control strategy from the current angle to the target angle.
[0103] The steps to calculate the optimal control strategy are as follows:
[0104] Calculate the control signal based on the angle difference, assuming that proportional control k is used p :
[0105] Control signal = k p × Angle difference;
[0106] Set the proportional control coefficient k p =2, then the control signal at time t=1 is: control signal=0.02×71.914=1.4383v; at t=1 second, the control signal voltage after optimization calculation is 1.4383V.
[0107] The optimization algorithm comprehensively considers the data of the motor vector control system and the pressure sensor at the end of the propulsion shaft to ensure the optimality and efficiency of the control strategy;
[0108] Motor vector control system: realizes precise speed control by controlling the current and voltage of the motor;
[0109] Propulsion shaft end pressure sensor: monitors the pressure changes of the hydraulic system to ensure the stability and response speed of the system;
[0110] This control strategy dynamically adjusts the motor current and hydraulic system pressure, and updates the control signal in real time to ensure accurate control of the rotary drilling rig's rotation angle;
[0111] Step S4: Control signal output
[0112] The generated optimal control strategy is converted into the control signal of the electro-hydraulic proportional valve. The central control unit accurately adjusts the motor and hydraulic system through the control signal to realize the rotary action of the rotary drilling rig. During the rotation process, the angle, acceleration and angular velocity data are monitored in real time, and the control signal is dynamically adjusted to ensure the precise control of the rotation angle.
[0113] Table 1 is the experimental data record of this embodiment
[0114]
[0115] High-precision control:
[0116] Through multi-sensor data fusion and Kalman filter algorithm, the present invention can obtain accurate rotation angle estimation value in real time through the control strategy generated by the optimization algorithm, so that the system can be more stable during the rotation process, and the control error caused by noise and abnormal values is reduced. The data shows that at 1 second, the measured angle is 10 degrees, and the angle estimation value calculated by the fusion data is 18.086 degrees (refer to the calculation process at 1 second in Example 1), indicating that the system has significantly improved in real time and accuracy.
[0117] Quick response:
[0118] The control strategy generated by the optimization algorithm, combined with the rapid response capability of the electro-hydraulic proportional valve, enables the system to achieve precise control of the target angle in a relatively short time. Experimental data shows that at 1 second, the control signal voltage is 1.2V. Through optimization calculation, the generated control signal is 1.4383V, which further improves the response speed of the system compared to the original 1.2V.
[0119] Stability and robustness:
[0120] During the experiment, the stability and robustness of the rotation angle were ensured by dynamically adjusting the control signal. Even when the angular velocity and angular acceleration change greatly, the system can still maintain precise angle control, showing a high anti-interference ability.
[0121] It can be seen from the experimental data table that the measured angle of the automatic rotary positioning control system of the present invention at each time point gradually approaches the target angle, and achieves precise control from 0 degrees to 90 degrees within 6 seconds.
[0122] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.
Claims
1. An automatic rotary positioning control system for a rotary drilling rig, characterized in that: include: Angle sensor, acceleration sensor, gyroscope, central control unit, target rotation angle setting module and electro-hydraulic proportional valve control module; the angle sensor, acceleration sensor and gyroscope are used to collect the angle, acceleration and angular velocity data of the rotary drilling rig in real time, and transmit the data to the central control unit; the central control unit calculates the optimal rotation angle estimation value of the rotary drilling rig based on the data obtained from the angle sensor, acceleration sensor and gyroscope, and uses the data fusion result to estimate the current posture and motion state of the rotary drilling rig, and provides accurate input data for the control system; the target rotation angle setting module sets the target rotation angle of the rotary drilling rig according to the operation requirements or the preset operation plan, and generates the optimal control strategy from the current angle to the target angle; the electro-hydraulic proportional valve control module converts the generated control strategy into the control signal of the electro-hydraulic proportional valve, so as to accurately control the rotation action of the rotary drilling rig.
2. The automatic rotation positioning control system of a rotary drilling rig according to claim 1, characterized in that: The angle sensor, acceleration sensor and gyroscope are respectively installed on the rotating part, the center of gravity and the rotating shaft of the rotary drilling rig to ensure the accuracy and real-time performance of the data.
3. The automatic rotary positioning control system for a rotary drilling rig according to claim 2 is characterized in that: The sensor data is transmitted to the central control unit via a wireless network.
4. The automatic rotation positioning control system of a rotary drilling rig according to claim 3, characterized in that: The central control unit is used to filter the collected data, so that the system is more stable during actual operation.
5. The automatic rotation positioning control system of a rotary drilling rig according to claim 4, characterized in that: The central control unit uses a Kalman filter to fuse the data of the angle sensor, the acceleration sensor and the gyroscope to generate a more accurate and robust rotation angle estimation value; Settings: N represents the number of sensors. The sensor data fusion part calculates the overall calibration value and measurement error by summing the data of N sensors; M represents the number of samples of angular velocity data or the number of angular velocity sensors; θ t is the estimated value of the rotation angle at time t; α t is the angular acceleration at time t; ω t is the angular velocity at time t; The specific steps are as follows: S1: Determine the noise filter function of the weighted sum of angular velocities as g(x), which is in the form of: In the above formula, the noise filter function performs exponential weighted summation on different angular velocity data to ensure that the filtered angular velocity data is more accurate and stable; represents the exponential decay of the j-th angular velocity, with the weight being λ j Decide; λ j : Weight parameter, which controls the influence of different angular velocity data in noise filtering. j sets the serial number of the sample or angular velocity sensor used to index the angular velocity data. In the noise filtering part, by processing each angular velocity data sample or sensor data, combined with the weight parameter λ j Realize weighted average of different angular velocity data; S2: Determine the correction and weighted sum of the sensor measurement values; correct the measurement value of each sensor, and the corrected value is: z i,t -k i ·P i,t ; In the above formula, z i,t is the measurement value of the i-th sensor at time t; P i,t : is the covariance matrix, which is used to measure the distribution of the estimation error and is dynamically updated over time; k i : Kalman gain, which reflects the impact of the current measurement value on the estimated value and is dynamically adjusted through the Kalman filter algorithm; k i ·P i,t represents the calibration deviation of the i-th sensor; The modified values are then weighted summed: In the above formula, h i (x) is a sensor calibration function based on feature x, used to correct for inaccuracies in sensor readings, using a function based on linear or nonlinear regression; S3: Attenuation term of combined acceleration: Consider the acceleration term that decays over time: This formula is used to correct the angular velocity in combination with the time integral, taking into account the effect of acceleration decay over time; In this formula, α t ·e -αt It represents the acceleration that decays with time; It represents the integral of the angular velocity from time 0 to time t, representing the change in the rotation angle; α: represents the attenuation coefficient, which controls the degree to which the influence of acceleration on the estimated value decays over time; dt: represents a small time increment. During the integration process, dt is used to discretize time, convert the continuous time integral into the discrete time sum, and calculate the estimated value of the rotation angle from the initial time to the current time.
6. The automatic rotation positioning control system of a rotary drilling rig according to claim 5, characterized in that: The estimated value of the rotation angle from the initial time to the current time is calculated by time integration. The calculation formula is as follows: Among them, θ t : The final calculated rotation angle estimate is a dynamically changing quantity, and its value range depends on the range of the sensor measurement value and the working state of the system.
7. The automatic rotation positioning control system of a rotary drilling rig according to claim 6, characterized in that: In the target rotation angle setting module, the method for generating the optimal control strategy from the current angle to the target angle is: calculate the control signal according to the angle difference, assuming that the proportional control k p , then the control signal = k p ×Angle difference.
8. The automatic rotation positioning control system of a rotary drilling rig according to claim 7, characterized in that: The optimal control strategy achieves precise speed control by controlling the motor current and voltage. It monitors the pressure changes of the hydraulic system to ensure the stability and response speed of the system. This control strategy dynamically adjusts the motor current and hydraulic system pressure, and updates the control signal in real time to ensure precise control of the rotary drilling rig's rotation angle.
9. The automatic rotation positioning control system of a rotary drilling rig according to claim 8, characterized in that: In the electro-hydraulic proportional valve control module, after the generated optimal control strategy is converted into the control signal of the electro-hydraulic proportional valve, the central control unit accurately adjusts the motor and hydraulic system through the control signal to realize the rotation action of the rotary drilling rig; during the rotation process, the angle, acceleration and angular velocity data are monitored in real time, and then the control signal is continuously and dynamically adjusted in the above manner to ensure precise control of the rotation angle.
10. The automatic rotation positioning control system of a rotary drilling rig according to claim 9, characterized in that: The posture of the rotary drilling rig indicates the rotation angle of the rotary drilling rig, and the motion state is specifically the angular velocity and angular acceleration.
Citation Information
Cited By
Milling drill equipment, control method and robot
CN119498924A
