Plant protection machine state estimation and attitude adaptive control algorithm based on Kalman filtering

Abstract

The invention discloses a plant protection machine state estimation and attitude adaptive control algorithm based on Kalman filtering, and belongs to the technical field of intelligent agriculture and intelligent agricultural machinery equipment research and development, and the algorithm comprises the steps: scanning the surrounding environment of a plant protection machine through laser SLAM, extracting terrain gradient information, detecting the angular velocity and acceleration of the plant protection machine through an inertial measurement unit IMU, and calculating the attitude of the plant protection machine; acquiring position information of the plant protection machine through a GPS; laser gradient information, angular velocity, acceleration and position information are fused through Kalman filtering, and real-time state estimation is generated; based on the real-time state estimation, designing an attitude adaptive control algorithm to control the driving force and the steering angle of the plant protection machine; and a Ziegler-Nichols method is adopted to determine a proportional gain, an integral gain and a differential gain in an attitude adaptive control algorithm, and a final driving force and a steering angle are output, so that attitude adaptive control is realized. According to the invention, high-precision state estimation and self-adaptive attitude control of the plant protection machine are realized, and the operation performance and stability are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of smart agriculture and smart agricultural machinery equipment research and development, and specifically relates to a plant protection machine state estimation and attitude adaptive control algorithm based on Kalman filtering. Background Art

[0002] Plant protection machines are important equipment for spraying pesticides, fertilizers and other substances in modern agriculture. They are widely used in pest control and crop management in large areas of farmland. With the advancement of agricultural mechanization, ground-based plant protection machines are favored because of their large drug load and high operating efficiency. However, the adaptability of existing plant protection machine technology in complex terrain is still significantly insufficient, especially in hilly and mountainous farmlands with large slopes, which generally face problems such as poor stability and low operating efficiency. Traditional plant protection machines are mostly designed for flat terrain, and their spray boom structure and power system are difficult to cope with sloping terrain, resulting in uneven spraying or increased risk of vehicle rollover. In addition, existing equipment lacks precise dynamic modeling and intelligent control, making it difficult to achieve terrain adaptation and efficient operation.

[0003] In recent years, progress has been made in the field of laser SLAM and multimodal sensors, which has made it possible for agricultural machinery to be intelligent. However, the application of these technologies in plant protection machines is still immature, especially in the integration of dynamic models and real-time control, and there is a lack of systematic solutions. The adjustment range of ground height is limited, which makes it difficult to meet the needs of large-scale, high-efficiency modern agriculture. At the same time, the control system mostly relies on manual operation or simple feedback, and it is difficult to dynamically adjust the vehicle's posture and speed, which limits its application potential in complex environments. Summary of the invention

[0004] In order to solve the technical problem of inaccurate and untimely adjustment of the posture and speed of the above-mentioned plant protection machine, the present invention provides a plant protection machine state estimation and posture adaptive control algorithm based on Kalman filtering.

[0005] According to one aspect of the present invention, a state estimation and attitude adaptive control algorithm for a plant protection machine based on Kalman filtering is provided, comprising: scanning the surrounding environment of the plant protection machine through laser SLAM to extract terrain slope information, detecting the angular velocity and acceleration of the plant protection machine through an inertial measurement unit IMU, and obtaining the position information of the plant protection machine through GPS; using a Kalman filtering algorithm to fuse the laser SLAM slope information, the IMU angular velocity and acceleration, and the GPS position information to generate a real-time state estimation of the plant protection machine, wherein the state estimation includes the position, velocity, and attitude angle of the plant protection machine; constructing a kinematic and dynamic model of the plant protection machine, and designing an attitude adaptive control algorithm based on PID control, wherein the attitude adaptive control algorithm includes driving force control and steering angle control; adjusting the driving force and steering angle of the plant protection machine based on the attitude adaptive control algorithm and according to the real-time state estimation; and using the Ziegler-Nichols tuning method to determine the proportional gain in the attitude adaptive control algorithm. , integral gain and differential gain , generate the final driving force and steering angle control signal, and realize the adaptive control of the posture of the plant protection machine.

[0006] Optionally, a Kalman filter algorithm is used to fuse the laser SLAM slope information, IMU angular velocity and acceleration, and GPS position information to generate a real-time state estimate of the plant protection machine, including: The state vector of the plant protection machine is defined as: ; In the formula, represents the state vector; Indicates the location coordinates of the plant protection machine; Indicates that the plant protection machine is x , y , z Speed ​​in direction; are attitude angles, including roll angle, pitch angle, and yaw angle; upper right corner represents transpose; Establish the state transfer equation of the plant protection machine: ; In the formula, Represents the state vector at the current moment; Represents the state vector at the previous moment; is the state transfer matrix, describing the evolution of the state over time; is the control input matrix, which is used to convert the control input Map to state; is the control input, including the driving force and steering angle , ; is process noise, which obeys normal distribution; Defining the measurement matrix , map the state vector to the sensor measurement value and establish the sensor measurement model: ; In the formula, Represents the sensor observation value; To measure noise; Calculate the Kalman gain: ; ; In the formula, is the Kalman gain; is the measurement noise covariance matrix, It is the mathematical expectation operator, which represents the statistical average of random variables; is the prior error covariance matrix at the current moment; in, The recursive formula is: ; Represents the prior error covariance matrix of the previous moment; initial state covariance Based on sensor accuracy settings; represents the process noise covariance; Status updated to: ; In the formula, It is the state estimation at the current moment, including position, speed, and attitude angle; is the state estimate at the previous moment.

[0007] Optionally, the state transfer matrix The construction process includes: Update the formula according to the position: ; ; ; Construct the state transition matrix of the position part for: ; In the formula, , , is the current position; , , is the position at the previous moment; , , For the previous moment x , y , z Speed ​​in direction; Indicates a time interval; Assuming that the velocity and attitude changes are driven only by the control input and ignoring nonlinear terms, the state transfer matrix of the velocity part is constructed for: ; State transfer matrix of the posture part for: ; According to the definition of the state vector and the , , Get the complete state transfer matrix for: .

[0008] Optionally, the control input matrix The construction process includes: According to driving force Generate acceleration and then affect the velocity component, and get the velocity update formula: ; ; ; In the formula, , , Indicates the current speed; , , Indicates the speed at the previous moment; Indicates the yaw angle at the previous moment; For the quality of plant protection machines; Indicates a time interval; The control input matrix of the speed part is obtained according to the speed update formula for: ; Rotation Angle Affects yaw angle , the yaw angle update formula is: ; Indicates the yaw angle at the current moment; Indicates the wheelbase of the plant protection machine; Assuming the speed is known, , and get the control input matrix of the yaw angle part for: ; According to the definition of the state vector and the and Get the complete control input matrix : .

[0009] Optionally, the kinematic and dynamic models of the plant protection machine are constructed, and the attitude adaptive control algorithm based on PID control is designed, including: Overturning moment of spray boom of plant protection machine The calculation formula is: ; In the formula, is the quality of the spray boom; It is the horizontal distance between the center of gravity of the spray boom and the center of the frame; is the pitch angle of the plant protection machine; is the acceleration due to gravity; In order to keep the rolling angle of the plant protection machine within the reference value range, adjust the steering angle of the plant protection machine. To change the lateral force of the tire or the suspension system to generate control torque, control torque The calculation formula is: ; In the formula, is the proportionality coefficient, which depends on the suspension characteristics and tire grip of the agricultural machinery; The rolling dynamics model of the plant protection machine is established to describe the relationship between the rolling angular acceleration and the torque: ; In the formula, is the moment of inertia of the plant protection machine around the roll axis; is the rolling angular acceleration; Design a steering angle control law based on PID control: ; In the formula, is the roll angle error, which is the difference between the reference roll angle and the current roll angle; is the proportional gain; is the integral gain; is the differential gain; If the reference value of the rolling angle of the plant protection machine is 0, then , then the steering angle control law is expressed as: ; In the formula, is the current roll angle; is the rolling angular velocity; The control torque for: ; Substituting into the rolling dynamics model, we get: ; Arranged into the closed-loop system dynamic equation: ; The rearranged formula describes the closed-loop dynamics of a second-order system and the response characteristics of the roll angle under PID control, where For external disturbances, by adjusting the PID gain , , Make the roll angle approach the reference value.

[0010] Optionally, the driving force control in the attitude adaptive control algorithm is designed as follows: ; In the formula, Indicates driving force; is the reference speed; is the current speed of the plant protection machine, that is, the speed estimated by the real-time state; , are proportional and integral gains respectively; For the quality of plant protection machines; is the pitch angle; The resistance to the plant protection machine; Among them, the resistance of the plant protection machine Including air resistance and friction resistance , the calculation formula is: ; ; ; In the formula, is the air density; is the drag coefficient; is the windward area; is the velocity of the object relative to the fluid; is the friction coefficient.

[0011] Optionally, the steering angle control in the attitude adaptive control algorithm is designed as follows: ; In the formula, To control the steering angle of the output; is the roll angle error, that is, the difference between the reference roll angle and the current roll angle; is the proportional gain; is the integral gain; is the differential gain.

[0012] Optionally, the Ziegler-Nichols tuning method is used to determine the proportional gain in the attitude adaptive control algorithm , integral gain and differential gain , generating the final driving force and steering angle control signals, including: set up = 0, = 0; Gradually increase Until the system output shows continuous oscillation, that is, the system output presents the characteristics of fixed amplitude, fixed period and critical stability on the time axis, record the critical gain at this time and the oscillation period ; According to the records and The proportional gain is calculated using the following formula: , integral gain , differential gain : ; ; ; Output final driving force and the final steering angle The control signal performs adaptive control on the posture of the plant protection machine: ; ; in, represents the critical gain of the driving force; represents the oscillation period of the driving force; Indicates the critical gain of the steering angle; Represents the oscillation period of the steering angle.

[0013] Optionally, before defining the state vector of the plant protection machine, the following is also included: Get the geomagnetic field vector provided by IMU , The geomagnetic field along the carrier coordinate system is x,y,z The weight of the axis; According to the geomagnetic field vector and the roll angle in the slope data and pitch angle Calculate the yaw angle , the calculation formula is: .

[0014] The beneficial effects of the present invention are: The present invention realizes high-precision state estimation of the plant protection aircraft in complex terrain by fusing multi-source sensor data such as laser SLAM, IMU and GPS, and designs a proportional-integral-differential (PID) control attitude adaptive control algorithm by combining kinematic and dynamic models to realize real-time adjustment of the driving force and steering angle of the plant protection aircraft, ensuring stable flight and efficient operation of the plant protection aircraft in complex terrain.

[0015] The present invention combines laser SLAM with multimodal sensors, which can sense the terrain slope in real time and adjust the vehicle posture. This design significantly improves the stability of the plant protection machine on slopes and undulating terrain, solves the problem that traditional plant protection machines are prone to rollover or have low operating efficiency in complex terrain, and provides a reliable mechanized solution for slope agriculture.

[0016] The present invention realizes state estimation through Kalman filtering and derives steady-state Kalman gain. This high-precision state estimation provides reliable data support for path planning and attitude control. The adaptive control algorithm and steering angle adjustment formula can adjust the driving force and vehicle attitude in real time according to terrain changes, maintain speed stability and horizontal state. Compared with traditional plant protection equipment that relies on manual operation, the present invention significantly improves the level of intelligence, reduces human intervention, and improves operation accuracy.

[0017] In addition, the present invention analyzes and adjusts PID parameters through the Ziegler-Nichols method to ensure the rapid response and no steady-state error of the control system. The stability analysis of the closed-loop dynamic equation shows that the reasonably designed PID parameters effectively suppress oscillation and offset external disturbances. This optimized control performance not only ensures the stability of the spray boom in a dynamic environment, but also extends the service life of the equipment and reduces maintenance costs. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings: Figure 1 The present invention is a flowchart of a Kalman filter-based plant protection machine state estimation and attitude adaptive control algorithm. DETAILED DESCRIPTION

[0019] In order to enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only embodiments of a part of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work should fall within the scope of protection of the present application. It should be noted that the embodiments in the present application and the features in the embodiments can be combined with each other without conflict.

[0020] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The term "in one embodiment" that appears in different places in this specification does not necessarily refer to the same embodiment, nor does it refer to a separate or selective embodiment that is mutually exclusive with other embodiments.

[0021] The terms "comprises" and "having" and any variations thereof in the specification and claims of this application are intended to cover non-exclusive inclusions. For example, a process, method, product or apparatus comprising a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to these processes, methods, products or apparatuses.

[0022] Reference Figure 1 , Figure 1 FIG. 1 is a flow chart of a state estimation and attitude adaptive control algorithm for a plant protection machine based on Kalman filtering in an embodiment of the present invention. Figure 1 As shown, the method comprises the following steps: S1, scans the surrounding environment of the plant protection machine through laser SLAM, extracts terrain slope information, detects the angular velocity and acceleration of the plant protection machine through the inertial measurement unit IMU, and obtains the location information of the plant protection machine through GPS; Laser SLAM (Simultaneous Localization and Mapping) technology is used to scan the surrounding environment of the plant protection machine, such as hilly farmland, to build a real-time three-dimensional terrain model of the plant protection machine's operating area, and obtain slope information such as roll angle and pitch angle. At the same time, the inertial measurement unit (IMU) detects the vehicle's roll angle and motion acceleration. The GPS module provides precise location data to help determine the coverage of the operating area. These sensors update data every 0.1 seconds to form a continuous information flow. Through this multi-source perception, the plant protection machine can accurately grasp the external environment and internal status, providing a reliable basis for subsequent intelligent control.

[0023] S2, uses the Kalman filter algorithm to fuse the laser SLAM slope information, IMU angular velocity and acceleration, and GPS position information to generate a real-time state estimate of the plant protection machine; The data collected by the sensor contains noise, which causes a random deviation between the sensor measurement value and the true value. It usually appears as Gaussian noise, and its characteristics include: Mean: Usually assumed to be 0 (no systematic bias).

[0024] Standard deviation (or variance): reflects the noise amplitude and determines the measurement uncertainty.

[0025] Among them, laser SLAM: slope measurement noise, the standard deviation is about 1°.

[0026] IMU: acceleration noise, standard deviation is about 0.1 m / s²; angular velocity noise, standard deviation is about 0.01 rad / s.

[0027] GPS: Position noise, standard deviation about 2 meters.

[0028] This data is standard data. In actual use, it is necessary to collect sensor data through experiments and analyze its noise characteristics.

[0029] Kalman filtering is an algorithm that uses the linear system state equation to make the best estimate of the system state. Since the observed data includes the influence of noise and interference in the system, the optimal estimate can also be regarded as a filtering process. Kalman filtering can remove noise and restore the real data from a series of data with measurement noise when the measurement variance is known, and estimate the state of the dynamic system more accurately.

[0030] The embodiment of the present invention uses the Kalman filter algorithm to fuse the acquired data. The slope information of the laser radar, the angular velocity and acceleration of the IMU and the position signal of the GPS are integrated to generate a high-precision state estimate, which includes key parameters such as the vehicle's position, speed and attitude angle (roll angle, pitch angle, yaw angle). The filter smoothes the noise through the preset gain value, making the position error less than 1 meter and the speed error less than 0.1 meters per second. This precise estimation lays the foundation for the stable control of speed and attitude, ensuring the accuracy of system decision-making.

[0031] IMU directly measures the angular velocity and acceleration of the agricultural drone. The slope information of SLAM includes the roll angle. and pitch angle (Unit: rad). Speed and yaw angle The estimate of is part of the Kalman filter fusion step. The Kalman filter updates the velocity by state transitions, using the GPS position Indirect constraints, such as, for example, , Likewise, the difference between the two can be analogous to the noise of the sensor.

[0032] The yaw angle is usually not provided directly and needs to be indirectly assisted in attitude calculation. Kalman calculates the yaw angle through the geomagnetic field. , Roll Angle and pitch angle Update the yaw angle. The following is the yaw angle calculation process: IMU provides the geomagnetic field vector , which is the vector describing the earth's magnetic field obtained directly by the IMU; among them, is the geomagnetic field along the carrier coordinate system x Axis components (unit: microtesla, μT , or normalized units); is the geomagnetic field along the carrier coordinate system y The weight of the axis; is the geomagnetic field along the carrier coordinate system z axis; then according to the roll angle and pitch angle The yaw angle is calculated by the following formula : .

[0033] For the roll angle at any moment and pitch angle The above formula can be used to get the corresponding yaw angle .

[0034] In the embodiment of the present invention, S2 specifically includes: S21, define the state vector of the plant protection machine as: ; In the formula, represents the state vector; Indicates the location coordinates of the plant protection machine; Indicates that the plant protection machine is x , y , z Speed ​​in direction; are attitude angles, including roll angle, pitch angle, and yaw angle; upper right corner Indicates transpose.

[0035] Laser SLAM provides global positioning and yaw angle correction to compensate for the drift of IMU. The fusion algorithm (Kalman filter) integrates the data of both and outputs a complete six-degree-of-freedom attitude ( ).

[0036] S22, establish the state transfer equation of the plant protection machine: ; In the formula, Represents the state vector at the current moment; Represents the state vector at the previous moment; is the control input. In this embodiment, the control input includes the driving force and steering angle ,Right now ; is the process noise, which obeys the normal distribution and can be obtained by consulting the sensor manual; is the state transfer matrix. According to the kinematic definition, it reflects the physical laws of the system, such as position changes with velocity, and velocity changes with acceleration. Its value is derived from the kinematic relationship and depends on the input of acceleration and angular acceleration as well as the state vector. The specific construction process is: S221, based on uniform linear motion, the position update formula is obtained: ; ; ; In the formula, , , is the current position; , , is the position at the previous moment; , , For the previous moment x , y , z Speed ​​in direction; Indicates a time interval; S222, the above position update formula is expressed in matrix form: ; S223, therefore, the state transfer matrix of the position part is obtained for: ; S224, Linearization Assumption, assumes that the velocity and attitude angle changes are driven only by control inputs (such as driving force) and ignores nonlinear terms (such as , , ), which is incorporated into the control input , without the influence of external control input, construct the state transfer matrix of the speed part for: ; State transfer matrix of the posture part for: ; S225, based on the definition of the state vector and the , , Get the complete state transfer matrix for: .

[0037] is the control input matrix, which describes how the control input affects the system state. The construction process includes: S226, according to driving force Generate acceleration and then affect the velocity component, and get the velocity update formula: ; ; (Assumption z Direction is not affected by driving force); In the formula, , , Indicates the current speed; , , Indicates the speed at the previous moment; Indicates the yaw angle at the previous moment; For the quality of plant protection machines; Indicates a time interval; S227, the speed update formula is expressed in matrix form to obtain a control input matrix of the speed part for: ; S228, rotation angle Affects yaw angle , the yaw angle update formula is: ; Indicates the yaw angle at the current moment; Indicates the wheelbase of the agricultural machinery, used to adjust the steering angle The change of is converted into the change of yaw angle; S229, assuming the speed is known, , and get the control input matrix of the yaw angle part for: ; S2210, based on the definition of the state vector and the and Get the complete control input matrix : .

[0038] S23, define the measurement matrix , map the state vector to the sensor measurement value and establish the sensor measurement model: ; In the formula, Represents the sensor observation value; To measure noise, based on the sensor accuracy, it is usually described in the sensor manual and can be obtained by querying the sensor manual; Measurement Matrix Describes the system status With sensor measurement value The relationship between is used to map the state to the measurement space. Among them, the position coordinates, roll angle, and pitch angle of the plant protection machine are directly measured by the sensor, and the calculation method of the speed and yaw angle is also given. Every data describing the system state can be directly observed (for example, output the data to a display screen), so the observation matrix is the identity matrix.

[0039] S24, calculate the Kalman gain: ; ; In the formula, is the Kalman gain; is the prior error covariance matrix at the current moment; The noise covariance matrix is ​​determined by the sensor noise characteristics and is usually based on the sensor's technical specifications or experimental data. It is the mathematical expectation operator, which represents the statistical average of random variables; The recursive formula is: ; Represents the prior error covariance matrix of the previous moment; initial state covariance Based on sensor accuracy, if the initial position is provided by GPS, the error is 2 meters and the variance is 4m 2 , the initial velocity is unknown (not known precisely), assuming that the possible range is large (such as ±1m / s), and the variance is 1(m / s) 2, the initial posture is unknown (not precisely known), assuming the error is ±5° (about 0.0873rad), the variance is (0.0873) 2 ≈0.0076rad 2 ,but Set to diag(4, 4, 4, 1, 1, 1, 0.0076, 0.0076, 0.0076), which means the position variance is 4 m², the velocity variance is 1 (m / s)², and the attitude variance is 0.0076 rad²; represents the process noise covariance, which is determined by the sensor noise; is the transpose of the state transfer matrix; S25, the status is updated to: ; In the formula, is the state estimate at the current moment, including position, velocity, and attitude angle, that is, the definition And other data; It is the state estimate of the previous moment, which is recursively derived from the initial state. The initial state is generally the uniform motion of the plant protection machine. The state at each moment is recursively derived from the state of the previous moment.

[0040] S3, construct the kinematic and dynamic model of the plant protection machine and design the attitude adaptive control algorithm based on PID control; The motion model of the plant protection machine describes a process. Each data is iterated from the initial state. For example, if there is no external force input and the error is ignored, .

[0041] The adaptive control algorithm includes driving force control and steering angle control.

[0042] Based on the estimated state, the control system of the agricultural machinery runs an adaptive algorithm. For speed control, the system compares the target speed with the actual speed and calculates the required driving force. For attitude control, the system calculates the steering angle adjustment when it detects that the roll angle deviates from the horizontal. These calculations are performed in real time according to terrain changes, ensuring that the vehicle can travel fast and remain stable, avoiding rollover or loss of efficiency due to slope.

[0043] During the operation of the spray boom, due to the shift of the center of gravity and the change of the posture of the plant protection machine, an overturning moment will be generated, affecting the stability of the plant protection machine. The calculation formula is: ; In the formula, is the quality of the spray boom; It is the horizontal distance between the center of gravity of the spray boom and the center of the frame; is the pitch angle of the plant protection machine; is the acceleration due to gravity; The overturning moment reflects the impact of the boom's center of gravity offset on the stability of the plant protection machine. By calculating the overturning moment, the stability of the plant protection machine at different pitch angles can be evaluated, providing a basis for the design of the control algorithm.

[0044] In order to keep the rolling angle of the plant protection machine within the reference value range, adjust the steering angle of the plant protection machine. To change the lateral force of the tire or the suspension system to generate control torque, control torque The calculation formula is: ; In the formula, is the proportionality coefficient, which depends on the suspension characteristics and tire grip of the agricultural machinery; The rolling dynamics model of the plant protection machine is established to describe the relationship between the rolling angular acceleration and the torque: ; In the formula, is the moment of inertia of the plant protection machine around the roll axis; is the rolling angular acceleration; Design a steering angle control law based on PID control: ; In the formula, is the roll angle error, which is the difference between the reference roll angle and the current roll angle; is the proportional gain; is the integral gain; is the differential gain; If the reference value of the rolling angle of the plant protection machine is 0, then , then the steering angle control law is expressed as: ; In the formula, is the current roll angle; is the rolling angular velocity; The control torque for: ; Substituting into the rolling dynamics model, we get: ; Arranged into the closed-loop system dynamic equation: ; The rearranged formula describes the closed-loop dynamics of a second-order system and the response characteristics of the roll angle under PID control, where For external disturbances, by adjusting the PID gain , , Make the roll angle approach the reference value.

[0045] The significance of the above steps is that the PID control using the Ziegler-Nichols method needs to first prove that it is a second-order closed-loop dynamic system, and at the same time, this system needs to be used for tuning.

[0046] The driving force control does not need to be too precise (largely affected by the external environment), and the angle steering needs to be precisely controlled to offset the overturning moment (single external disturbance). In the embodiment of the present invention, the speed control includes: S31, calculate the driving force using the following driving force control law formula: ; In the formula, Indicates driving force; is the reference speed; is the current speed of the plant protection machine, that is, the speed estimated by the real-time state of the Kalman filter; , They are proportional and integral gains respectively. The driving force is also PID controlled, but the default is uniform motion and the acceleration is 0, so it is ignored. ; For the quality of plant protection machines; is the pitch angle; The resistance to the plant protection machine; Including air resistance and friction resistance , the calculation formula is: ; ; ; In the formula, is the air density; is the drag coefficient; is the windward area; is the speed of the object relative to the fluid, that is, the speed of the plant protection machine minus the wind speed; is the friction coefficient.

[0047] Posture control includes: S32, calculating the steering angle using the following steering angle control law formula: ; In the formula, To control the steering angle of the output; is the roll angle error, i.e., the difference between the reference roll angle and the current roll angle, where the current roll angle is the roll angle in the real-time state estimated by the Kalman filter; is the proportional gain; is the integral gain; is the differential gain.

[0048] S4, adjusting the driving force and steering angle of the plant protection machine based on the attitude adaptive control algorithm and according to the real-time state estimation; The control system sends the driving force calculation results to the actuator, and the engine increases power according to the driving force requirements to accelerate the plant protection machine. The steering system adjusts the tire angle to change the vehicle posture to offset the overturning effect of the spray boom caused by the slope, and the hydraulic system also fine-tunes the spray boom height to maintain the distance from the crop. These adjustments work together to ensure the efficiency and stability of the operation.

[0049] S5, using the Ziegler-Nichols tuning method to determine the proportional gain in the attitude adaptive control algorithm , integral gain and differential gain , generate the final driving force and steering angle control signal, and realize the adaptive control of the posture of the plant protection machine.

[0050] The Ziegler-Nichols method is a method for tuning a PID controller and exploring its control parameters. This method is based on the step response characteristics of the system and determines the proportional gain of the PID controller by observing indicators such as system overshoot, oscillation frequency, and attenuation ratio. , integral gain and differential gain The debugging method is to first set the integral and differential gains to 0, and then gradually increase the proportional gain from 0 until the limit gain is reached. , at this time the controller output value oscillates at a constant value. and the oscillation period According to different types, the proportional, integral and differential gains are set in the manner in the preset table. Specifically, S5 includes: S51, Settings = 0, = 0, use proportional control only; S52, gradually increasing , until the system output continues to oscillate, and record the critical gain at this time and the oscillation period ; Sustained Oscillation means that the system output has the following characteristics on the time axis: 1. Fixed amplitude: The peak and valley values ​​of the oscillation remain constant, neither increasing (diverging) nor decreasing (decaying) over time; 2. Fixed period: the period of oscillation (Unit: seconds) remains constant, and the output cycles in a regular sine or approximate sine wave form; 3. Critical stability: The system is in a boundary state between stability and instability. A slight increase in the controller gain will lead to divergence, while a slight decrease will lead to oscillation attenuation.

[0051] S53, according to the records and The proportional gain is calculated using the following formula: , integral gain , differential gain : ; ; ; S54, output final driving force and the final steering angle The control signal performs adaptive control on the posture of the plant protection machine: ; ; in, represents the critical gain of the driving force; represents the oscillation period of the driving force; Indicates the critical gain of the steering angle; Represents the oscillation period of the steering angle.

[0052] The plant protection machine continuously monitors speed, roll angle, slope and other conditions during operation, and feeds the data back to the control system. If the speed deviates from the target or the posture is unstable, the system will immediately adjust the driving force or steering angle, and the control system will recalculate the parameters to maintain stable performance. This closed-loop feedback mechanism enables the plant protection machine to dynamically adapt to complex terrain.

[0053] The state estimation and attitude adaptive control algorithm of the plant protection machine based on Kalman filtering in the present invention uses laser SLAM to scan the terrain of hilly farmland, and the inertial measurement unit (IMU) detects the roll angle and motion acceleration of the vehicle to ensure that its own attitude is understood. The GPS module provides accurate position data to help determine the coverage of the operating area, and then estimates the precise state through the Kalman filter to generate a high-precision state estimate. According to the estimated state, the control system of the plant protection machine runs an adaptive algorithm. For speed control, the system compares the target speed with the actual speed and calculates the required driving force. For attitude control, when the system detects that the roll angle deviates from the horizontal, it calculates the steering angle adjustment. Ensure that the vehicle can travel fast and remain stable, avoiding rollover or efficiency loss caused by slope. The plant protection machine continuously monitors the speed, roll angle, slope and other states during operation, and feeds the data back to the control system. If the speed deviates from the target or the attitude is unstable, the system will immediately adjust the driving force or steering angle. The control system recalculates the PID parameters to maintain stable performance. This closed-loop feedback mechanism enables the plant protection machine to dynamically adapt to complex terrain, significantly improving the stability of the plant protection machine on slopes and undulating terrain, solving the problem of traditional plant protection machines being prone to rollover or having low operating efficiency in complex terrain, and providing a reliable mechanized solution for slope agriculture.

[0054] An embodiment of the present invention further provides a storage medium, in which a computer program is stored, wherein the computer program is configured to execute the steps in the above method embodiment when running.

[0055] Optionally, in this embodiment, the above-mentioned storage medium may include but is not limited to: a USB flash drive, a read-only memory (ROM), a random access memory (RAM), a mobile hard disk, a magnetic disk or an optical disk, and other media that can store computer programs.

[0056] An embodiment of the present invention further provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program implements the steps of the above-described method when executed by the processor.

[0057] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0058] In the above embodiments of the present invention, the description of each embodiment has its own emphasis. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0059] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A plant protection machine state estimation and attitude adaptive control algorithm based on Kalman filtering, characterized in that: include: Scan the surrounding environment of the plant protection machine through laser SLAM to extract terrain slope information, detect the angular velocity and acceleration of the plant protection machine through the inertial measurement unit IMU, and obtain the location information of the plant protection machine through GPS; Use the Kalman filter algorithm to fuse the laser SLAM slope information, IMU angular velocity and acceleration, and GPS location information to generate a real-time state estimate of the plant protection machine, which includes the position, velocity, and attitude angle of the plant protection machine; Construct the kinematic and dynamic models of the plant protection machine, and design a posture adaptive control algorithm based on PID control, which includes driving force control and steering angle control; Adjusting the driving force and steering angle of the plant protection machine based on the attitude adaptive control algorithm and according to the real-time state estimation; Using Ziegler-Nichols tuning method to determine the proportional gain in the attitude adaptive control algorithm , integral gain and differential gain , generate the final driving force and steering angle control signal, and realize the adaptive control of the posture of the plant protection machine.

2. The Kalman filter-based plant protection machine state estimation and attitude adaptive control algorithm according to claim 1, characterized in that: The Kalman filter algorithm is used to fuse the laser SLAM slope information, IMU angular velocity and acceleration, and GPS position information to generate the real-time state estimation of the plant protection machine, including: The state vector of the plant protection machine is defined as: ； In the formula, represents the state vector; Indicates the location coordinates of the plant protection machine; Indicates that the plant protection machine is x , y , z Speed ​​in direction; are attitude angles, including roll angle, pitch angle, and yaw angle; upper right corner represents transpose; Establish the state transfer equation of the plant protection machine: ； In the formula, Represents the state vector at the current moment; Represents the state vector at the previous moment; is the state transfer matrix, describing the evolution of the state over time; is the control input matrix, which is used to convert the control input Map to state; is the control input, including the driving force and steering angle , ; is process noise, which obeys normal distribution; Defining the measurement matrix , map the state vector to the sensor measurement value and establish the sensor measurement model: ； In the formula, Represents the sensor observation value; To measure noise; Calculate the Kalman gain: ； ； In the formula, is the Kalman gain; is the measurement noise covariance matrix, It is the mathematical expectation operator, which represents the statistical average of random variables; is the prior error covariance matrix at the current moment; in, The recursive formula is: ； Represents the prior error covariance matrix of the previous moment; initial state covariance Based on sensor accuracy settings; represents the process noise covariance; The status is updated to: ； In the formula, It is the state estimation at the current moment, including position, speed, and attitude angle; is the state estimate at the previous moment.

3. The plant protection machine state estimation and attitude adaptive control algorithm based on Kalman filtering according to claim 2 is characterized in that: The state transition matrix The construction process includes: Update the formula according to the position: ； ； ； Construct the state transition matrix of the position part for: ； In the formula, , , is the current position; , , is the position at the previous moment; , , For the previous moment x , y , z Speed ​​in direction; Indicates a time interval; Assuming that the velocity and attitude changes are driven only by the control input and ignoring nonlinear terms, the state transfer matrix of the velocity part is constructed for: ； State transfer matrix of the posture part for: ； According to the definition of the state vector and the , , Get the complete state transfer matrix for: 。 4. The plant protection machine state estimation and attitude adaptive control algorithm based on Kalman filtering according to claim 3 is characterized in that: The control input matrix The construction process includes: According to driving force Generate acceleration and then affect the velocity component, and get the velocity update formula: ； ； ； In the formula, , , Indicates the current speed; , , Indicates the speed at the previous moment; Indicates the yaw angle at the previous moment; For the quality of plant protection machines; Indicates a time interval; The control input matrix of the speed part is obtained according to the speed update formula for: ； Rotation Angle Affects yaw angle , the yaw angle update formula is: ； Indicates the yaw angle at the current moment; Indicates the wheelbase of the plant protection machine; Assuming the speed is known, , and get the control input matrix of the yaw angle part for: ； According to the definition of the state vector and the and Get the complete control input matrix : 。 5. The plant protection machine state estimation and attitude adaptive control algorithm based on Kalman filtering according to claim 1 is characterized in that: Construct the kinematic and dynamic models of the plant protection machine and design the attitude adaptive control algorithm based on PID control, including: Overturning moment of spray boom of plant protection machine The calculation formula is: ； In the formula, is the quality of the spray boom; It is the horizontal distance between the center of gravity of the spray boom and the center of the frame; is the pitch angle of the plant protection machine; is the acceleration due to gravity; In order to keep the rolling angle of the plant protection machine within the reference value range, adjust the steering angle of the plant protection machine. To change the lateral force of the tire or the suspension system to generate control torque, control torque The calculation formula is: ； In the formula, is the proportionality coefficient, which depends on the suspension characteristics and tire grip of the agricultural machinery; The rolling dynamics model of the plant protection machine is established to describe the relationship between the rolling angular acceleration and the torque: ； In the formula, is the moment of inertia of the plant protection machine around the roll axis; is the rolling angular acceleration; Design a steering angle control law based on PID control: ； In the formula, is the roll angle error, which is the difference between the reference roll angle and the current roll angle; is the proportional gain; is the integral gain; is the differential gain; If the reference value of the rolling angle of the plant protection machine is 0, then , then the steering angle control law is expressed as: ； In the formula, is the current roll angle; is the rolling angular velocity; The control torque for: ； Substituting into the rolling dynamics model, we get: ； Arranged into the closed-loop system dynamic equation: ； The rearranged formula describes the closed-loop dynamics of a second-order system and the response characteristics of the roll angle under PID control, where For external disturbances, by adjusting the PID gain , , Make the roll angle approach the reference value.

6. The plant protection machine state estimation and attitude adaptive control algorithm based on Kalman filtering according to claim 1 is characterized in that: The driving force control design in the attitude adaptive control algorithm is as follows: ； In the formula, Indicates driving force; is the reference speed; is the current speed of the plant protection machine, that is, the speed estimated by the real-time state; , are proportional and integral gains respectively; For the quality of plant protection machines; is the pitch angle; The resistance to the plant protection machine; Among them, the resistance of the plant protection machine Including air resistance and friction resistance , the calculation formula is: ； ； ； In the formula, is the air density; is the drag coefficient; is the windward area; is the velocity of the object relative to the fluid; is the friction coefficient.

7. The plant protection machine state estimation and attitude adaptive control algorithm based on Kalman filtering according to claim 1 is characterized in that: The steering angle control design in the attitude adaptive control algorithm is as follows: ； In the formula, To control the steering angle of the output; is the roll angle error, that is, the difference between the reference roll angle and the current roll angle; is the proportional gain; is the integral gain; is the differential gain.

8. The Kalman filter-based plant protection machine state estimation and attitude adaptive control algorithm according to claim 1, characterized in that: Using Ziegler-Nichols tuning method to determine the proportional gain in the attitude adaptive control algorithm , integral gain and differential gain , generating the final driving force and steering angle control signals, including: set up = 0, = 0; Gradually increase Until the system output shows continuous oscillation, that is, the system output presents the characteristics of fixed amplitude, fixed period and critical stability on the time axis, record the critical gain at this time and the oscillation period ; According to the records and The proportional gain is calculated using the following formula: , integral gain , differential gain : ； ； ； Output final driving force and the final steering angle The control signal performs adaptive control on the posture of the plant protection machine: ； ； in, represents the critical gain of the driving force; represents the oscillation period of the driving force; Indicates the critical gain of the steering angle; Represents the oscillation period of the steering angle.

9. The plant protection machine state estimation and attitude adaptive control algorithm based on Kalman filtering according to claim 2 is characterized in that: Before defining the state vector of the plant protection machine, it also includes: Get the geomagnetic field vector provided by IMU , The geomagnetic field along the carrier coordinate system is x,y,z The weight of the axis; According to the geomagnetic field vector and the roll angle in the slope data and pitch angle Calculate the yaw angle , the calculation formula is: 。

Images