Tobacco transplanter attitude control method

Abstract

The invention provides a posture control method for a tobacco transplanter. The posture control method comprises a heading posture control method and a vehicle body horizontal posture control method. The course attitude control method comprises the following steps: (1) setting a target course angle of the tobacco transplanter; (2) an angle sensor collects a current angle; (3) calculating an error and a control quantity increment; and (4) changing the wheel speed to realize angle adjustment. The vehicle body horizontal attitude control method comprises the following steps: (1) designing the leveling mechanism; (2) setting a target pitch angle and a target roll angle of the tobacco transplanter; (3) an angle sensor detects a current angle; (4) adjusting the transverse levelness; and (5) adjusting the longitudinal levelness. The invention aims to realize automatic posture control of the tobacco transplanter and improve the working efficiency of the tobacco transplanter.

Description

Technical Field

[0001] The present invention relates to a method for attitude control of a tobacco transplanter, belonging to the field of agricultural machinery intelligence. Technical Background

[0002] Tobacco transplanting is one of the key links in tobacco production. The straight driving of the transplanter helps to improve the quality of tobacco transplanting and the operation specification. However, since most tobacco producing areas are concentrated in hilly and mountainous areas, mechanized operation is difficult, there is a lack of small self-propelled models, and the course adjustment mainly relies on manual labor, resulting in high labor intensity and low intelligence level. At the same time, in hilly and mountainous areas, the climate is humid and the soil is wet. When the transplanter is operating, the wheels are prone to sink, causing the body plane to tilt. If the tilt degree is too large, it will affect the quality of transplanting and digging holes and reduce the uprightness of tobacco seedlings. In order to improve the operation efficiency, accuracy and stability of the transplanter and ensure the maximum growth environment of tobacco seedlings, it is necessary to design a method that can automatically control the course attitude and body horizontal attitude of the tobacco transplanter. Summary of the Invention

[0003] Based on the above background, in order to improve the quality of tobacco transplanting and reduce the labor cost, the present invention provides a method for attitude control of a tobacco transplanter, including course attitude control and body horizontal attitude control. The hardware used includes a single-chip microcomputer, an angle sensor, an encoder, a motor drive board, a motor and an electric rod.

[0004] Course attitude control and body horizontal attitude control. The specific steps of course attitude control are as follows:

[0005] (a1) Set the target course angle θ of the tobacco transplanter target ;

[0006] (a2) The angle sensor collects the current course angle θ of the tobacco transplanter current ;

[0007] (a3) Calculate the error err k =θ target -θ current ; Calculate the control quantity increment:

[0008] Δμ = k p (err k -err k-1 ) + k i err k +k a (err k -2err k-1 +err k-2 );

[0009] (a4) When Δμ < 0, change the speed of the right wheel of the tobacco transplanter, increase the speed value by |Δμ|, and keep the speed of the left wheel unchanged until θ target = θ current ; when Δμ > 0, change the speed of the left wheel of the tobacco transplanter, increase the speed value by Δμ, and keep the speed of the right wheel unchanged until θ target = θ current ;

[0010] The specific steps for controlling the horizontal attitude of the vehicle body are as follows:

[0011] (b1) Design a leveling mechanism. Electric rods are installed in the middle of the cross beams on the four sides of the wheels and fixedly connected to the roof plane. The transplanting mechanism is below the roof plane; the IO port of the single-chip microcomputer is connected to the motor drive board of the electric rod, and the motor drive board of the electric rod is connected to the electric rod motor;

[0012] (b2) Set the target pitch angle α target , α target = 0; Set the target roll angle β target , β target = 0;

[0013] (b3) The angle sensor collects the current pitch angle α current of the tobacco transplanter and the current roll angle β current ;

[0014] (b4) First, adjust the horizontal direction. When α current < 0, the single-chip microcomputer controls the left electric rod to rise, the IO port outputs a high level, and the motor rotates forward; when α current > 0, the single-chip microcomputer controls the right electric rod to rise, the IO port outputs a high level, and the motor rotates forward;

[0015] (b5) When the horizontal adjustment is made so that the vehicle body is level left and right, the electric rod motors on both sides stop operating, and the longitudinal level is adjusted. When β current < 0, the single-chip microcomputer controls the front electric rod to rise, the IO port outputs a high level, and the motor rotates forward; when β current > 0, the single-chip microcomputer controls the rear electric rod to rise, the IO port outputs a high level, and the motor rotates forward. After the longitudinal adjustment makes the vehicle body level front and back, the electric rods on both the front and back sides stop operating, and the leveling is completed.

[0016] The beneficial effects of the present invention are as follows:

[0017] ① The present invention provides a method for controlling the attitude of a tobacco transplanter, which is suitable for automatic operation of tobacco transplanting in hilly and mountainous areas;

[0018] ② By means of attitude angle adjustment, the stability of the heading driving of the transplanter and the levelness of the vehicle body are ensured, and the quality of tobacco transplanting is improved. Brief Description of the Drawings

[0019] Figure 1 is the overall framework diagram of the present invention;

[0020] Figure 2 is the flowchart of the vehicle body horizontal attitude control method of the present invention. Detailed Description of the Invention

[0021] The present invention will be further described in detail below in conjunction with the drawings and the detailed description of the invention.

[0022] The present invention provides a method for controlling the attitude of a tobacco transplanter, including a heading attitude control method and a vehicle body horizontal attitude control method, as Figure 1 shown. The hardware used includes an STM32F407VET6 single-chip microcomputer, an MPU6050 and an HWT101CT angle sensor, an encoder, a motor driver board, a motor, and an electric rod.

[0023] For the heading attitude control and the vehicle body horizontal attitude control, the specific steps of the heading attitude control are as follows:

[0024] (a1) Set the target heading angle θ of the tobacco transplanter target ;

[0025] (a2) The angle sensor collects the current heading angle θ of the tobacco transplanter current ;

[0026] (a3) Calculate the error err k = θ target - θ current ; Calculate the control quantity increment:

[0027] Δμ = k p (err k - err k-1 ) + k i err k + k d (err k - 2err k-1 + err k-2 );

[0028] (a4) When Δμ < 0, change the speed of the right wheel of the tobacco transplanter, the speed value increases by |Δμ|, and the speed of the left wheel remains unchanged until θ target = θ current ; When Δμ > 0, change the speed of the left wheel of the tobacco transplanter, the speed value increases by Δμ, and the speed of the right wheel remains unchanged until θ target = θ current ;

[0029] The specific steps for controlling the body's horizontal attitude are as follows:

[0030] (b1) Design a leveling mechanism. Electric rods are installed in the middle of the crossbeams on the four sides of the wheels and fixedly connected to the roof plane. Below the roof plane is a transplanting mechanism; the IO port of the single-chip microcomputer is connected to the motor drive board of the electric rod, and the motor drive board of the electric rod is connected to the electric rod motor;

[0031] (b2) Set the target pitch angle α of the tobacco transplanter target , α target = 0; Set the target roll angle β of the tobacco transplanter target , β target = 0;

[0032] (b3) The angle sensor collects the current pitch angle α of the tobacco transplanter current and the current roll angle β current ;

[0033] (b4) First, adjust the horizontal direction. When α current < 0, the single-chip microcomputer controls the left electric rod to rise, the IO port outputs a high level, and the motor rotates forward; when α current > 0, the single-chip microcomputer controls the right electric rod to rise, the IO port outputs a high level, and the motor rotates forward;

[0034] (b5) When the horizontal adjustment is completed and the left and right sides of the body are level, the motors of the left and right electric rods stop operating, and then adjust the longitudinal level. When β currrent < 0, the single-chip microcomputer controls the front electric rod to rise, the IO port outputs a high level, and the motor rotates forward; when β currrent > 0, the single-chip microcomputer controls the rear electric rod to rise, the IO port outputs a high level, and the motor rotates forward. After the longitudinal adjustment is completed and the front and rear sides of the body are level, the front and rear electric rods stop operating, and the leveling is completed. As Figure 2 shown

[0035] The method for increasing the speed value by Δμ in step (a4) is as follows:

[0036] (a401) The IO port of the single-chip microcomputer is connected to the motor drive board and the encoder. The motor drive board is connected to the wheel motor, and the encoder is installed on the motor shaft;

[0037] (a402) Convert the target speed value into the target number of pulses per second;

[0038] (a403) Enable the single-chip microcomputer timer 3 for PWM output and timer 2 for encoder counting;

[0039] (a404) The encoder measures the current number of pulses per second of the motor;

[0040] (a405) Compare the current number of pulses with the target number of pulses and increase the set value of the PWM duty cycle of the single-chip microcomputer;

[0041] (a406) Substitute the set value of the PWM duty cycle at this time into the CCRx register in Timer 3 of the single-chip microcomputer to generate a PWM output, thereby controlling the level output of the IO port connected to the motor driver board by the single-chip microcomputer, and further controlling the rotation of the motor to achieve an increase in speed.

[0042] During the process of the angle sensor collecting angle data in step (b3), the Kalman filter algorithm is used. The steps are as follows:

[0043] (b301) Establish the state transition equation:

[0044] x k+1 = Ax k + Bu k + w k ;

[0045] where x k+1 represents the state at time k + 1, A is the state transition matrix, used to represent the conversion relationship of the system from time k to time k + 1; B is the control matrix; u k is the control input; w k is the process noise;

[0046] (b302) Define the measurement model:

[0047] Z k = Hx k + v k ;

[0048] Z k is the measurement value; H is the observation matrix, indicating how to obtain the observation value from the system state; v k is the measurement noise;

[0049] (b303) Prediction step:

[0050] At each time k, predict the state at the next time according to the current state and control input:

[0051]

[0052] Predicted covariance:

[0053] P k|k-1 = A · P k-1|k-1 · A T + Q

[0054] where Q is the covariance matrix of the process noise;

[0055] (b304) Update step:

[0056] Update the state estimate based on new observation data and calculate the Kalman gain:

[0057] K k = P k-1|k-1 · H T · (H · P k|k-1 · H T + R) -1 ;

[0058] where R is the covariance matrix of the measurement noise;

[0059] Update the estimated state:

[0060]

[0061] Update the covariance:

[0062] P k|k = (I - K k · H) · P k|k-1 ;

[0063] where I is the identity matrix.

[0064] The software part of this process uses the embedded real-time operating system FreeRTOS, and the specific implementation steps are as follows:

[0065] (1) Port the framework of FreeRTOS to the microcontroller;

[0066] (2) Define each task to be executed and the priorities assigned to the tasks;

[0067] (3) Define the queues and semaphores for communication between tasks;

[0068] (4) Hand over the control of the microcontroller to FreeRTOS. When FreeRTOS executes, it will first initialize various hardware, delete this task after the initialization task ends, start executing the main task, define the main task as the body horizontal attitude adjustment, and then execute the secondary task, define the secondary task as the heading attitude adjustment.

Claims

1. A posture control method for a tobacco transplanter, characterized in that, It includes the following steps: heading attitude control and vehicle body horizontal attitude control. The specific steps of heading attitude control are as follows: (a1) Set the target heading angle θ of the tobacco transplanter target ; (a2) The angle sensor collects the current heading angle θ of the tobacco transplanter current ; (a3) Calculate the error err k = θ target - θ current ; Calculate the control quantity increment: Δμ = k p (err k -err k-1 ) + k i err k + k d (err k -2err k-1 + err k-2 ) When Δμ < 0, change the speed of the right wheel of the tobacco transplanter, and increase the speed value by |Δμ| while keeping the speed of the left wheel unchanged until α target = λ current ; when Δμ > 0, change the speed of the left wheel of the tobacco transplanter, increase the speed value by Δμ, and keep the speed of the right wheel unchanged until α target = θ current ; The specific steps of vehicle body horizontal attitude control are: (b1) Design a leveling mechanism. An electric rod is installed in the middle of the cross beams on the four sides of the wheels and fixedly connected to the roof plane. Below the roof plane is a transplanting mechanism. The IO port of the single-chip microcomputer is connected to the motor drive board of the electric rod, and the motor drive board of the electric rod is connected to the electric rod motor; (b2) Set the target pitch angle α of the tobacco transplanter target , α target = 0; Set the target roll angle β of the tobacco transplanter target , β target = 0; (b3) The angle sensor collects the current pitch angle α of the tobacco transplanter current and the current roll angle β current ; (b4) First, adjust the horizontal level. When α current < 0, the single-chip microcomputer controls the left electric rod to rise, the IO port outputs a high level, and the motor rotates forward. When α current > 0, the single-chip microcomputer controls the right electric rod to rise, the IO port outputs a high level, and the motor rotates forward; (b5) After the horizontal adjustment is made so that the left and right sides of the vehicle body are level, the electric rod motors on both sides stop operating and start adjusting the longitudinal level. When β current < 0, the single-chip microcomputer controls the front electric rod to rise, the IO port outputs a high level, and the motor rotates forward; when β current > 0, the single-chip microcomputer controls the rear electric rod to rise, the IO port outputs a high level, and the motor rotates forward. After the longitudinal adjustment is made so that the front and rear of the vehicle body are level, the electric rods on both the front and rear sides stop operating, and the leveling is completed.

2. The method according to claim 1, wherein The method for increasing the speed value by Δμ in step (a4) is as follows: (a401) The IO port of the single-chip microcomputer is connected to the motor drive board and the encoder. The motor drive board is connected to the wheel motor, and the encoder is installed on the motor shaft; (a402) Convert the target speed value into the target number of pulses per second; (a403) Enable the timer 3 of the single-chip microcomputer for PWM output, and timer 2 for encoder counting; (a404) The encoder measures the current number of pulses per second of the motor; (a405) Compare the current number of pulses with the target number of pulses, and increase the set value of the PWM duty cycle of the single-chip microcomputer; (a406) Substitute the set value of the PWM duty cycle at this time into the CCRx register in the timer 3 of the single-chip microcomputer to generate PWM output, so as to control the level output of the IO port connected to the motor drive board of the single-chip microcomputer, and then control the rotation of the motor to achieve an increase in speed.

3. The method according to claim 1, wherein In the process of the angle sensor collecting angle data in step (b3), the Kalman filtering algorithm is used. The steps are as follows: (b301) Establish a state transition equation: x k+1 = Ax k + Bu k + w k ; where x k+1 represents the state at time k + 1, A is the state transition matrix, which is used to represent the conversion relationship of the system from time k to time k + 1; B is the control matrix; u k is the control input; w k is the process noise; (b302) Define a measurement model: Z k = Hx k + v k ; Z k is the measured value; H is the observation matrix, indicating how the observed value is obtained from the system state; v k is the measurement noise; (b303) Prediction step: At each moment k, predict the state at the next moment according to the current state and control input: Predicted covariance: P k|k-1 = A·P k-1|k-1 ·A T + Q where Q is the covariance matrix of the process noise; (b304) Update step: Update the state estimate according to the new observation data and calculate the Kalman gain: K k = P k-1|k-1 · H T · (H · P k|k-1 · H T + R) -1 ; where R is the covariance matrix of the measurement noise; Update the estimated state: Update the covariance: P k|k = (I - K k · H) · P k|k-1 ; where I is the identity matrix.

Images