Unmanned agricultural machine finite time output feedback control method and control equipment under unknown course information

Abstract

The invention discloses an unmanned agricultural machine finite time output feedback control method and control equipment under unknown course information, and belongs to the field of unmanned agricultural machine path tracking control. According to the method, the unmanned agricultural machine can accurately track the reference path within limited time. The method mainly comprises the following steps: 1, based on an unmanned agricultural machine kinematics model, considering disturbance terms, constructing a deviation model, and further converting the deviation model into a state-space equation; 2, designing a state observer, and observing a state variable containing course information; and 3, based on the state observer, designing a finite time output feedback controller to realize path tracking control. The method has the advantages that firstly, disturbance is considered in the deviation model, and the accuracy of the model is improved; secondly, a state observer is constructed to observe a state variable containing course information, and the stability of the system is ensured when the course information cannot be measured; thirdly, a finite time output feedback path tracking control method is designed, and finite time convergence of tracking errors is ensured.

Description

Technical Field

[0001] The invention belongs to the field of path tracking control of unmanned agricultural machinery, and relates to a limited-time output feedback control method and control equipment of an unmanned agricultural machinery under unknown heading information. Background Art

[0002] Agricultural machinery is an important foundation for the development of modern agriculture and an important symbol of agricultural modernization. The degree of agricultural mechanization and automation is one of the important factors affecting agricultural production efficiency and productivity. It is worth mentioning that autonomous navigation technology is crucial in agricultural operation scenarios such as rotary tillage, planting, pesticide application, and harvesting. It mainly consists of three parts: farmland environment perception, operation task decision-making and path planning, and agricultural machinery path tracking control. Among them, high-precision and stable agricultural machinery path tracking control is a key technology that needs to be solved urgently.

[0003] The above technologies rely on reliable high-precision positioning systems and attitude sensors, as well as control algorithms that can achieve fast convergence and small steady-state errors. It is worth noting that 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 usually relies on the inertial measurement unit, which often has the problem of zero drift. Affected by the inertial measurement unit (IMU) bias and changes in input / output scale factors, the measurement error of the heading angle will increase with the accumulation of time, resulting in poor tracking accuracy and even instability, which is difficult to meet the needs of long-term and efficient operation of unmanned agricultural machinery. In addition, the cost of high-precision IMU / RTK combined systems is also very high. Therefore, it is very meaningful to design a state observer that can estimate the heading information.

[0004] In unstructured farmland environments, due to the uncertainty of agricultural machinery modeling and the unevenness of the road surface, there are internal and external disturbances in the agricultural machinery path tracking system. Existing path tracking control methods, such as PID control, Stanley control, fuzzy control, model predictive control, etc., are difficult to achieve finite time convergence, and the agricultural machinery path tracking system has high requirements for convergence time, so it is urgent to design a control method that can achieve finite time convergence.

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

[0006] In order to achieve finite-time convergence of the agricultural machinery path tracking control system when the heading information is unmeasurable, and at the same time have good anti-interference performance and steady-state performance, the present 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, disturbances related to lateral and heading deviations are introduced to construct the unmanned agricultural machinery deviation model, which is then converted into a state space equation;

[0008] Step 2: Considering the situation where the heading information is unmeasurable, a state observer is constructed to observe the state variables containing the heading information to improve 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 adaptation 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 apply u=ta to the controller. n δ performs inverse transformation to obtain the front wheel steering angle δ(t)=arctanu(t) required by the agricultural machinery;

[0012] Step 6: According to the CAN network protocol for controlling the agricultural machinery, a data transmission module is constructed to send the control signal of the system, including the speed V of the agricultural machinery and the front wheel steering angle δ of the agricultural machinery obtained in step 5, to the agricultural machinery vehicle controller through the CAN bus to realize the path tracking control of the agricultural machinery.

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

[0014]

[0015] Where P = [x, y] T is the position coordinate vector of the unmanned agricultural machinery; V is the longitudinal speed of the unmanned agricultural machinery; θ is the heading angle; L t is the wheelbase of the unmanned agricultural machine, that 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 machine;

[0016] The present invention provides the following definitions:

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

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

[0019] The specific formula is as follows:

[0020]

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

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

[0023]

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

[0025] Assume that the unmanned agricultural machine always moves forward and tracks the reference path in the clockwise direction, that is, V>0, λ=-1; then, based on the above deviation model, select the state variable x1=L os , x2=Vsinθ os +Δ1, construct the state space equation as follows:

[0026]

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

[0028] Furthermore, in step 2, considering the existence of working conditions such as occlusion or communication denial in the actual farmland environment, which will cause the heading information of the unmanned agricultural machinery to be unmeasurable, and thus make the state variable x2 containing the heading information unknown, so it is necessary to estimate the value of x2 by designing an observer. The state observer can be constructed as:

[0029]

[0030] Among them, z is an intermediate variable, K(x1) and N(x1) are both positive functions; is the estimated value of the state variable x2.

[0031] Furthermore, according to 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 guaranteed converges to zero in finite time.

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

[0033]

[0034] Among them, β1>0 is the control gain, β2(x1) is a positive function, g is the lower bound of the function g(t, x), satisfying 0< g <g(t,x); At this time, by constructing an output feedback controller, only the lateral deviation is used to achieve agricultural machinery path tracking control.

[0035] Furthermore, in step 4, in order to reduce the complexity of adjusting 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] Among them, β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 a positive value.

[0038] Furthermore, in step 5, the adaptive rate and the finite time output feedback control method are combined to obtain an adaptive finite time output feedback controller, which is in the following form:

[0039]

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

[0041] Finally, the controller u = tanδ is inversely transformed to obtain the front wheel steering angle δ(t) = arctanu(t) required by the agricultural machinery.

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

[0043] Beneficial effects of the present invention:

[0044] 1. The present invention considers the case where the heading information is unknown and constructs a state observer to estimate the state variables containing the heading information;

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

[0046] 3. Build a CAN bus control network for the control of agricultural machinery, and send CAN signals including vehicle speed and front wheel angle to the agricultural machinery in real time to achieve closed-loop control of the agricultural machinery path tracking system. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

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

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

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

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

[0053] Figure 7 Comparison diagram of lateral deviation response at different initial positions;

[0054] Figure 8 Comparison diagram of heading deviation response under different initial positions;

[0055] Fig. 9 Comparison diagram of lateral deviation response at different speeds;

[0056] Fig.10 Comparison of heading deviation responses at different speeds. DETAILED DESCRIPTION

[0057] The present invention will be further described below in conjunction with the accompanying drawings.

[0058] The following describes the implementation of the present invention through specific examples, and those skilled in the art can easily implement it according to the contents 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, the simulation parameters of the agricultural machinery path tracking system of the present invention are shown in Table 1.

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

[0061]

[0062]

[0063] A limited-time output feedback control method for an unmanned agricultural machine under unknown heading information. The implementation process of the method comprises the following steps:

[0064] 1. Based on Figure 1 The schematic diagram of the dynamic model of the agricultural machinery path tracking deviation is given. 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 is the position coordinate vector of the unmanned agricultural machine; V is the longitudinal speed of the unmanned agricultural machine; θ is the heading angle; L t is the wheelbase of the unmanned agricultural machine, that 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 machine;

[0067] Given the following definition:

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

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

[0070] The specific formula is as follows:

[0071]

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

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

[0074]

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

[0076] Assume that the unmanned agricultural machine always moves forward and tracks the reference path in the clockwise direction, that is, V>0, λ=-1; then, based on the above deviation model, select the state variable x1=L os , x2=Vsinθ os +Δ1, construct the state space equation as follows:

[0077]

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

[0079] 2. Considering the existence of working conditions such as occlusion or communication denial in the actual farmland environment, which will make the heading information of the unmanned agricultural machinery unmeasurable, and thus make the state variable x2 containing the heading information unknown, so it is necessary to estimate the value of x2 by designing an observer. The state observer can be constructed as:

[0080]

[0081] Among them, z is an intermediate variable, K(x1) and N(x1) are both positive functions; is the estimated value of the state variable x2.

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

[0083] 4. Estimated value based on state observer 2 Considering that farmland operations need to be quickly put online, there are high requirements for convergence time, such as Figure 2 Based on the control principle shown, the finite-time output feedback controller is constructed as follows:

[0084]

[0085] Among them, β1>0 is the adaptive control gain, β2(x1) is a positive function, g is the lower bound of the function g(t, x), and satisfies 0< g <g(t,x); At this time, by constructing an output feedback controller, only the lateral deviation is used to achieve agricultural machinery path tracking control.

[0086] 5. In order to reduce the complexity of adjusting parameters and improve the tracking effect, an adaptive rate is designed to design the control gain β1 as a time-varying gain β1(t), which has the following form:

[0087]

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

[0089] 6. Combining the adaptive rate with the finite-time output feedback controller, we get an adaptive finite-time output feedback controller, which is in the following form:

[0090]

[0091] Among them, β1(t)>0 is the adaptive control gain; in addition, the controller u=tanδ is inversely transformed to obtain the front wheel steering angle δ(t)=arctanu(t) required by the agricultural machinery.

[0092] 7. According to the specific CAN network protocol of unmanned agricultural machinery control, a data sending 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 the CAN communication message matching the unmanned agricultural machinery is designed; using the data sending module, the system control signal, including the speed V of the agricultural machinery and the actual front wheel steering angle δ obtained in 6, is sent to the agricultural machinery vehicle controller through the CAN bus to realize the path tracking control of the agricultural machinery.

[0093] Example:

[0094] The design is verified by the following simulation results:

[0095] First, considering that the agricultural machinery mainly has straight-line driving and line-changing driving processes in the actual operation process, a reference path Ω consisting of a straight line and a semicircle is selected to simulate the actual driving path of the agricultural machinery, which is given as follows:

[0096]

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

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

[0099] The first part is that under the control method of the present invention, the initial state error of the agricultural machinery is selected as (L os (0), θ os The simulation results are as follows: Figure 3-6 shown.

[0100] Figure 3 This is a tracking trajectory diagram under the finite time output feedback control method of the present invention. It can be seen 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 They are lateral deviation diagram and heading deviation diagram respectively. From the simulation deviation 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 the heading error can converge to zero quickly, and can remain near zero in the presence of disturbances.

[0102] Figure 6 This is the front wheel angle diagram. It can be seen that the front wheel angle is always kept within a reasonable range.

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

[0104] Figure 7 This is a comparison chart of lateral deviation; Figure 8 It can be seen from the simulation results that under different initial conditions, the control method of the present invention has good stability and anti-disturbance performance, and the lateral error and heading error can converge to zero quickly and remain near zero in the presence of disturbances.

[0105] The third part is that under the control method of the present invention, the initial lateral deviation of the agricultural machinery is selected as L os (0) = 0.5, the initial heading deviation is selected as θ os The simulation results are shown in Figure 2 when (0) = 0 and the speed is selected as V = 2, 3, and 4 m / s. Figure 9-10 shown.

[0106] Fig. 9 This is a comparison chart of lateral deviation; Fig.10 The comparison diagram of heading deviation. From the simulation results, it can be seen that the control method of the present invention has good stability and anti-interference performance at different driving speeds of agricultural machinery. Both the lateral error and the heading error can converge to zero quickly and can remain near zero in the presence of disturbances.

[0107] Based on the above control method, an embodiment of the present invention also proposes a control device for unmanned agricultural machinery, which includes an information processing module, a data receiving module and a data sending module, which is specifically completed by an STM32F4 single-chip microcomputer. Based on the unmanned agricultural machinery position information received by the data receiving module from the Beidou positioning system, the information processing module can implement the contents of the above steps 1-6, and the data sending module can implement the contents of the above step 7, thereby realizing the whole vehicle control of the unmanned agricultural machinery.

[0108] The series of detailed descriptions listed above are only specific descriptions of feasible implementation methods of the present invention. They are not intended to limit the scope of protection of the present invention. All equivalent methods or changes that do not deviate from the technical creation of the present invention should be included in the scope of protection of the present invention.

Claims

1. A limited-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, disturbances related to lateral deviation and heading deviation are introduced to construct the unmanned agricultural machinery deviation model, which is then converted into a state space equation; Step 2: Considering the situation where the heading information is unmeasurable, a state observer is constructed to observe the state variables containing the heading information; 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; Step 4: Design the controller parameter adaptation rate; Step 5: Combining the adaptive rate with the finite time output feedback controller to obtain an adaptive finite time output feedback controller, and performing an inverse transformation on the controller to obtain the front wheel steering angle required by the agricultural machine; Step 6: Send the control signal, including the speed of the agricultural machinery and the front wheel steering 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 method for controlling an unmanned agricultural machine with limited time output feedback under unknown heading information according to claim 1 is characterized in that: In step 1, the unmanned agricultural machine is front-wheel steering and rear-wheel drive, and its kinematic model is as follows: Where P = [x, y] T is the position coordinate vector of the unmanned agricultural machine; V is the longitudinal speed of the unmanned agricultural machine; θ is the heading angle; L t 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; δ is the front wheel steering angle of the unmanned agricultural machinery.

3. The method for controlling an unmanned agricultural machine with limited time output feedback under unknown heading information according to claim 2 is characterized in that: In step 1, the lateral deviation L os , defined as the distance between the current position of the agricultural machine and the nearest point on the reference path; the heading deviation θ os , defined as the difference between the current heading angle of the agricultural machine and the desired heading angle; The specific formula is as follows: Among them, P d =[x d ,y d ] T is the position coordinate vector of the reference point; P t =[x t ,y t ] T is the current position coordinate vector of the agricultural machinery; θ t is the current heading angle of the agricultural machinery; θ d is the desired heading angle.

4. The method for controlling an unmanned agricultural machine with limited time output feedback under unknown heading information according to claim 3 is characterized in that: In step 1, the unmanned agricultural machinery deviation model is established as: Where λ is the steering coefficient; Δ1 and Δ2 are disturbances related to lateral deviation and heading deviation, respectively; Assuming that the agricultural machine always moves forward and tracks the reference path in the clockwise direction, that is, V>0, λ=-1, based on the above deviation model, the state variable x1=L os , x2=Vsinθ os +Δ1, construct the state space equation as follows: Where x = [x1, x2] T is the state vector of the agricultural machinery path tracking system, u=tanδ is the control variable, represents the lumped disturbance.

5. The method for controlling an unmanned agricultural machine with limited time output feedback under unknown heading information according to claim 4 is characterized in that: In step 2, the state observer can be constructed as: Among them, z is an intermediate variable, K(x1) and N(x1) are both positive functions; is the estimated value of the state variable x2. The values ​​of the functions K(x1) and N(x1) in the state observer must satisfy Under the above relationship, the observation error can be guaranteed converges to zero in finite time.

6. The method for controlling an unmanned agricultural machine with limited time output feedback under unknown heading information according to claim 5 is characterized in that: In step 3, the finite time output feedback controller is designed as follows: Among them, β1>0 is the control gain, β2(x1) is a positive function, g is the lower bound of the function g(t,x), and satisfies 0<g< g (t, x); At this time, by constructing an output feedback controller, only the lateral deviation is used to achieve agricultural machinery path tracking control.

7. The method for controlling an unmanned agricultural machine with limited time output feedback under unknown heading information according to claim 6 is characterized in that: In step 4, the adaptation rate is designed as follows: The control gain β1 is designed as a time-varying gain β1(t), which is as follows: Among them, β1(0)>0, which is the initial value of the time-varying gain β1(t), k>0, ξ>0, μ>0, which are auxiliary variables, and the values ​​of ξ and μ are small. The purpose of introducing μ is to ensure that the adaptive parameter β1(t) is a positive value.

8. The method for controlling an unmanned agricultural machine with limited time output feedback under unknown heading information according to claim 7, characterized in that: In step 5, the adaptive finite-time output feedback controller is designed as follows: Among them, β1(t)>0 is the adaptive control gain; By performing an inverse transformation on the controller u=tanδ, the front wheel steering angle δ(t)=arctanu(t) required by the agricultural machinery is obtained.

9. The method for controlling an unmanned agricultural machine with limited time output feedback under unknown heading information according to claim 1, characterized in that: The specific implementation of step 6 includes the following: The CAN bus ID uses the extended ID, the CAN format uses the Intel format, and the CAN communication message is designed to match the unmanned agricultural machinery. The control signal (including the agricultural machinery speed V and the front wheel steering angle δ obtained in step 5) is sent to the agricultural machinery vehicle controller through the CAN bus using the data sending module to realize the path tracking control of the agricultural machinery.

10. 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 unmanned agricultural machinery location information received by the data receiving module from the Beidou positioning system, the information processing module can implement the contents of steps 1-5 described in any one of claims 1-9; the data sending module can implement the contents of step 6 described in any one of claims 1-9.

Images