Middle vertical drilling mechanism and using method

Abstract

The invention discloses a centrally-mounted vertical drilling mechanism and a using method, and relates to the technical field of drilling mechanisms, the centrally-mounted vertical drilling mechanism comprises a lower cover plate, a front cover is arranged at the top of the lower cover plate close to the left side, a rear cover is arranged at the top of the lower cover plate close to the right side, antennas are arranged at the tops of the front cover and the rear cover, and a chassis is arranged at the bottom of the lower cover plate; crawlers are arranged on the front side and the rear side of the chassis, a plurality of thrust wheels are arranged at the positions, close to the center, of the front side and the rear side of the chassis, and real-time vertical control (+ / -0.5-degree precision) of a drill bit on a dynamic sloping field is achieved through a sensing-decision-execution closed-loop framework based on a multi-sensor fusion active vertical control system. Compared with the prior art, the scheme solves the three bottlenecks of insufficient manual operation precision (+ / -10 degrees), mechanical adjustment delay (gt, 3 seconds) and open-loop control failure in a breakthrough manner.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of drilling mechanism, in particular to a middle-placed vertical drilling mechanism and a use method thereof. BACKGROUND

[0002] At present, the drilling operation in mountain orchard mainly relies on three types of machines, namely hand-held, towed and suspended machines. The towed and suspended drilling machines are only suitable for flat terrain because of their large structure and dependence on the power output of the towing vehicle, and cannot enter the hilly area with a slope of more than 15°. Although the hand-held drilling machine can adapt to complex terrain, the feeding of the drill bit completely depends on manual operation: the operator needs to hold the equipment weighing more than 20 kg and manually maintain the verticality of the drill bit on the slope. This operation mode has the following defects: 1. Posture control failure: when working on a slope, the drill bit will naturally deviate due to the influence of gravity component. It is difficult to compensate for the angle deviation in real time by manual adjustment according to experience, resulting in a 10° to 15° inclination between the actual drilling axis and the vertical direction (measured data on site). For example, when working on a 20° slope, the horizontal deviation at the bottom of the hole can reach 18% of the hole depth (calculated deviation > 14 cm based on standard 80 cm hole depth), which seriously damages the upright requirement of fruit tree planting, leading to the growth of the fruit tree being tilted and the survival rate being reduced.

[0003] 2. Safety and precision defects: holding a vibrating device (vibration frequency 8-15 Hz) by hand is prone to cause muscle strain of the operator, and there is a risk of slipping on a slippery slope. More importantly, the existing technology lacks real-time posture feedback mechanism. When the drill bit encounters underground stones or other foreign objects, the impact force will further amplify the angle deviation, and the operator cannot correct it effectively within the human reaction time of 0.5 seconds.

[0004] In view of the above problems, some high-end agricultural machines try to use a mechanical gravity pendulum structure for passive vertical adjustment, which makes the drill bit tend to the direction of gravity by using a counterweight. However, this scheme has the following fundamental limitations: 1. Insufficient adjustment accuracy: the pendulum mechanism is affected by inertia and produces a delay in swinging, and the actual adjustment accuracy is only ±8°, which cannot meet the ±3° accuracy required for fruit tree planting; 2. Lack of dynamic response: when working continuously on a slope (such as a sudden change from a 20° slope to a 30° slope), the mechanical structure needs 3-5 seconds to rebalance, which is far lower than the response requirement of terrain change; 3. Weak anti-interference ability: sudden change of drill cutting resistance (>200 N·m) will cause the pendulum to oscillate, further amplifying the angle error. SUMMARY

[0005] In view of the above shortcomings of the prior art, the present application aims to provide a middle-placed vertical drilling mechanism and a use method thereof.

[0006] In order to achieve the above object, the present application provides the following technical scheme: the middle vertical drilling mechanism, including the lower cover plate, the top of the lower cover plate is provided with the front cover near the left side, the top of the lower cover plate is provided with the rear cover near the right side, the top of the front cover and the rear cover is provided with the antenna, the bottom of the lower cover plate is provided with the chassis, the front and rear sides of the chassis are provided with the track, the center of the front and rear sides of the chassis is provided with a plurality of supporting wheels, the top of the supporting wheels is provided with the supporting wheel, the top of the chassis is provided with the tensioning device near the front and rear sides.

[0007] Preferably, the top of the chassis is provided with two rear bridges near the right side, the rear bridge is provided with the rear bridge pipe on one side, the rear bridge pipe is provided with the belt disc half shaft, and the belt disc half shaft is provided with the driving disc on one side.

[0008] Preferably, the top of the lower cover plate is provided with the opening in the center, the right side wall of the opening is fixedly connected with the gantry, the gantry is sleeved with the side plate, the left side of the side plate is provided with the back plate, the right side of the side plate is provided with the transmission motor set, the power output shaft of the transmission motor set is fixedly connected with the adjacent sprocket on one side.

[0009] Preferably, the back plate is provided with a plurality of fixed pulleys, the fixed pulleys and the two sprockets are connected with the chain, and the chain is provided with the tripod.

[0010] Preferably, the top of the tripod is provided with the motor, the top of the motor is provided with the engine top cover, the power output shaft of the motor is fixedly connected with the drill bit, and the bottom end of the drill bit penetrates the tripod and the opening.

[0011] The use method of the middle vertical drilling mechanism, comprising the following steps: S1: system initialization: after the STM32 is started, the Kalman filter parameters are loaded, and the hydraulic cylinder is zeroed; S2: real-time attitude sensing: the MPU6050 outputs a group of 6-axis data every 5ms, and the pitch angle θ_pitch and the roll angle θ_roll are obtained after Kalman filtering; S3: verticality decision: the deviation of the current attitude and the target attitude (0, 0, -g) is calculated: pitch_error=θ_pitch-0 roll_error=θ_roll-0; S4: hydraulic execution: when |pitch_error|>0.3° or |roll_error|>0.3°, the control is triggered: PWM duty ratio=PID_Compute(error) The opening degree of the electro-hydraulic proportional valve is proportional to the PWM duty ratio. S5: Dynamic correction verification: when encountering a 30° slope mutation during drilling: the system detects the angle deviation (θ_pitch=2.5°) within 80ms, and the PID output is 72% PWM driving the cylinder to extend; Actual correction time: 120ms, final verticality error 0.4°.

[0012] Compared with the prior art, the present application has the beneficial effects that: The present application is based on a multi-sensor fusion active vertical control system, which realizes real-time vertical control of the drill bit on dynamic slope land (±0.5° precision) through a "perception-decision-execution" closed-loop architecture. Compared with the prior art, the present solution has broken through the three major bottlenecks of insufficient manual operation precision (±10°), mechanical adjustment delay (>3 seconds) and open-loop control failure; MPU6050 six-axis sensor (accelerometer + gyroscope) is used to collect the original attitude data of the drill bit at a frequency of 200Hz. Based on the Kalman filter algorithm (process noise Q=diag([0.001,0.003]), observation noise R=0.03), the acceleration and angular velocity information is fused to eliminate vibration interference and gyro drift. The quaternion differential equation is used to solve the Euler angle in real time, and the attitude sensing precision of ±0.05° is realized under the condition of 35° slope and 15Hz mechanical vibration; The response time of the hydraulic cylinder is ≤100ms, and the thrust control resolution is 0.1kN; An improved PID with integral limiting is realized in STM32, small error range (|θ_error|<1°): Kp=12.0, Ki=0.8, Kd=4.5; large error range (|θ_error|≥1°): Kp=18.0, Ki=0.5, Kd=6.0; 35° slope step response time 0.12s, steady-state error <±0.5°; The synchronous adjustment range of the two degrees of freedom reaches ±5°, which is improved by 66% compared with the single degree of freedom scheme; Through the four-level collaborative architecture of "Kalman filter perception → anti-saturation PID decision → hydraulic differential execution → orthogonal mechanical decoupling", the engineering application of dynamic vertical control of slope drilling (±0.5° precision / 0.1s response) is realized for the first time, which fundamentally solves the verticality problem of fruit tree planting hole. BRIEF DESCRIPTION OF DRAWINGS

[0013] Figure 1 is a front view of the present application; Figure 2 is a perspective view of the present application; Figure 3 is a bottom perspective view of the present application; Figure 4 is an exploded view of the present application; Figure 5The component drill bit front view of the present application; Figure 6 The component antenna front view of the present application; Figure 7 The component belt disc half shaft front view of the present application; Figure 8 The component chassis structure schematic view of the present application; Figure 9 The component rear axle bridge tube front view of the present application; Figure 10 The component rear axle explosion view of the present application; Figure 11 The overall architecture block diagram of the present application.

[0014] Marked figure: 1, lower cover plate; 2, antenna; 3, tripod; 4, front cover; 5, track; 6, supporting wheel; 7, belt roller; 8, driving disc; 9, motor; 10, engine top cover; 11, transmission motor set; 12, gantry; 13, chassis; 14, drill bit; 15, back plate; 16, side plate; 17, fixed pulley; 18, sprocket; 19, rear cover; 20, rear axle; 21, chain; 22, tensioning device; 23, belt disc half shaft; 24, rear axle bridge tube. DETAILED DESCRIPTION

[0015] Please refer to Figures 1-11 The present application provides a technical solution: middle vertical drilling mechanism, including lower cover plate 1, lower cover plate 1 top near left side is equipped with front cover 4, lower cover plate 1 top near right side is equipped with rear cover 19, front cover 4 and rear cover 19 top are equipped with antenna 2, lower cover plate 1 bottom is equipped with chassis 13, chassis 13 front and rear sides are equipped with track 5, chassis 13 front and rear sides near the center are equipped with a plurality of supporting wheels 6, a plurality of supporting wheels 6 top are commonly equipped with belt roller 7, chassis 13 top near the front and rear sides are equipped with tensioning device 22, chassis 13 top near the right side is equipped with two rear axles 20, rear axles 20 sides are equipped with rear axle bridge tubes 24, rear axle bridge tubes 24 inner cavities are equipped with belt disc half shafts 23, belt disc half shafts 23 sides are equipped with driving discs 8; Lower cover plate 1 top center is equipped with an opening, the right side wall of the opening is fixedly connected with gantry 12, gantry 12 is sleeved with side plate 16, side plate 16 left side is equipped with back plate 15, side plate 16 right side is equipped with transmission motor set 11, side plate 16 and back plate 15 are equipped with sprocket 18, transmission motor set 11 power output shaft is fixedly connected with adjacent sprocket 18 side, back plate 15 is equipped with a plurality of fixed pulleys 17, a plurality of fixed pulleys 17 and two sprockets 18 are commonly connected with chain 21, chain 21 is equipped with tripod 3, tripod 3 top is equipped with motor 9, motor 9 top is equipped with engine top cover 10, motor 9 power output shaft is fixedly connected with drill bit 14, drill bit 14 bottom end penetrates tripod 3 and opening; I. Core control logic and algorithm: 1. Data flow architecture: MPU6050 → I²C raw data → Kalman filter → quaternion attitude solution →

[0016] 2. Key algorithm implementation: Kalman filter: fuse accelerometer and gyroscope data, eliminate vibration noise (Q_angle=0.001, R_measure=0.03) Quaternion solution: use gradient descent method to iteratively update, avoid Euler angle gimbal lock problem:

[0017] Anti-saturation PID: with integral limit and differential lead (Kp=12.0, Ki=0.8, Kd=4.5)

[0018] 3. Execution control logic: - Trigger hydraulic correction when |θ_error|>0.3° - Pitch control: forward tilt → shrink front cylinder / extend rear cylinder (PWM duty cycle ∝ PID output) - Roll control: left tilt → shrink left cylinder / extend right cylinder - Dynamic response: complete 35° slope ±0.5° accuracy correction within 120ms Innovative synergy mechanism: The orthogonally arranged back plate 15 and side plate 16 are mechanically decoupled by inserting the pin shaft, allowing the STM32 to independently control the pitch / roll degrees of freedom; the 200Hz high-frequency sampling of MPU6050 is filtered by Kalman filter, and forms a closed loop with the hydraulic system with 15ms response, breaking through the mechanical delay bottleneck of traditional solutions, achieving ±0.5° dynamic vertical control on slope conditions (precision improved by 16 times); II. Key circuit implementation: 1. Sensor acquisition circuit MPU6050 interface design: 4-layer PCB board design, sensor powered separately (3.3V LDO voltage regulator), connected with STM32 through I²C bus (SCL / SDA pins with 10kΩ pull-up resistor); Accelerometer range ±16g (resolution 0.0488mG / LSB), gyroscope range ±2000° / s (resolution 16.4LSB / ° / s); Configure data output rate 200Hz, trigger STM32 to read data through interrupt pin (INT); Anti-interference design: the bottom of the sensor is filled with heat-conducting silica gel and covered with an electromagnetic shield (0.2mm copper foil), I²C wiring is treated as ground, length ≤5cm, to avoid parallel with motor drive circuit; 2. STM32 master control circuit / / STM32 pin configuration core code snippet voidGPIO_Config(){ / / MPU6050 interface I2C1_SCL_Pin = PB6; I2C1_SDA_Pin = PB7; / / Hydraulic valve PWM output (TIM3 channels 1-4) PWM_Front_Pin = PA6; PWM_Rear_Pin = PA7; PWM_Left_Pin = PB0; PWM_Right_Pin = PB1; / / Serial debugging interface USART1_TX_Pin = PA9; USART1_RX_Pin = PA10;} 3. Hydraulic drive circuit L298N drive part: input voltage 24VDC (matching hydraulic valve working voltage), PWM control signal after optical coupling isolation (TLP521-4) to drive MOSFET; Electro-hydraulic proportional valve parameters: Rated flow: 5L / min @ 10MPa Response time: ≤15ms PWM control curve scheme:

[0019] Where

[0020] III. Hydraulic actuator mechanical model:

[0021] Where: F required : Required force (N) m: Drill mass (kg) g: Acceleration of gravity (9.81 / s 2 ) θ error : Angle error (rad) I: Moment of inertia (kg / m 2 ) α: Angular acceleration (rad / s 2 ) IV. Software architecture flowchart: Start -> Sensor data collection -> Kalman filter -> Attitude solution -> Bias calculation -> PID control -> PWM output -> Hydraulic execution -> Verticality verification 1. The use of Kalman filter algorithm in this structure (code): / / Define state vector [theta, theta_bias]^T float Q_angle = 0.001; / / Process noise covariance float Q_gyroBias = 0.003; float R_measure = 0.03; / / Observation noise covariance void Kalman_Filter(float accel[3], float gyro[3]) { 1) State prediction angle += (gyro[0] - gyroBias) * dt; P[0][0] += dt * (P[1][1] * dt - P[0][1] - P[1][0] + Q_angle); P[0][1] -= dt * P[1][1]; P[1][0] -= dt * P[1][1]; P[1][1] += Q_gyroBias * dt; 2) Calculate Kalman gain float S = P[0][0] + R_measure; float K[2] = {P[0][0] / S, P[1][0] / S}; 3) State update float y = accel[0] - angle; / / Accelerometer observation value angle += K[0] * y; gyroBias += K[1] * y; 4) Covariance update P[0][0] -= K[0] * P[0][0]; P[0][1] -= K[0] * P[0][1]; P[1][0] -= K[1] * P[0][0]; P[1][1] -= K[1] * P[0][1]; } 2. The use of attitude solution and bias control in this structure (code): / / Quaternion method to solve Euler angles (avoid gimbal lock) void Quaternion_Update(float ax, float ay, float az, float gx, float gy, float gz) { float q0 = 1, q1 = 0, q2 = 0, q3 = 0; / / initialize quaternion float norm = sqrt(ax * ax + ay * ay + az * az); ax / = norm; ay / = norm; az / = norm; / / Gradient descent iteration float vx = 2 * (q1 * q3 - q0 * q2) - ax; float vy = 2 * (q0 * q1 + q2 * q3) - ay; float vz = 2 * (0.5 - q1 * q1 - q2 * q2) - az; / / Quaternion differential equations q0 += (-q1 * gx - q2 * gy - q3 * gz) * 0.5 * dt; q1 += (q0 * gx + q2 * gz - q3 * gy) * 0.5 * dt; q2 += (q0 * gy - q1 * gz + q3 * gx) * 0.5 * dt; q3 += (q0 * gz + q1 * gy - q2 * gx) * 0.5 * dt; / / Euler angle output roll = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)); pitch = asin(2 * (q0 * q2 - q3 * q1)); } 3. PID controller design in this structure (code): / / Improved anti-windup PID (with derivative lead) typedef struct { float Kp, Ki, Kd; float integral, prev_error; float out_max, out_min; } PID_Controller; float PID_Compute(PID_Controller *pid, float error) { / / P term floatP_out = pid->Kp * error; / / I term (anti-windup handling) pid->integral += error; if (pid->integral > pid->out_max) pid->integral = pid->out_max; else if (pid->integral < pid->out_min) pid->integral = pid->out_min; <pid->pid->integral = pid->out_min; float I_out = pid->Ki * pid->integral; / / D term (derivative lead) float D_out = pid->Kd * (error - pid->prev_error); pid->prev_error = error; / / output limit float output = P_out + I_out + D_out; if (output > pid->out_max) output = pid->out_max; elseif (output < pid->out_min) output = pid->out_min; <pid->out_min) output = pid->out_min; return output; } / / Controller parameters (optimized for hydraulic system) PID_Controller pitch_pid = { 12.0, 0.8, 4.5, 0, 0, 100, -100}; PID_Controller roll_pid = { 11.5, 0.75, 4.2, 0, 0, 100, -100}; 4. Use of actuator control logic within this structure (code): void Hydraulic_Control(float pitch_error, float roll_error) { / / Pitch control (front / rear cylinders) float pitch_output = PID_Compute(&pitch_pid, pitch_error); if (pitch_output > 0) { / / Correct for pitch forward set_PWM(PWM_Front, 0); / / Front cylinder retracted set_PWM(PWM_Rear, abs(pitch_output)); / / Rear cylinder extended } else { / / Correct for pitch backward set_PWM(PWM_Front, abs(pitch_output)); set_PWM(PWM_Rear, 0); } / / Roll control (left / right cylinders) float roll_output = PID_Compute(&roll_pid, roll_error); if (roll_output > 0) { / / Correct for roll left set_PWM(PWM_Left, 0); set_PWM(PWM_Right, abs(roll_output)); } else { / / Correct for roll right set_PWM(PWM_Left, abs(roll_output)); set_PWM(PWM_Right, 0); } } Perpendicularity error ≤ ± 0.5° (20 times better than manual operation ± 10°, 16 times better than mechanical ± 8°), bottom horizontal offset ≤ 7mm when drilling depth is 80cm; maximum response slope is 35°, 90% step response time is 0.12s, vibration suppression capability is > 15Hz; hydraulic system only starts when deviation > 0.3°, average power consumption < 200W (1.6kWh for 8 hours of continuous operation), 40% energy saving compared with electric push rod scheme, Kalman filter makes the misoperation rate < 0.1% under vibration interference, and the hinge mechanism life > 500,000 times of reciprocating motion.

[0022] The use method of the middle vertical drilling mechanism, comprising the following steps: S1: system initialization: after the STM32 is started, the Kalman filter parameters are loaded, and the hydraulic cylinder is zeroed; S2: real-time attitude sensing: MPU6050 outputs a set of 6-axis data every 5ms, and the pitch angle θ_pitch and the roll angle θ_roll are obtained after Kalman filtering; S3: perpendicularity decision: calculate the deviation of the current attitude and the target attitude (0, 0, -g): pitch_error=θ_pitch-0 roll_error=θ_roll-0; S4: hydraulic execution: trigger control when |pitch_error|> 0.3° or |roll_error|> 0.3°: PWM duty cycle=PID_Compute(error) Electro-hydraulic proportional valve opening ∝ PWM duty cycle; S5: dynamic correction verification: when a 30° slope mutation is encountered during drilling: the system detects the angle deviation (θ_pitch=2.5°) within 80ms, and the cylinder is extended after the PID output 72% PWM drive; Actual correction time: 120ms, final perpendicularity error 0.4°.

Claims

1. A mid-set vertical drilling mechanism comprising a lower cover plate (1), characterized in that: The lower cover plate (1) top near the left side is equipped with a front cover (4), the lower cover plate (1) top near the right side is equipped with a rear cover (19), the front cover (4) and the rear cover (19) top are equipped with an antenna (2), the lower cover plate (1) bottom is equipped with a chassis (13), the chassis (13) front and rear sides are equipped with a track (5), the chassis (13) front and rear sides near the center are equipped with a plurality of supporting wheels (6), the plurality of supporting wheels (6) top are commonly equipped with a supporting wheel (7), the chassis (13) top near the front and rear sides is equipped with a tensioning device (22).

2. The mid-mount vertical boring mechanism of claim 1, wherein: The chassis (13) top near the right side is equipped with two rear bridges (20), the rear bridges (20) one side are equipped with rear bridge pipes (24), the rear bridge pipes (24) inner cavities are equipped with belt disc half shafts (23), the belt disc half shafts (23) one side are equipped with driving discs (8).

3. The mid-mount vertical boring mechanism of claim 2, wherein: The lower cover plate (1) top center is equipped with an opening, the opening right side wall is fixedly connected with a portal frame (12), the portal frame (12) is sleeved with a side plate (16), the side plate (16) left side is equipped with a back plate (15), the side plate (16) right side is equipped with a transmission motor set (11), the side plate (16) and the back plate (15) are equipped with sprockets (18), the transmission motor set (11) power output shaft is fixedly connected with one side of the adjacent sprocket (18).

4. The mid-mount vertical boring mechanism of claim 3, wherein: The back plate (15) is equipped with a plurality of fixed pulleys (17), the plurality of fixed pulleys (17) and the two sprockets (18) are commonly connected with a chain (21), the chain (21) is equipped with a tripod (3).

5. The mid-mount vertical boring mechanism of claim 4, wherein: The tripod (3) top is equipped with a motor (9), the motor (9) top is equipped with an engine top cover (10), the motor (9) power output shaft is fixedly connected with a drill bit (14), the drill bit (14) bottom end penetrates the tripod (3) and the opening.

6. The method of using a midset vertical bore mechanism according to any one of claims 1-5, wherein, Comprise the following steps: S1: system initialization: after the STM32 starts, load the Kalman filter parameters, and zero the hydraulic cylinder; S2: real-time attitude sensing: MPU6050 outputs a set of 6-axis data every 5ms, and obtains the pitch angle θ_pitch and the roll angle θ_roll after Kalman filtering; S3: verticality decision: calculate the deviation of the current attitude and the target attitude (0, 0, -g): pitch_error=θ_pitch-0 roll_error=θ_roll-0; S4: hydraulic execution: when |pitch_error|>0.3° or |roll_error|>0.3°, trigger control: PWM duty cycle=PID_Compute(error) Electro-hydraulic proportional valve opening∝PWM duty cycle; S5: dynamic correction verification: when a 30° slope mutation is encountered during drilling: the system detects the angle deviation (θ_pitch=2.5°) within 80ms, and the PID output is 72% PWM driving after the cylinder is stretched out; Actual correction time: 120ms, final verticality error 0.4°.

Images