An unmanned agricultural machine finite time output feedback control method and control device under unknown heading information

Abstract

The application discloses an unmanned agricultural machine finite time output feedback control method and control equipment under unknown heading information, and belongs to the unmanned agricultural machine path tracking control field.Under the method, the unmanned agricultural machine can realize the goal of finite time accurate tracking of a reference path.Main steps are as follows: 1, based on the kinematic model of the unmanned agricultural machine, a deviation model is constructed by considering disturbance terms, and then converted into a state space equation;2, a state observer is designed to observe state variables containing heading information;3, based on the state observer, a finite time output feedback controller is designed to realize path tracking control.The application has the following advantages: firstly, the disturbance is considered in the deviation model, improving the model accuracy;secondly, a state observer is constructed to observe state variables containing heading information, ensuring the stability of the system when the heading information is unmeasurable;thirdly, a finite time output feedback path tracking control method is designed to ensure the finite time convergence of tracking error.

Description

Technical Field

[0001] This invention belongs to the field of path tracking control for unmanned agricultural machinery, and relates to a finite-time output feedback control method and control device for unmanned agricultural machinery under unknown heading information. Background Technology

[0002] Agricultural machinery is a crucial foundation for modern agricultural development and a significant indicator of agricultural modernization. The degree of agricultural mechanization and automation is a key factor influencing agricultural production efficiency and productivity. Notably, autonomous navigation technology is vital in agricultural operations such as rotary tillage, planting, pesticide application, and harvesting. It primarily comprises three parts: farmland environment perception, task decision-making and path planning, and agricultural machinery path tracking control. Among these, high-precision and stable agricultural machinery path tracking control is a critical technology that urgently needs to be addressed.

[0003] The aforementioned technologies rely on reliable high-precision positioning systems and attitude sensors, as well as control algorithms capable of rapid convergence and low steady-state error. Notably, the BeiDou RTK system can provide centimeter-level positioning accuracy to measure the position information of agricultural machinery and calculate the lateral deviation from the reference path. On the other hand, heading information typically relies on inertial measurement units (IMUs), which are prone to zero drift. Affected by IMU bias and variations in input / output scale factors, the measurement error of the heading angle increases over time, leading to decreased tracking accuracy and even instability, making it difficult to meet the requirements of long-term, high-efficiency operation of unmanned agricultural machinery. Furthermore, high-precision IMU / RTK combined systems are very expensive. Therefore, designing a state observer capable of estimating heading information is highly meaningful.

[0004] In unstructured farmland environments, uncertainties in agricultural machinery modeling and uneven road surfaces lead to internal and external disturbances in agricultural machinery path tracking systems. Existing path tracking control methods, such as PID control, Stanley control, fuzzy control, and model predictive control, struggle to achieve finite-time convergence. Since agricultural machinery path tracking systems have high requirements for convergence time, there is an urgent need to design a control method capable of achieving finite-time convergence.

[0005] Therefore, this invention designs a finite-time output feedback control method for unmanned agricultural machinery based on a state observer capable of observing heading information, so as to ensure that the lateral deviation can converge to zero in a finite time when the heading information is unmeasurable, thus guaranteeing its speed and steady-state performance. Summary of the Invention

[0006] To achieve finite-time convergence of the agricultural machinery path tracking control system when heading information is unpredictable, while also possessing good anti-interference and steady-state performance, this invention proposes a finite-time output feedback control method for unmanned agricultural machinery under unknown heading information, comprising the following steps:

[0007] Step 1: Based on the kinematic model of the unmanned agricultural machinery, introduce disturbances related to lateral and heading deviations to construct the deviation model of the unmanned agricultural machinery, and then convert it into state-space equations;

[0008] Step 2: Considering the case where heading information is unmeasurable, construct a state observer to observe state variables containing heading information, thereby improving the stability and tracking accuracy of the tracking system;

[0009] Step 3: Based on the state observer constructed in Step 2, a finite-time output feedback controller is designed according to the finite-time Lyapunov control theory.

[0010] Step 4: Design the controller parameter adaptive rate;

[0011] Step 5: Combine the adaptive rate with the finite-time output feedback control method to obtain an adaptive finite-time output feedback controller, and then apply the controller u = ta. n By performing an inverse transformation on δ, we obtain the required front wheel rotation angle δ(t) = arctanu(t) for the agricultural machinery;

[0012] Step 6: Based on the CAN network protocol for agricultural machinery control, construct a data transmission module to send the system's control signals, including the agricultural machinery's speed V and the agricultural machinery's front wheel steering angle δ obtained in Step 5, to the agricultural machinery controller via the CAN bus to achieve path tracking control of the agricultural machinery.

[0013] Furthermore, in step 1, the unmanned agricultural machinery is front-wheel steering and rear-wheel drive, and its kinematic model is as follows:

[0014]

[0015] Where P = [x, y] T V is the position coordinate vector of the unmanned agricultural machinery; θ is the longitudinal speed of the unmanned agricultural machinery; L is the heading angle; t δ is the wheelbase of the unmanned agricultural machinery, which is the distance from the center of the front wheel to the center of the rear wheel; δ is the front wheel steering angle of the unmanned agricultural machinery.

[0016] This invention provides the following definitions:

[0017] Lateral deviation L os , is defined as the distance between the current location of the agricultural machinery and the nearest point on the reference path;

[0018] Heading deviation θ os, is defined as the difference between the current heading angle and the desired heading angle of the agricultural machinery;

[0019] The specific formula is as follows:

[0020]

[0021] Among them, P d =[x d y d ] T P is the position coordinate vector of the reference point; t =[x t y t ] T θ is the current position coordinate vector of the unmanned agricultural machinery; t θ represents the current heading angle of the unmanned agricultural machinery. d The desired heading angle;

[0022] Based on the kinematic model of the unmanned agricultural machinery, and by introducing disturbances related to lateral and heading deviations, the deviation model of the unmanned agricultural machinery can be established as follows:

[0023]

[0024] Where λ is the steering coefficient; Δ1 and Δ2 are the disturbances related to lateral deviation and heading deviation, respectively;

[0025] Assume the unmanned agricultural machinery always moves forward and follows the reference path in a clockwise direction, i.e., V>0, λ=-1; furthermore, based on the above deviation model, select the state variable x1=L os x2=Vsinθ os +Δ1, construct the state-space equations as follows:

[0026]

[0027] Where x = [x1, x2] T It is the state vector of the unmanned agricultural machinery path tracking system. u = tanδ is the control variable. This indicates a lumped disturbance.

[0028] Furthermore, in step 2, considering the working conditions such as obstruction or communication rejection in actual farmland environments, which can lead to the unmanned agricultural machinery's heading information being unpredictable, the state variable x2 containing the heading information is unknown. Therefore, it is necessary to design an observer to estimate the value of x2. The state observer can be constructed as follows:

[0029]

[0030] Where z is an intermediate variable, and K(x1) and N(x1) are both positive functions; It is an estimate of the state variable x2.

[0031] Furthermore, based on the state observer constructed in step 2, the values ​​of functions K(x1) and N(x1) satisfy... Under the above relationship, the observation error can be ensured. It converges to zero within a finite amount of time.

[0032] Furthermore, in step 3, the estimated value based on the state observer... Considering that farmland operations need to be deployed quickly and have high requirements for convergence time, a finite-time output feedback controller is constructed as follows:

[0033]

[0034] Where β1 > 0 is the control gain, and β2(x1) is a positive function. g It is the lower bound of the function g(t, x), satisfying 0 < g <g(t, x); At this point, by constructing an output feedback controller, the agricultural machinery path tracking control is achieved using only the lateral deviation.

[0035] Furthermore, in step 4, to reduce the complexity of adjusting the parameters and improve the tracking effect, an adaptive rate is designed, and the control gain β1 is designed as a time-varying gain β1(t), which has the following form:

[0036]

[0037] Where β1(0)>0 is the initial value of the time-varying gain β1(t), k>0, ξ>0, and μ>0 are auxiliary variables. The values ​​of ξ and μ are small. The purpose of introducing μ is to ensure that the adaptive parameter β1(t) is positive.

[0038] Furthermore, in step 5, by combining the adaptive rate and finite-time output feedback control methods, an adaptive finite-time output feedback controller is obtained, which takes the following form:

[0039]

[0040] Where β1(t)>0 is the adaptive control gain;

[0041] Finally, an inverse transformation is performed on the controller u = tanδ to obtain the required front wheel rotation angle δ(t) = arctanu(t) for the agricultural machinery.

[0042] Furthermore, in step 6, a data transmission module is constructed according to the specific CAN network protocol for the unmanned agricultural machinery vehicle control. The specific construction process is as follows: the CAN bus ID adopts the extended ID, the CAN format adopts the Intel format, and a CAN communication message matching the unmanned agricultural machinery is designed; using the data transmission module, the system's control signals, including the agricultural machinery's speed V and the actual front wheel steering angle δ obtained in step 5, are sent to the agricultural machinery vehicle controller via the CAN bus to realize the path tracking control of the agricultural machinery.

[0043] The beneficial effects of this invention are:

[0044] 1. This invention considers the situation where heading information is unknown, and constructs a state observer to estimate state variables containing heading information;

[0045] 2. Based on the above state observer, a finite-time output feedback control method is constructed to achieve finite-time convergence of the lateral deviation of the agricultural machinery path tracking system to zero;

[0046] 3. Construct a CAN bus control network for the overall control of agricultural machinery, and send CAN signals containing vehicle speed and front wheel angle to the agricultural machinery in real time to realize closed-loop control of the agricultural machinery path tracking system. Attached Figure Description

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

[0048] Figure 2 This is a block diagram of the finite-time output feedback control principle of the present invention;

[0049] Figure 3 This is a comparison chart of the reference path and the actual running trajectory;

[0050] Figure 4 This is a diagram of the lateral deviation response under the reference path;

[0051] Figure 5 This is a diagram showing the heading deviation response under the reference path;

[0052] Figure 6 This is a diagram showing the front wheel steering angle response under the reference path;

[0053] Figure 7 A comparison of the lateral deviation response at different initial positions;

[0054] Figure 8 A comparison chart of heading deviation responses at different initial positions;

[0055] Figure 9 A comparison chart of lateral deviation response at different speeds;

[0056] Figure 10 A comparison chart of heading deviation response at different speeds. Detailed Implementation

[0057] The invention will now be further described with reference to the accompanying drawings.

[0058] The following specific examples illustrate the implementation of the present invention, and those skilled in the art can easily implement it based on the content disclosed in this specification.

[0059] The deviation model of the agricultural machinery path tracking system of the present invention is as follows: Figure 1 As shown in Table 1, the simulation parameters of the agricultural machinery path tracking system of the present invention are as follows.

[0060] Table 1. Parameters of the agricultural machinery path tracking system used in the simulation

[0061]

[0062]

[0063] A finite-time output feedback control method for unmanned agricultural machinery under unknown heading information. The implementation process of the method includes the following steps:

[0064] 1. Based on Figure 1 The given schematic diagram of the dynamic model of agricultural machinery path tracking deviation shows that the unmanned agricultural machinery is front-wheel steering and rear-wheel drive, and its kinematic model is as follows:

[0065]

[0066] Where P = [x, y] T V is the position coordinate vector of the unmanned agricultural machinery; θ is the longitudinal speed of the unmanned agricultural machinery; L is the heading angle; t δ is the wheelbase of the unmanned agricultural machinery, which is the distance from the center of the front wheel to the center of the rear wheel; δ is the front wheel steering angle of the unmanned agricultural machinery.

[0067] The following definition is given:

[0068] Lateral deviation L os , is defined as the distance between the current location of the unmanned agricultural machinery and the nearest point on the reference path;

[0069] Heading deviation θ os , defined as the difference between the current heading angle and the desired heading angle of the unmanned agricultural machinery;

[0070] The specific formula is as follows:

[0071]

[0072] Among them, P d =[x d yd ] T P is the position coordinate vector of the reference point; t =[x t y t ] T θ is the current position coordinate vector of the unmanned agricultural machinery; t θ represents the current heading angle of the unmanned agricultural machinery. d The desired heading angle;

[0073] Based on the kinematic model of the unmanned agricultural machinery, and by introducing disturbances related to lateral and heading deviations, the deviation model of the unmanned agricultural machinery can be established as follows:

[0074]

[0075] Where λ is the steering coefficient; Δ1 and Δ2 are the disturbances related to lateral deviation and heading deviation, respectively;

[0076] Assume the unmanned agricultural machinery always moves forward and follows the reference path in a clockwise direction, i.e., V>0, λ=-1; furthermore, based on the above deviation model, select the state variable x1=L os x2=Vsinθ os +Δ1, construct the state-space equations as follows:

[0077]

[0078] Where x = [x1, x2] T It is the state vector of the unmanned agricultural machinery path tracking system. u = tanδ is the control variable. This indicates a lumped disturbance.

[0079] 2. Considering the working conditions in actual farmland environments, such as obstruction or communication denial, the heading information of unmanned agricultural machinery may be unpredictable. Consequently, the state variable x2, which contains heading information, is unknown. Therefore, it is necessary to design an observer to estimate the value of x2. The state observer can be constructed as follows:

[0080]

[0081] Where z is an intermediate variable, and K(x1) and N(x1) are both positive functions; It is an estimate of the state variable x2.

[0082] 3. The values ​​of functions K(x1) and N(x1) in the above state observer satisfy the following: Under the above relationship, the observation error can be ensured. It converges to zero within a finite amount of time.

[0083] 4. Estimated values ​​based on two state observers Considering that farmland operations need to be deployed quickly and have high requirements for convergence time, such as Figure 2 Based on the control principle shown, a finite-time output feedback controller is constructed as follows:

[0084]

[0085] Where β1>0 is the adaptive control gain, β2(x1) is a positive function, and g is the lower bound of the function g(t, x), satisfying 0 < g <g(t, x); At this point, by constructing an output feedback controller, the agricultural machinery path tracking control is achieved using only the lateral deviation.

[0086] 5. To reduce the complexity of parameter adjustment and improve tracking performance, an adaptive rate is designed, with the control gain β1 designed as a time-varying gain β1(t), which has the following form:

[0087]

[0088] Where β1(0)>0 is the initial value of the time-varying gain β1(t), k>0, ξ>0, μ>0 are auxiliary variables, the values ​​of ξ and μ are small, and the purpose of introducing μ is to ensure that the adaptive parameter β1(t) is positive.

[0089] 6. Combining the adaptive rate and the finite-time output feedback controller, we obtain the adaptive finite-time output feedback controller, which has the following form:

[0090]

[0091] Where β1(t)>0 is the adaptive control gain; in addition, by performing an inverse transformation on the controller u=tanδ, the required front wheel rotation angle δ(t)=arctanu(t) of the agricultural machinery is obtained.

[0092] 7. Based on the specific CAN network protocol for unmanned agricultural machinery vehicle control, a data transmission module is constructed. The specific construction process is as follows: the CAN bus ID adopts the extended ID, the CAN format adopts the Intel format, and a CAN communication message matching the unmanned agricultural machinery is designed; using the data transmission module, the system's control signals, including the agricultural machinery's speed V and the actual front wheel steering angle δ obtained in step 6, are sent to the agricultural machinery vehicle controller via the CAN bus to realize the path tracking control of the agricultural machinery.

[0093] Example:

[0094] The design is validated using the following simulation results:

[0095] First, considering that agricultural machinery mainly involves straight-line travel and lane-changing travel during actual operation, a reference path Ω composed of a straight line and a semicircle is selected to simulate the actual travel path of the agricultural machinery, as shown below:

[0096]

[0097] Where, θ r (t) is the expected heading angle of the reference path, [x r (t), y r [(t)] is the reference position vector, and the generated (x) r y r The trajectory is the reference path.

[0098] Secondly, the Matlab / Simulink simulation program was built according to the parameters given in Table 1, and the perturbations related to the lateral deviation were analyzed. The disturbance related to the heading deviation is Δ2 = 0.1sin(2t). This simulation consists of the following three parts:

[0099] The first part describes the initial state error of the agricultural machinery under the control method of this invention, which is selected as (L). os (0), θ os Simulation verification when (0))=(0.5,0) is shown in the simulation results. Figure 3-6 As shown.

[0100] Figure 3 The tracking trajectory diagram under the finite-time output feedback 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.

[0101] Figure 4 and Figure 5 The figures show the lateral deviation and heading deviation, respectively. From the simulation results, the control method of the present invention has good dynamic and steady-state performance in the agricultural machinery path tracking system. Both the lateral error and heading error can converge to zero quickly, and can remain near zero even in the presence of disturbances.

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

[0103] The second part involves selecting L as the initial lateral deviation of the agricultural machinery under the control method of this invention. os (0) = 0.5, 0.8, 1, and the initial heading deviation is selected as θ. os Comparative simulation verification when (0) = 0, the simulation results are as follows: Figure 7-8 As shown.

[0104] Figure 7 This is a comparison chart of lateral deviations; Figure 8 The diagram shows a comparison of heading deviations. Simulation results indicate that, under different initial conditions, the control method of this invention exhibits good stability and disturbance rejection performance. Both lateral and heading errors can converge quickly to zero, and can remain near zero even in the presence of disturbances.

[0105] The third part describes the initial lateral deviation of the agricultural machinery under the control method of this invention, which is selected as L. os (0) = 0.5, and the initial heading deviation is selected as θ. os (0) = 0, and comparative simulations were conducted with velocities of V = 2, 3, and 4 m / s respectively. The simulation results are as follows: Figure 9-10 As shown.

[0106] Figure 9 This is a comparison chart of lateral deviations; Figure 10 The diagram shows a comparison of heading deviations. Simulation results indicate that the control method of this invention exhibits good stability and anti-interference performance at different agricultural machinery speeds. Both lateral and heading errors can quickly converge to zero, and can remain near zero even under disturbance conditions.

[0107] Based on the above control method, this embodiment of the invention also proposes a control device for unmanned agricultural machinery. The control device includes an information processing module, a data receiving module, and a data sending module, specifically implemented by an STM32F4 microcontroller. Based on the location information of the unmanned agricultural machinery received from the Beidou positioning system by the data receiving module, the information processing module can implement the contents of steps 1-6 above, and the data sending module can implement the contents of step 7 above, thereby realizing the overall vehicle control of the unmanned agricultural machinery.

[0108] The detailed descriptions listed above are merely specific descriptions of feasible embodiments of the present invention, and are not intended to limit the scope of protection of the present invention. All equivalent methods or modifications that do not depart from the technology of the present invention should be included within the scope of protection of the present invention.

Claims

1. A finite-time output feedback control method for unmanned agricultural machinery under unknown heading information, characterized in that, include: Step 1: Based on the kinematic model of the unmanned agricultural machinery, introduce disturbances related to lateral deviation and heading deviation to construct the deviation model of the unmanned agricultural machinery, and then convert it into state-space equations; In step 1, the unmanned agricultural machinery deviation model is established as follows: in, It is the steering coefficient; , These are disturbances related to lateral deviation and heading deviation, respectively. Assuming the agricultural machinery always moves forward and follows the reference path in a clockwise direction, that is... Based on the above deviation model, state variables are selected. The state-space equations are constructed as follows: in, It is the state vector of the agricultural machinery path tracking system. , To control variables, Indicates lumped disturbance; in, It is the longitudinal travel speed of the unmanned agricultural machinery; It refers to the wheelbase of the unmanned agricultural machinery; It is a lateral deviation; It is a heading deviation; The desired heading angle; It refers to the steering angle of the front wheels of the unmanned agricultural machinery; Step 2: Considering the case where heading information is unmeasurable, construct a state observer to observe state variables containing heading information; Step 3: Based on the state observer constructed in Step 2, and according to the finite-time Lyapunov control theory, a finite-time output feedback controller is designed; the specific details of the finite-time output feedback controller are as follows: in, It is about controlling the gain. It is a positive function. It is a function The lower bound value satisfies At this point, by constructing an output feedback controller, the agricultural machinery path tracking control is achieved using only the lateral deviation; Step 4: Design the controller parameter adaptive rate; In step 4, the adaptive rate is designed as follows: Control gain Designed for time-varying gain Specifically, it is as follows: in, , for time-varying gain initial value, , as an auxiliary variable and The value is small, so introduce The goal is to ensure adaptive parameters It is a positive value; Step 5: Combine the adaptive rate with the finite-time output feedback controller to obtain the adaptive finite-time output feedback controller, and perform an inverse transformation on the controller to obtain the required front wheel angle of the agricultural machinery; Step 6: Send the control signals, including the speed of the agricultural machinery and the front wheel angle of the agricultural machinery obtained in Step 5, to the agricultural machinery vehicle controller to realize the path tracking control of the agricultural machinery.

2. The finite-time output feedback control method for unmanned agricultural machinery under unknown heading information as described in claim 1, characterized in that, In step 1, the unmanned agricultural machinery is front-wheel steering and rear-wheel drive, and its kinematic model is as follows: in, It is the position coordinate vector of the unmanned agricultural machinery; It is the longitudinal travel speed of the unmanned agricultural machinery; For heading angle; It is the wheelbase of the unmanned agricultural machinery, that is, the distance from the center of the front wheel to the center of the rear wheel; It is the steering angle of the front wheels of the unmanned agricultural machinery.

3. The finite-time output feedback control method for unmanned agricultural machinery under unknown heading information as described in claim 2, characterized in that, In step 1, the lateral deviation The heading deviation is defined as the distance between the current position of the agricultural machinery and the nearest point on the reference path; , is defined as the difference between the current heading angle and the desired heading angle of the agricultural machinery; The specific formula is as follows: in, It is the position coordinate vector of the reference point; It is the current position coordinate vector of the agricultural machinery; This refers to the current heading angle of the agricultural machinery. The desired heading angle.

4. The finite-time output feedback control method for unmanned agricultural machinery under unknown heading information as described in claim 1, characterized in that, In step 2, the state observer can be constructed as follows: in, It is an intermediate variable. All are positive functions; It is a state variable The estimated value; Functions in the state observer The value of must satisfy Under the above relationship, the observation error can be ensured. It converges to zero within a finite amount of time.

5. The finite-time output feedback control method for unmanned agricultural machinery under unknown heading information as described in claim 1, characterized in that, In step 5, the adaptive finite-time output feedback controller is designed as follows: in, It is adaptive control gain; For controller By performing the inverse transformation, the required front wheel rotation angle of the agricultural machinery can be obtained. .

6. The finite-time output feedback control method for unmanned agricultural machinery under unknown heading information as described in claim 1, characterized in that, The specific implementation of step 6 includes the following: The CAN bus ID adopts an extended ID, and the CAN format adopts the Intel format. The CAN communication message is designed to match the unmanned agricultural machinery. The control signal is sent to the agricultural machinery controller via the CAN bus using the data transmission module to realize the path tracking control of the agricultural machinery.

7. A control device for unmanned agricultural machinery, characterized in that, The control device includes an information processing module, a data receiving module, and a data sending module; based on the location information of the unmanned agricultural machinery received from the Beidou positioning system by the data receiving module, the information processing module can implement the content of steps 1-5 as described in any one of claims 1-6; the data sending module can implement the content of step 6 as described in any one of claims 1-6.

Images