Unmanned agricultural machine path tracking preset performance control method and system

Abstract

The invention discloses an unmanned agricultural machine path tracking preset performance control method and system, and the method comprises the steps: collecting the pose information of an unmanned agricultural machine, building an unmanned agricultural machine path tracking error kinematic model, and designing a finite time preset performance function based on the unmanned agricultural machine path tracking error kinematic model. The constraint is converted through nonlinear mapping; based on the Lyapunov stability theory, adopting a backstepping method to recursively design an actual control law, and according to the actual control law, performing inverse transformation on the input of the unmanned agricultural machine tracking control system in the error kinematics model of the unmanned agricultural machine tracking control system to obtain the front wheel steering angle of the unmanned agricultural machine. According to the method, convergence is completed within finite time, and transient control performance and steady control performance of a path tracking control system are considered.

Description

Technical Field

[0001] This invention relates to the field of automatic navigation technology for agricultural machinery, and specifically discloses a preset performance control method and system for path tracking of unmanned agricultural machinery, which is particularly suitable for high-precision trajectory tracking scenarios in complex farmland environments. Background Technology

[0002] Currently, automated driving technology for agricultural machinery, as a foundational technology for smart agriculture and unmanned farms, has been widely applied in the agricultural field. Among these technologies, path tracking algorithms are a key component of automated driving systems for agricultural machinery. By inputting the front wheel steering angle, these algorithms enable path tracking and vehicle control, significantly improving operational efficiency. However, poor tracking can lead to crop damage and reduced yield. Therefore, it is necessary to improve the accuracy of path tracking control, especially lateral offset error, to achieve precise and stable tracking of the machinery. Furthermore, when operating on slopes, rugged terrain, or near field ridges or obstacles, the tracking error must be kept within acceptable limits; otherwise, economic losses or safety accidents may occur. Existing methods lack rigorous and safe transient performance control.

[0003] Due to the complexity of actual field conditions, the inaccuracy and strong nonlinearity of vehicle modeling, and the lack of certain vehicle state information, internal and external disturbances exist in agricultural machinery path tracking systems, posing a continued technical challenge to unmanned agricultural machinery path tracking control. Existing path tracking control methods, such as PID control, Stanley control, fuzzy control, and model predictive control, struggle to ensure that errors remain stable within a small range. While increasing the control gain can adjust lateral control accuracy, it also amplifies modeling errors and measurement noise, leading to reduced transient lateral control performance and, in severe cases, system instability. Therefore, there is an urgent need to design a control method that can quickly and accurately converge within the specified range within a finite time, while also exhibiting superior steady-state and transient performance. Summary of the Invention

[0004] To overcome the shortcomings of existing technologies, this invention proposes a preset performance control method for path tracking of unmanned agricultural machinery. This method can converge at a fixed time regardless of the state, ensuring predictable operational efficiency. It can solve the problem of steady-state and transient co-optimization of path tracking systems in high-uncertainty farmland environments. Based on the upper bound of the preset performance function for the operating width, and through error transformation, it converts bounded constraints into unconstrained stabilization problems of errors, ensuring that the lateral position of the agricultural machinery never exceeds the preset operating boundary. This provides theoretical support and technical guarantee for the autonomous upgrading of smart agricultural equipment.

[0005] The technical solution adopted in this invention is as follows:

[0006] A preset performance control method for path tracking of unmanned agricultural machinery:

[0007] Collect the pose information of unmanned agricultural machinery and establish a kinematic model of the path tracking error of unmanned agricultural machinery;

[0008] Based on the kinematic model of the path tracking error of the unmanned agricultural machinery, a finite-time preset performance function is designed, and constraints are transformed through nonlinear mapping.

[0009] Based on Lyapunov stability theory, the actual control law is designed recursively using the backstepping method.

[0010] The input of the unmanned agricultural machinery tracking control system in the error kinematic model of the unmanned agricultural machinery tracking control system is inversely transformed by the actual control law to obtain the front wheel steering angle of the unmanned agricultural machinery. The front wheel steering angle is changed by rotating the steering wheel of the unmanned agricultural machinery to track the path of the unmanned agricultural machinery. The error kinematic model of the unmanned agricultural machinery tracking control system is obtained by state transformation of the unmanned agricultural machinery path tracking error kinematic model.

[0011] Furthermore, the kinematic model for the path tracking error of the unmanned agricultural machinery is as follows:

[0012]

[0013] Among them, l e For lateral deviation, θ e For heading deviation, l e and They are l e θ e The first derivative, γ is the direction coefficient, c d Let V be the curvature of the reference path, V be the longitudinal speed of the unmanned agricultural machinery, and L be the curvature of the reference path. t Δ is the wheelbase of the unmanned agricultural machinery, and Δ is the front wheel steering angle of the unmanned agricultural machinery.

[0014] Furthermore, the kinematic model of the error of the unmanned agricultural machinery tracking and control system is as follows:

[0015]

[0016] Where x1 and x2 are both system states, x1 = l e x2=Vsinθ e , and These are the first derivatives of system states x1 and x2, respectively; intermediate quantities intermediate quantity u is the input of the unmanned agricultural machinery tracking and control system, u = tanΔ.

[0017] Furthermore, the finite-time preset performance function is designed based on the lateral deviation and heading deviation in the kinematic model of the unmanned agricultural machinery path tracking error, specifically as follows:

[0018]

[0019] Among them, ρ0, τ > 0 are all preset constants. Let ρ(0) be the initial value of the preset performance function, and let ρ(0) be the initial value of the tracking error in the non-steady state. τ is the initial value of the tracking error in steady state, T0 is the convergence parameter, and T0 is the preset finite time.

[0020] Furthermore, the upper and lower limits of the lateral deviation of the unmanned agricultural machine are set by a finite-time predetermined performance function: -ρ(t)<e1(t)<ρ(t), where e1(t) is the lateral deviation of the unmanned agricultural machine after constraint.

[0021] Furthermore, we introduce mapping functions. Make e1(t)=ρ(t)χ(ε(t)); Perform error transformation on the upper and lower limits of the lateral deviation constraints of the unmanned agricultural machinery with the introduction of the mapping function: Among them, the intermediate quantity ζ = e1(t) / ρ(t).

[0022] Furthermore, based on Lyapunov stability theory, the actual control law is designed recursively using the backstepping method, specifically as follows:

[0023] First choice of Lyapunov function Let the derivative of V1 be semi-negative definite, then we obtain the virtual control law. Where c1 > 0, intermediate quantity intermediate quantity The first derivative of the target value of the lateral deviation;

[0024] Second choice of Lyapunov function Let the derivative of V2 be negative half-definite, and obtain the input of the unmanned agricultural machinery tracking and control system through recursion. Among them, the intermediate quantity z2 = x2 - α1, c2 > 0.

[0025] Furthermore, the front wheel steering angle of the unmanned agricultural machinery

[0026] A preset performance control system for path tracking of unmanned agricultural machinery includes:

[0027] The path planning module is used to generate reference paths and calculate the curvature of the reference paths in real time.

[0028] The data acquisition module is used to collect the pose information of the unmanned agricultural machinery;

[0029] The controller module receives the curvature of the reference path and the pose information of the unmanned agricultural machine, and calculates the front wheel steering angle of the unmanned agricultural machine in real time based on the actual control law;

[0030] The actuator module is used to respond to control commands for the front wheel steering angle of the unmanned agricultural machinery;

[0031] The communication module is used to transmit the pose information of the unmanned agricultural machinery.

[0032] In the above technical solution, the data acquisition module includes a GNSS receiver, an IMU, a speed sensor, a position sensor, and a steering sensor, used to collect the speed information, position information, and heading angle information of the unmanned agricultural machinery.

[0033] The beneficial effects of this invention are as follows:

[0034] (1) In the case of parameter uncertainty, the present invention constructs a finite-time predetermined performance function, which can guarantee that the system converges in a finite time independent of the state and converges to the preset performance range, thus guaranteeing predictable work efficiency.

[0035] (2) Based on the finite-time preset performance function and backstep control, the present invention recursively constructs a preset performance composite controller to ensure that the lateral deviation of the unmanned agricultural machinery path tracking system converges to an arbitrarily small predetermined neighborhood within a fixed time, so that the steady-state and transient performance of the agricultural machinery are guaranteed.

[0036] (3) This invention effectively solves the problem that traditional control methods cannot simultaneously achieve convergence speed, accuracy and transient performance when dealing with uncertainties in farmland environment. It realizes high precision, high reliability and predictable fast convergence of agricultural machinery paths, and provides key technical support for improving the overall quality and automation level of agricultural operations. Attached Figure Description

[0037] Figure 1 This is a block diagram illustrating the control principle of the present invention;

[0038] Figure 2 This is a schematic diagram of the deviation model of the unmanned agricultural machinery path tracking system of the present invention;

[0039] Figure 3 This is a reference path curve diagram for the unmanned agricultural machinery of the present invention;

[0040] Figure 4 This is a comparison diagram of the reference path and the actual running trajectory in this invention;

[0041] Figure 5 This is a diagram showing the lateral deviation response of the present invention under the reference path;

[0042] Figure 6 This is a diagram showing the heading deviation response of the present invention under the reference path;

[0043] Figure 7 This is a diagram showing the front wheel steering angle response of the present invention under a reference path. Detailed Implementation

[0044] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the present invention will be further described below with reference to the accompanying drawings. It should be noted that the specific embodiments described herein are only for explaining the technical solutions of the present invention and do not limit the present invention.

[0045] The specific process of the unmanned agricultural machinery path tracking preset performance control method proposed in this invention is as follows: Figure 1 ( Figure 1 As shown in the image (where UAVs represent unmanned agricultural machinery), the process includes the following steps:

[0046] Step 1: Establish a kinematic model of the path tracking error of unmanned agricultural machinery. The specific process is as follows:

[0047] Step 1.1: Collect the pose information of the unmanned agricultural machinery and establish its kinematic model, represented as:

[0048]

[0049] Where θ is the heading angle of the unmanned agricultural machinery, V is the longitudinal speed of the unmanned agricultural machinery, and L is the longitudinal speed of the unmanned agricultural machinery. t Where is the wheelbase of the unmanned agricultural machinery, and Δ is the front wheel steering angle of the unmanned agricultural machinery. Let be the first derivative of the x-coordinate vector of the unmanned agricultural machinery. Let be the first derivative of the y-coordinate vector of the unmanned agricultural machinery. The first derivative of the heading angle of the unmanned agricultural machinery;

[0050] Step 1.2, Combining Figure 2 A kinematic model for the path tracking error of unmanned agricultural machinery is established based on the kinematic model of the unmanned agricultural machinery, and is expressed as follows:

[0051]

[0052] Among them, l e For lateral deviation (l) e =-(x t -x d sinθ d +(y t -y d cosθ d ,(x t ,y t (x) represents the current position coordinate vector of the unmanned agricultural machinery in the Frenet coordinate system. d ,y dθ is the position coordinate vector of the reference point in the Frenet coordinate system. e For heading deviation (θ) e =θ t -θ d θ t θ is the current steering angle of the unmanned agricultural machinery. d (for the ideal steering angle of unmanned agricultural machinery) and They are l e θ e The first derivative, γ is the direction coefficient (equal to -1 when rotating clockwise), c d Let the curvature of the reference path be , and:

[0053]

[0054] Where t represents the travel time of the unmanned agricultural machinery.

[0055] For ease of path design, it is assumed that the agricultural machinery travels forward along the trajectory in a clockwise direction, i.e., the direction coefficient γ = -1.

[0056] Step 1.3: Perform state transformation on the kinematic model of the unmanned agricultural machinery path tracking error to obtain the kinematic model of the unmanned agricultural machinery tracking control system error, as shown below:

[0057]

[0058] Where x1 and x2 are both system states, and x1 represents the lateral deviation l. e That is, x1 = l e Let x2 represent Vsinθ e That is, x2 = Vsinθ e intermediate quantity intermediate quantity and denoted as the first derivatives of system states x1 and x2, respectively; u is the input of the unmanned agricultural machinery tracking and control system, expressed as u = tanΔ.

[0059] Step 2: Based on the kinematic model of the unmanned agricultural machinery path tracking error established in Step 1, design a finite-time preset performance function and transform the constraints through nonlinear mapping.

[0060] Step 2.1: Based on lateral deviation and heading deviation, design the finite-time predetermined performance function ρ, expressed as:

[0061]

[0062] Among them, ρ0, τ>0 are all preset constants, and appropriate values ​​are selected according to the working width; Let ρ(0) be the initial value of the preset performance function, and let ρ(0) be the initial value of the tracking error in the non-steady state. τ is the initial value of the tracking error in steady state; τ is the convergence parameter, which controls the "shape" and speed of the function's convergence; T0 is the preset finite time.

[0063] Considering the characteristics of the unmanned agricultural machinery's power system and its working environment, specific upper and lower limits are set for the lateral deviation of the unmanned agricultural machinery, as follows:

[0064] -ρ(t)<e1(t)<ρ(t)

[0065] Where e1(t) is the lateral deviation of the unmanned agricultural machinery after constraint;

[0066] Step 2.2: To achieve the control objective and ensure the validity of the proposed finite-time predetermined performance function under uncertain initial conditions, a mapping function χ(ε(t)) is introduced, expressed as:

[0067]

[0068] Let e1(t)=ρ(t)χ(ε(t));

[0069] Step 2.3: Perform error transformation on the upper and lower limits of the lateral deviation constraints of the unmanned agricultural machinery with the introduced mapping function, specifically expressed as follows:

[0070]

[0071] Among them, the intermediate quantity ζ = e1(t) / ρ(t).

[0072] Step 3: Based on Lyapunov stability theory, recursively design the actual control law using the backstepping method;

[0073] Step 3.1: If the parameter ε converges to zero, the tracking error can be brought within the specified performance boundary, thus achieving a stable state of the system. The Lyapunov function is chosen, specifically expressed as:

[0074]

[0075] Let the derivative of V1 be half-negative definite, then the virtual control law α1 can be expressed as:

[0076]

[0077] Where c1 > 0, intermediate quantity intermediate quantity x 1d The target value for the lateral deviation. The first derivative of the target value of the lateral deviation;

[0078] Step 3.2: Select the Lyapunov function again, specifically as follows:

[0079]

[0080] Among them, the intermediate quantity z2 = x2 - α1;

[0081] Similarly, setting the derivative of V2 to be half-negative definite, the input u (i.e., the actual control law) of the unmanned agricultural machinery tracking control system can be obtained recursively as follows:

[0082]

[0083] Where c2 > 0.

[0084] Step 4: Combine the finite-time preset performance function in Step 2 and the backstepping method recursive design of the actual control rate in Step 3 to construct a preset performance controller; obtain the actual control quantity of the front wheel steering angle of the unmanned agricultural machinery from Step 1 and the preset performance controller, and perform tracking control of the agricultural machinery path based on the control quantity.

[0085] The specific process for tracking and controlling the agricultural machinery path is as follows:

[0086] Based on the actual control law designed using the inverse step recursive method, the inverse transformation of u = tanΔ in step 1 yields the front wheel steering angle of the unmanned agricultural machinery as follows:

[0087]

[0088] By turning the steering wheel of the unmanned agricultural machinery, the steering angle Δ of the front wheels is changed, thereby tracking and controlling the path of the unmanned agricultural machinery.

[0089] A path tracking preset performance control system for unmanned agricultural machinery includes a path planning module, a data acquisition module, a controller module, an actuator module, and a communication module mounted on the unmanned agricultural machinery; the functions of each module are as follows:

[0090] The path planning module is used to generate a reference path and calculate the curvature of the reference path in real time.

[0091] The data acquisition module is mounted on the unmanned agricultural machine and is used to collect the attitude information of the unmanned agricultural machine. The data acquisition module includes a GNSS receiver, an IMU (inertial measurement unit), a speed sensor, a position sensor, and a steering sensor, and collects the speed information, position information, and heading angle information of the unmanned agricultural machine.

[0092] The controller module receives the curvature of the reference path and the pose information of the unmanned agricultural machine, and calculates the front wheel steering angle command of the unmanned agricultural machine in real time based on the preset performance actual control law.

[0093] The actuator module includes an electric steering system for responding to commands from a preset performance controller;

[0094] The communication module is used to transmit the position and orientation information of the unmanned agricultural machinery, and realizes the communication connection with the base station based on the Beidou antenna and radio receiver.

[0095] To better verify the dynamic response characteristics of the designed path tracking algorithm under preset performance constraints, this invention constructs a numerical simulation model based on the Matlab platform. The simulation uses the forward Euler method with a time step of 0.01 s. The simulation results in the example of this invention are as follows: Figure 3-7 As shown.

[0096] Figure 3 This is a reference path curve diagram for the unmanned agricultural machinery of the present invention, that is, the reference path generated by the path planning module.

[0097] Figure 4 The tracking trajectory diagram under the finite-time predetermined performance control method of the present invention shows that the actual tracking trajectory basically coincides with the reference path, indicating that the tracking error is very small.

[0098] Figure 5 and Figure 6 The figures show the lateral deviation and heading deviation, respectively. From the simulation results, the control method of the present invention has good steady-state and transient performance in the agricultural machinery path tracking system. Both the lateral error and the heading error can quickly converge to zero and remain within the working width.

[0099] Figure 7 The diagram shows the front wheel steering angle, indicating that the front wheel steering angle is always kept within a reasonable range.

[0100] The embodiments listed in this invention are merely illustrative of its core design concepts and technical features, intended to enable those skilled in the art to clearly grasp the technical content of this invention and put it into practice. The scope of protection of this invention is by no means limited to these specific examples. Any equivalent methods or improvements made based on the fundamental principles and design ideas embodied in this invention should be considered within the scope of protection of this invention.

Claims

1. A method for preset performance control of path tracking for unmanned agricultural machinery, characterized in that: Collect the pose information of unmanned agricultural machinery and establish a kinematic model of the path tracking error of unmanned agricultural machinery; Based on the kinematic model of the path tracking error of the unmanned agricultural machinery, a finite-time preset performance function is designed, and constraints are transformed through nonlinear mapping. Based on Lyapunov stability theory, the actual control law is designed recursively using the backstepping method. The input of the unmanned agricultural machinery tracking control system in the error kinematic model of the unmanned agricultural machinery tracking control system is inversely transformed by the actual control law to obtain the front wheel steering angle of the unmanned agricultural machinery. The front wheel steering angle is changed by rotating the steering wheel of the unmanned agricultural machinery to track the path of the unmanned agricultural machinery. The error kinematic model of the unmanned agricultural machinery tracking control system is obtained by state transformation of the unmanned agricultural machinery path tracking error kinematic model.

2. The unmanned agricultural machinery path tracking preset performance control method according to claim 1, characterized in that, The kinematic model for the path tracking error of the unmanned agricultural machinery is as follows: Among them, l e For lateral deviation, θ e For heading deviation, and They are l e θ e The first derivative, γ is the direction coefficient, c d Let V be the curvature of the reference path, V be the longitudinal speed of the unmanned agricultural machinery, and L be the curvature of the reference path. t Δ is the wheelbase of the unmanned agricultural machinery, and Δ is the front wheel steering angle of the unmanned agricultural machinery.

3. The unmanned agricultural machinery path tracking preset performance control method according to claim 2, characterized in that, The kinematic model of the error of the unmanned agricultural machinery tracking and control system is as follows: Where x1 and x2 are both system states, x1 = l e x2=Vsinθ e , and These are the first derivatives of system states x1 and x2, respectively; intermediate quantities intermediate quantity u is the input of the unmanned agricultural machinery tracking and control system, u = tanΔ.

4. The unmanned agricultural machinery path tracking preset performance control method according to claim 3, characterized in that, The finite-time preset performance function is designed based on the lateral deviation and heading deviation in the kinematic model of the unmanned agricultural machinery path tracking error, specifically as follows: Among them, ρ0, τ > 0 are all preset constants. Let ρ(0) be the initial value of the preset performance function, and let ρ(0) be the initial value of the tracking error in the non-steady state. τ is the initial value of the tracking error in steady state, T0 is the convergence parameter, and T0 is the preset finite time.

5. The unmanned agricultural machinery path tracking preset performance control method according to claim 4, characterized in that, The upper and lower limits of the lateral deviation of the unmanned agricultural machine are set by a finite-time predetermined performance function: -ρ(t)<e1(t)<ρ(t), where e1(t) is the lateral deviation of the unmanned agricultural machine after constraint.

6. The unmanned agricultural machinery path tracking preset performance control method according to claim 5, characterized in that, Introducing mapping functions Make e1(t)=ρ(t)χ(ε(t)); Perform error transformation on the upper and lower limits of the lateral deviation constraints of the unmanned agricultural machinery with the introduction of the mapping function: Among them, the intermediate quantity ζ = e1(t) / ρ(t).

7. The unmanned agricultural machinery path tracking preset performance control method according to claim 6, characterized in that, Based on Lyapunov stability theory, the actual control law is designed recursively using the backstepping method, specifically as follows: First choice of Lyapunov function Let the derivative of V1 be semi-negative definite, then we obtain the virtual control law. Where c1 > 0, intermediate quantity intermediate quantity The first derivative of the target value of the lateral deviation; Second choice of Lyapunov function Let the derivative of V2 be negative half-definite, and obtain the input of the unmanned agricultural machinery tracking and control system through recursion. Among them, the intermediate quantity z2 = x2 - α1, c2 >

0.

8. The unmanned agricultural machinery path tracking preset performance control method according to claim 7, characterized in that, The front wheel steering angle of the unmanned agricultural machinery 9. A system for implementing the preset performance control method for path tracking of unmanned agricultural machinery according to any one of claims 1-8, characterized in that, include: The path planning module is used to generate reference paths and calculate the curvature of the reference paths in real time. The data acquisition module is used to collect the pose information of the unmanned agricultural machinery; The controller module receives the curvature of the reference path and the pose information of the unmanned agricultural machine, and calculates the front wheel steering angle of the unmanned agricultural machine in real time based on the actual control law; The actuator module is used to respond to control commands for the front wheel steering angle of the unmanned agricultural machinery; The communication module is used to transmit the pose information of the unmanned agricultural machinery.

10. The system according to claim 9, characterized in that, The data acquisition module includes a GNSS receiver, an IMU, a speed sensor, a position sensor, and a steering sensor, used to collect speed information, position information, and heading angle information of the unmanned agricultural machinery.

Images