Agricultural machine path tracking control method and system based on data sampling output feedback

Abstract

The invention discloses an agricultural machine path tracking control method and system based on data sampling output feedback. The method comprises the following steps: firstly, establishing an agricultural machine path tracking model containing an unknown state; designing a discrete time observer based on the established agricultural machine path tracking model containing the unknown state, and designing a data sampling output feedback controller by using vehicle state information estimated by the discrete time observer; the actual control quantity of the agricultural machine front wheel rotating angle is obtained through a data sampling output feedback controller, and the agricultural machine path is tracked and controlled based on the control quantity. The data sampling controller model adopted by the invention is in a discrete expression form, is more suitable for application scenes with a relatively long sampling period and relatively rough equipment, and is convenient to realize in a digital control system, and relatively accurate path tracking can be realized only by using a low-cost sensor of the vehicle, so that the cost can be reduced.

Description

Technical Field

[0001] The present invention relates to the field of agricultural machinery navigation technology, and in particular to an agricultural machinery path tracking control method and system based on data sampling output feedback. Background Art

[0002] In recent years, with the improvement of agricultural mechanization, agricultural machinery has been widely used in the agricultural field, and its popularity and application degree are increasing. As an important agricultural production tool in modern society, the popularization of unmanned agricultural machinery has brought great convenience to the work of rural labor, but it has also brought serious consequences. Among them, the large path tracking error has caused food waste, which has become a global problem.

[0003] Due to the complexity of agricultural machinery working scenes, in order to improve the control accuracy of vehicle path tracking, a series of control algorithms, such as sliding mode control, Stanley control, fuzzy control, etc., have emerged. For example, the patent with application number 202410279298.8, "A method for controlling agricultural machinery path tracking based on fixed-time non-singular terminal sliding mode", although it can achieve the control of path tracking accuracy, does not consider the impact of excessive computer sampling time on algorithm accuracy. Therefore, the implementation of existing path tracking control algorithms all depends on continuous systems. On the other hand, the low measurement accuracy of low-cost sensors often affects the accuracy of path tracking. Because the RTK navigation system is very expensive and the computer computing power required to execute the algorithm is too high, the implementation cost of the path tracking algorithm is too high and the effect is not ideal.

[0004] Therefore, how to use the information measured by various existing sensors of the vehicle to achieve more accurate path tracking is an important part of unmanned agricultural machinery research and has attracted the attention of many domestic and foreign researchers. Summary of the invention

[0005] In order to address the deficiencies in the prior art, the present application proposes an agricultural machinery path tracking control method and system based on data sampling output feedback. The present invention designs a data sampling controller based on the data sampling control method, which has good adaptability to computer control systems; and relatively accurate path tracking can be achieved by utilizing the vehicle's own low-cost sensors.

[0006] The technical solution adopted by the present invention is as follows:

[0007] A method for controlling agricultural machinery path tracking based on data sampling output feedback comprises the following steps:

[0008] Step 1: Establish an agricultural machinery path tracking model including unknown states;

[0009] Step 2: Based on the agricultural machinery path tracking model with unknown states established in step 1, a discrete time observer is designed, and the heading deviation is estimated using the discrete time observer;

[0010] Step 3: Design a data sampling output feedback controller using the vehicle state information estimated by the discrete-time observer;

[0011] Step 4: Utilize the data sampling output feedback controller in step 3 to obtain the actual control amount of the front wheel steering angle of the agricultural machinery, and track and control the path of the agricultural machinery based on the control amount.

[0012] Furthermore, the method for establishing the agricultural machinery path tracking model including unknown states in step 1 is:

[0013] Step 1.1: Establish an agricultural machinery path tracking model, expressed as:

[0014]

[0015] Among them, l od is the lateral deflection, θ od For deviation, l od ,θ od The first-order derivative of , β is the directional coefficient, v is the longitudinal speed of the unmanned agricultural machinery, l t is the wheelbase of the agricultural machinery, δ is the front wheel turning angle of the agricultural machinery, c r is the curvature of the reference path;

[0016] Step 1.2: Perform state conversion on the agricultural machinery path tracking model to obtain the vehicle system kinematic model, which is expressed as follows:

[0017]

[0018] Among them, x1 and x2 are both system states, and x1 represents the lateral deviation l od , that is, x1=l od ; Use x2 to represent the unknown state, that is, x2 = vsinθ od , and are the first-order derivatives of the system states x1 and x2, respectively; u is the input of the virtual controller, expressed as u = tanδ;

[0019] Step 1.3: Take the part containing state x2 as the uncertain part, and separate the uncertain part in the vehicle system kinematic model to obtain:

[0020]

[0021] In the formula, χ is the coefficient part of u after simplification, expressed as ψ is the uncertainty part including state x2, expressed as

[0022] Step 1.4: After separating the uncertain terms, the kinematic model of the vehicle system is transformed into a coordinate system to obtain an agricultural machinery path tracking model containing unknown states, which is expressed as:

[0023]

[0024] Among them, z1 represents the coordinate transformation of x1, which is recorded as z1=x1; z2 represents the ratio of x2 to the observer gain μ, which is recorded as z1(t) and z2(t) are the forms of z1 and z2 in continuous time respectively; z(t) is the matrix composed of z1(t) and z2(t), expressed as is the first-order derivative of z(t); is the state matrix of z(t), expressed as v(t) is the controller u and coefficient The product of J is the coefficient matrix of controller v, expressed as is the output matrix of the system, expressed as is the processed ψ, expressed as μ is the gain coefficient for The vector form of y(t) is the output of the system.

[0025] Furthermore, the construction method of the discrete-time observer is:

[0026] Step 2.1, first design the observer according to the lateral deviation, expressed as:

[0027]

[0028] Among them, t represents continuous time, t τ It represents a series of sampling points; and are the estimated values ​​of z1(t) and z2(t) respectively; and They are and The first-order derivative of; α1, α2 are the coefficients of the Hurwitz polynomial; v(t τ ) is the designed controller;

[0029] Simplifying the observer, we get:

[0030]

[0031] in, for and The constructed matrix is ​​expressed as D is the coefficient matrix composed of α1 and α2, expressed as

[0032] Step 2.2: Discretize the observer in step 2.1 to obtain a discrete-time observer, which is expressed as:

[0033]

[0034] in, and Both The coefficient of is z1(t τ ) coefficient.

[0035] Furthermore, the data sampling output feedback controller is expressed as:

[0036]

[0037] Among them, k1 and k2 are controller design parameters, and the matrix composed of them is expressed as

[0038] Furthermore, the process of tracking and controlling the agricultural machinery path is as follows:

[0039] Based on the data sampling output feedback controller output v(t τ ), according to v(t τ ) is calculated to get u, which is expressed as:

[0040]

[0041] Perform an inverse transformation on u=tanδ to obtain the front wheel steering angle of the agricultural machinery: δ=arctan(u); and track the path of the agricultural machinery according to the front wheel steering angle δ.

[0042] An agricultural machinery path tracking control system based on data sampling output feedback includes a path planning module mounted on the agricultural machinery, a data sampling controller, a discrete time observer, a data acquisition module and a communication module; the functions of each module are as follows:

[0043] The path planning module is used to output a reference path.

[0044] The data acquisition module is mounted on the agricultural machine and is used to collect the position and posture information of the agricultural machine;

[0045] The communication module is used to transmit the position information of the agricultural machinery;

[0046] The data sampling controller is designed based on a discrete time observer. The data sampling controller receives the lateral deviation between the reference path and the posture information, and outputs the actual control value of the front wheel steering angle of the agricultural machinery.

[0047] Furthermore, the data acquisition module includes a speed sensor, a position sensor, and a steering sensor to collect speed information, position information, and front wheel angle information of the agricultural machinery.

[0048] Furthermore, the communication module adopts the Beidou navigation system to achieve communication between the Beidou antenna, the radio receiver and the base station.

[0049] Beneficial effects of the present invention:

[0050] (1) The design of the present invention is to design a data sampling controller based on the data sampling control method; compared with the continuous-time controller, the data sampling controller has better adaptability to computer control systems. In addition, the controller designed based on continuous time is difficult to implement in programming, and the discrete expression form is also used in the programming process, so it is difficult to ensure the stability of the system. The data sampling controller model used in the present invention is a discrete expression form, which is more suitable for application scenarios with larger sampling periods and relatively rough equipment, and is easy to implement in digital control systems.

[0051] (2) The present invention can realize the observation of flight deviation, thereby reducing the number of required sensors and saving costs.

[0052] (3) When the present invention is actually used, only a few simple and cheap sensors mounted on agricultural machinery are needed to meet the requirements. The present invention is easy to operate and has good real-time performance and practicality. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 It is a schematic diagram of the control method of the present invention.

[0054] Figure 2 It is a schematic diagram of the vehicle kinematic model in the present invention.

[0055] Figure 3 It is the vehicle reference path curve diagram in the present invention.

[0056] Figure 4 This is the effect of the change of k1 on the path tracking effect in the present invention.

[0057] Figure 5 This is the effect of the change of k2 on the path tracking effect in the present invention.

[0058] Figure 6 It is a system structure block diagram of the present invention. DETAILED DESCRIPTION

[0059] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0060] The present invention proposes a method for controlling agricultural machinery path tracking based on data sampling output feedback, and its specific process is as follows: Figure 1 As shown, the following steps are included:

[0061] Step 1: Establish an agricultural machinery path tracking model including unknown states. The specific process is as follows:

[0062] Step 1.1: Combination Figure 2 The agricultural machinery path tracking model is established. The agricultural machinery path tracking model can be expressed as follows:

[0063]

[0064] Among them, l od is the lateral deflection, θ od For deviation, l od ,θ od The first-order derivative of , β is the directional coefficient, clockwise is defined as negative, v is the longitudinal speed of the unmanned agricultural machinery, l t is the wheelbase of the agricultural machinery, δ is the front wheel turning angle of the agricultural machinery, c r The curvature of the reference path is expressed as follows:

[0065]

[0066] Among them, t is the driving time of the agricultural machinery.

[0067] In order to facilitate path design, it is assumed that the agricultural machinery moves forward along the trajectory in a clockwise direction, that is, the direction coefficient β=-1, v>0.

[0068] Step 1.2: Perform state conversion on the above agricultural machinery path tracking model to obtain the vehicle system kinematic model, which is expressed as follows:

[0069]

[0070] Among them, x1 represents the horizontal deviation l od , that is, x1=l od ; Use x2 to represent the unknown state, that is, x2 = vsinθ od , x1 and x2 are both system states, and are the first-order derivatives of the system states x1 and x2 respectively; u is the input of the virtual controller, expressed as u=tanδ.

[0071] Step 1.3: The part containing state x2 is regarded as the uncertain part, so the uncertain part in the above vehicle system kinematic model is separated to obtain:

[0072]

[0073] In the formula, χ is the coefficient part of u after simplification, expressed as ψ is the uncertainty part of the state x2, which can be expressed as

[0074] Step 1.4: After separating the uncertain terms, the kinematic model of the vehicle system is transformed into a coordinate system to obtain an agricultural machinery path tracking model containing unknown states, which is expressed as:

[0075]

[0076] Among them, z1 represents the coordinate transformation of x1, which is recorded as z1=x1; z2 represents the ratio of x2 to the observer gain μ, which is recorded as z1(t) and z2(t) are the forms of z1 and z2 in continuous time respectively; z(t) is the matrix composed of z1(t) and z2(t), expressed as is the first-order derivative of z(t); is the state matrix of z(t), expressed as v(t) is the controller u and coefficient The product of J is the coefficient matrix of controller v, expressed as is the output matrix of the system, expressed as is the processed ψ, expressed as μ is the gain coefficient for The vector form of y(t) is the output of the system.

[0077] Step 2: Based on the unknown state in step 1, a discrete-time observer is designed to estimate the heading deviation.

[0078] Step 2.1, first design the observer according to the lateral deviation, expressed as:

[0079]

[0080] Among them, t represents continuous time, t τ It represents a series of sampling points. For example, if the operation time of agricultural machinery is 5 seconds and the sampling period is 1 second, then t τ It represents the five seconds 1, 2, 3, 4, and 5, and t represents tτ to τ+1 Any moment within a second.

[0081] and are the estimated values ​​of z1(t) and z2(t) respectively; and They are and The first-order derivative of; α1, α2 are the coefficients of the Hurwitz polynomial; v(t τ ) is the designed controller.

[0082] Simplifying it, we can get:

[0083]

[0084] in, Reason and The constructed matrix is ​​expressed as for The first derivative of ; is the state matrix of z(t); J is the coefficient matrix of controller v; D is the coefficient matrix composed of α1 and α2, expressed as

[0085] Step 2.2: Discretize the observer in step 2.1 to obtain a discrete-time observer, which is expressed as:

[0086]

[0087] in, and Both The coefficient of is z1(t τ ) coefficient.

[0088] Step 3: Using the vehicle state information estimated by the discrete-time observer, design a data sampling output feedback controller. The data sampling output feedback controller is expressed as:

[0089]

[0090] Among them, k1 and k2 are controller design parameters, and the matrix composed of them is expressed as

[0091] Step 4: Use the data sampling output feedback controller in step 3 to obtain the actual control amount of the front wheel angle of the agricultural machinery, and track the path of the agricultural machinery based on the control amount to achieve path tracking control of the agricultural machinery. The vehicle reference path curve is as follows: Figure 3 shown.

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

[0093] First, based on the data sampling output feedback controller output v(t τ ), according to v(t τ ) can be calculated to get u, which is expressed as:

[0094]

[0095] Perform inverse transformation on u = tanδ, and get the front wheel steering angle of the agricultural machinery: δ = arctan(u); track the path of the agricultural machinery according to the front wheel steering angle δ; during the path tracking control process of the agricultural machinery, its lateral deviation l od and yaw θ od will converge to 0.

[0096] An agricultural machinery path tracking control system based on data sampling output feedback includes a path planning module mounted on the agricultural machinery, a data sampling controller, a discrete time observer, a data acquisition module and a communication module; the functions of each module are as follows:

[0097] The path planning module is used to output a reference path.

[0098] The data acquisition module is mounted on the agricultural machinery and is used to collect the posture information of the agricultural machinery. Specifically, it includes a speed sensor, a position sensor, and a steering sensor to collect the speed information, position information, and front wheel angle information of the agricultural machinery.

[0099] The communication module adopts the Beidou navigation system, and specifically realizes communication between the Beidou antenna, the radio receiver and the base station, and is used to transmit the position information of the agricultural machinery.

[0100] The data sampling controller is designed based on a discrete time observer. The data sampling controller receives the lateral deviation between the reference path and the posture information, and outputs the actual control value of the front wheel steering angle of the agricultural machinery.

[0101] In order to better verify the control effect of the proposed data sampling output feedback path tracking algorithm, the present invention has carried out a simulation experiment based on Matlab, the simulation adopts the Euler method, and the sampling period is set to 0.01s. Under the simulation test of the present invention, the tracking effect and the influence of parameter changes on the tracking effect are shown in the figure. Figure 4 and Figure 5 As shown. Figure 4 and Figure 5 It is concluded that the control method designed by the present invention can achieve the goal of path tracking control, and the increase of k1 will reduce the convergence time of the system, and the increase of k2 will reduce the tracking error of the system.

[0102] The above embodiments are only used to illustrate the design ideas and features of the present invention, and their purpose is to enable those skilled in the art to understand the content of the present invention and implement it accordingly. The protection scope of the present invention is not limited to the above embodiments. Therefore, any equivalent changes or modifications made based on the principles and design ideas disclosed by the present invention are within the protection scope of the present invention.

Claims

1. A method for agricultural machinery path tracking control based on data sampling output feedback, characterized in that: The steps include: Step 1: Establish an agricultural machinery path tracking model including unknown states; Step 2: Based on the agricultural machinery path tracking model with unknown states established in step 1, a discrete time observer is designed, and the heading deviation is estimated using the discrete time observer; Step 3: Design a data sampling output feedback controller using the vehicle state information estimated by the discrete-time observer; Step 4: Utilize the data sampling output feedback controller in step 3 to obtain the actual control amount of the front wheel steering angle of the agricultural machinery, and track and control the path of the agricultural machinery based on the control amount.

2. The agricultural machinery path tracking control method based on data sampling output feedback according to claim 1 is characterized in that: The method for establishing the agricultural machinery path tracking model including unknown states in step 1 is: Step 1.1: Establish an agricultural machinery path tracking model, expressed as: Among them, l od is the lateral deflection, θ od For deviation, l od ,θ od The first-order derivative of , β is the directional coefficient, v is the longitudinal speed of the unmanned agricultural machinery, l t is the wheelbase of the agricultural machinery, δ is the front wheel turning angle of the agricultural machinery, c r is the curvature of the reference path; Step 1.2: Perform state conversion on the agricultural machinery path tracking model to obtain the vehicle system kinematic model, which is expressed as follows: Among them, x1 and x2 are both system states, and x1 represents the lateral deviation l od , that is, x1=l od ; Use x2 to represent the unknown state, that is, x2 = vsinθ od , and are the first-order derivatives of the system states x1 and x2, respectively; u is the input of the virtual controller, expressed as u = tanδ; Step 1.3: Take the part containing state x2 as the uncertain part, and separate the uncertain part in the vehicle system kinematic model to obtain: In the formula, χ is the coefficient part of u after simplification, expressed as ψ is the uncertainty part including state x2, expressed as Step 1.4: After separating the uncertain terms, the kinematic model of the vehicle system is transformed into a coordinate system to obtain an agricultural machinery path tracking model containing unknown states, which is expressed as: Among them, z1 represents the coordinate transformation of x1, which is recorded as z1=x1; z2 represents the ratio of x2 to the observer gain μ, which is recorded as z1(t) and z2(t) are the forms of z1 and z2 in continuous time respectively; z(t) is the matrix composed of z1(t) and z2(t), expressed as is the first-order derivative of z(t); is the state matrix of z(t), expressed as v(t) is the controller u and coefficient The product of J is the coefficient matrix of controller v, expressed as is the output matrix of the system, expressed as is the processed ψ, expressed as μ is the gain coefficient for The vector form of y(t) is the output of the system.

3. The agricultural machinery path tracking control method based on data sampling output feedback according to claim 2 is characterized in that: The construction method of the discrete-time observer is: Step 2.1, first design the observer according to the lateral deviation, expressed as: Among them, t represents continuous time, t τ It represents a series of sampling points; and are the estimated values ​​of z1(t) and z2(t) respectively; and They are and The first-order derivative of; α1, α2 are the coefficients of the Hurwitz polynomial; v(t τ ) is the designed controller; Simplifying the observer, we get: in, for and The constructed matrix is ​​expressed as D is the coefficient matrix composed of α1 and α2, expressed as Step 2.2: Discretize the observer in step 2.1 to obtain a discrete-time observer, which is expressed as: in, and Both The coefficient of is z1(t τ ) coefficient.

4. The agricultural machinery path tracking control method based on data sampling output feedback according to claim 1 is characterized in that: The sampled output feedback controller is expressed as: Among them, k1 and k2 are controller design parameters, and the matrix composed of them is expressed as 5. The agricultural machinery path tracking control method based on data sampling output feedback according to claim 1 is characterized in that: The process of tracking and controlling the path of agricultural machinery is as follows: Based on the data sampling output feedback controller output v(t τ ), according to v(t τ ) is calculated to get u, which is expressed as: Perform an inverse transformation on u=tanδ to obtain the front wheel steering angle of the agricultural machinery: δ=arctan(u); and track the path of the agricultural machinery according to the front wheel steering angle δ.

6. An agricultural machinery path tracking control system based on data sampling output feedback, characterized in that: The system is used to implement the agricultural machinery path tracking control method based on data sampling output feedback as described in claim 1, and includes a path planning module, a data sampling controller, a discrete time observer, a data acquisition module and a communication module mounted on the agricultural machinery; the functions of each module are as follows: The path planning module is used to output a reference path. The data acquisition module is mounted on the agricultural machine and is used to collect the position and posture information of the agricultural machine; The communication module is used to transmit the position information of the agricultural machinery; The data sampling controller is designed based on a discrete time observer. The data sampling controller receives the lateral deviation between the reference path and the posture information, and outputs the actual control value of the front wheel steering angle of the agricultural machinery.

7. The agricultural machinery path tracking control system based on data sampling output feedback according to claim 6, characterized in that: The data acquisition module includes a speed sensor, a position sensor, and a steering sensor to collect speed information, position information, and front wheel angle information of the agricultural machinery.

8. The agricultural machinery path tracking control system based on data sampling output feedback according to claim 6, characterized in that: The communication module adopts the Beidou navigation system and realizes communication between the Beidou antenna, the radio receiver and the base station.

Images