A linear path tracking anti-disturbance control method and control system for wheeled agricultural machinery

Abstract

The present invention belongs to the technical field of automatic driving control of agricultural machinery, and specifically relates to a straight path tracking anti-disturbance control method and control system for wheeled agricultural machinery. By acquiring vehicle parameters, a third-order straight path tracking linearization model containing disturbances is constructed; vehicle posture information is acquired and the deviation information of the vehicle is obtained in combination with a preset path; the deviation information is transitionally adjusted; a linear extended state observer based on an iterative step is designed according to the third-order straight path tracking linearization model containing disturbances to observe the vehicle state and the total disturbance; a single-parameter adjustment path tracking controller with disturbance feedforward compensation is designed according to the observation value of the linear extended state observer; the problems of poor operation quality, slow online speed and easy overshoot in traditional methods under disturbance conditions are solved; a single-parameter adjustment linear anti-disturbance controller is adopted to greatly simplify the parameter setting process while ensuring the robust performance of the controller.

Description

Technical Field

[0001] The present invention belongs to the technical field of automatic driving control of agricultural machinery, and in particular relates to a straight path tracking anti-disturbance control method and control system for a wheeled agricultural machinery. Background Art

[0002] With the development of technologies such as satellite positioning, laser radar and inertial navigation, autonomous driving agricultural machinery is gradually being promoted and applied. The main task of autonomous driving of agricultural machinery is to make agricultural machinery drive along the preset path through sensor positioning perception and vehicle control algorithm. In the agricultural field, the links of "ploughing, planting, management and harvesting" will greatly relieve manual pressure and improve productivity.

[0003] At present, the path tracking control methods of agricultural machinery include path tracking control methods based on kinematic models, path tracking control methods based on dynamic models, and control methods that are independent of models. Luo Xiwen et al. designed a navigation controller using a PID algorithm and verified the accuracy and reliability of the system through field tests, but this method requires the adjustment of multiple sets of control parameters. Connor et al. designed a path tracking optimal controller based on a kinematic model and an optimal quadratic form (LQR). Li Taochang et al. used a pure tracking model method combined with a fuzzy control method to conduct tests on a rice transplanter platform. The results showed that the system can quickly track the preset path but there is an overshoot phenomenon.

[0004] At present, the PID control method meets the needs to a certain extent, but there are problems with the adjustment of multiple sets of control parameters and poor anti-disturbance performance. The method lacks solutions to the problems of rapidity and overshoot, resulting in overshoot under large initial deviations and difficulty in quickly going online. The optimal control algorithm combined with the kinematic model or the dynamic model often uses complex nonlinear models for derivation, resulting in a more complex design of the control algorithm. At the same time, due to the complexity of field operation conditions, it will make it difficult to establish the kinematic model and dynamic model of agricultural machinery. At the same time, most current methods require full state feedback, which will increase production costs.

[0005] Therefore, based on the above technical problems, it is necessary to design a new straight path tracking anti-disturbance control method and control system for wheeled agricultural machinery. Summary of the invention

[0006] The object of the present invention is to provide a straight path tracking anti-disturbance control method and control system for a wheeled agricultural machine.

[0007] In order to solve the above technical problems, the present invention provides a straight path tracking anti-disturbance control method for a wheeled agricultural machine, comprising:

[0008] Obtain the parameters of the vehicle and construct a third-order linear path tracking model with disturbances;

[0009] Obtain vehicle posture information and obtain vehicle deviation information based on the preset path;

[0010] Transition adjustment of deviation information;

[0011] According to the third-order straight line path tracking linearization model with disturbance, a linear extended state observer based on iterative step is designed to observe the vehicle state and total disturbance.

[0012] A single-parameter adjustable path tracking controller with disturbance feedforward compensation is designed based on the observations of the linear extended state observer.

[0013] Further, the obtaining of vehicle parameters includes:

[0014] Get the vehicle operating speed and wheelbase length.

[0015] Further, the construction of a third-order straight line path tracking linearization model containing disturbances includes:

[0016] The kinematic model of agricultural machinery linear path tracking is:

[0017]

[0018] Where L is the vehicle wheelbase, ψ is the vehicle heading angle, V x is the vehicle's forward speed, x is the vehicle's position along the path, and y is the lateral deviation.

[0019] Further, expand at the equilibrium point (0, 0) and select Then the third-order linear path tracking model with disturbance is obtained:

[0020]

[0021] Where d(t) represents the internal and external disturbances of the system.

[0022] Further, the obtaining of the vehicle posture information and obtaining the deviation information of the vehicle in combination with the preset path includes:

[0023] The navigation module obtains the vehicle's position information based on the on-board sensors and obtains the vehicle's deviation information based on the preset path.

[0024] Further, the transition adjustment of the deviation information includes:

[0025] A linear differential tracker is designed to perform transition adjustment on the deviation information command from the navigation module, coordinate the contradiction between rapidity and overshoot, and avoid overshoot in the case of large initial deviation.

[0026] Further, the linear differential tracker comprises:

[0027]

[0028] Among them, r0 is the given input signal of the navigation module; r i＝(1，2，3，4) is the differential of each order of the deviation command signal; R is the tracking speed adjustment factor;

[0029] A linear differential tracker is used to perform transition adjustment on the deviation command sent by the navigation module, and at the same time, differential signals of various orders of the input signal are obtained.

[0030] Further, the design of a linear extended state observer based on an iterative step size according to a third-order straight line path tracking linearization model containing disturbances to observe the vehicle state and the total disturbance includes:

[0031]

[0032] Where h is the sampling step length; z i＝(1，2，3) is the vehicle status; x i＝(1，2，3) is the observed value; z4 is the observed value of internal disturbance and external disturbance during the operation; is the controller gain; u is the control input.

[0033] Further, the design of a single parameter adjustment path tracking controller with disturbance feedforward compensation according to the observation value of the linear extended state observer includes:

[0034] u=k1e1+k2e2+k3e3+k4e4;

[0035] Among them, e i =r i -z i , i=1,2,3,4; k i , i=1,2,3,4 represents the controller gain;

[0036] Based on the parameter bandwidth theory, let where ω c Represents the bandwidth of the controller, thereby achieving single parameter adjustment of the controller.

[0037] On the other hand, the present invention further provides a control system using the above-mentioned straight path tracking anti-disturbance control method for a wheeled agricultural machine, comprising:

[0038] A posture sensor, wherein the posture sensor is suitable for collecting the operating speed of the vehicle and the current heading angle of the vehicle;

[0039] An angle sensor, wherein the angle sensor is suitable for obtaining a steering wheel angle of the vehicle;

[0040] A navigation control module, wherein the navigation control module is adapted to generate navigation path points according to the field geographic information, and obtain the deviation information of the vehicle according to the vehicle posture information and in combination with the preset path;

[0041] A linear differential controller, the linear differential controller being adapted to perform transition adjustment on the deviation information instruction from the navigation module;

[0042] A linear extended state observer, wherein the linear extended state observer is suitable for observing the vehicle state and the total disturbance based on the linear extended state observer of the iteration step;

[0043] A linear active disturbance rejection controller is provided, wherein the linear active disturbance rejection controller contains a single parameter adjustment of disturbance feedforward compensation.

[0044] The beneficial effects of the present invention are as follows: the present invention constructs a third-order straight path tracking linearization model containing disturbances by acquiring vehicle parameters; acquires vehicle posture information and obtains vehicle deviation information in combination with a preset path; performs transition adjustment on the deviation information; designs a linear extended state observer based on an iterative step size according to the third-order straight path tracking linearization model containing disturbances to observe vehicle states and total disturbances; designs a single-parameter adjustment path tracking controller containing disturbance feedforward compensation according to the observed value of the linear extended state observer; solves the problems of poor operation quality, slow online speed and easy overshoot of traditional methods under disturbance conditions, and designs a straight path tracking control system for agricultural machinery with fast online speed, strong robustness and high tracking accuracy based on the controller; in view of the problem that traditional linear and nonlinear controllers are complex in design and need to adjust multiple sets of control parameters, the design steps of a straight path tracking anti-disturbance control method for wheeled agricultural machinery based on high-order anti-disturbance control theory are disclosed, and a single-parameter adjustment linear anti-disturbance controller is adopted to greatly simplify the parameter adjustment process while ensuring the robust performance of the controller.

[0045] Other features and advantages of the present invention will be described in the following description, and partly become apparent from the description, or understood by practicing the present invention. The purpose and other advantages of the present invention are realized and obtained by the structures particularly pointed out in the description and the drawings.

[0046] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0048] Figure 1 is a flow chart of a straight path tracking anti-disturbance control method for a wheeled agricultural machine of the present invention;

[0049] Figure 2 It is a schematic diagram of the anti-disturbance control framework of the agricultural machinery straight path tracking of the present invention;

[0050] Figure 3 It is a model diagram of the agricultural machinery straight path tracking anti-disturbance control of the present invention;

[0051] Figure 4 It is a simulation result diagram of the present invention. DETAILED DESCRIPTION

[0052] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0053] Example 1

[0054] like Figures 1 to 4As shown, this embodiment 1 provides a straight path tracking anti-disturbance control method for a wheeled agricultural machinery, including: step 1, obtaining vehicle parameters, and constructing a third-order straight path tracking linearization model containing disturbances; step 2, obtaining vehicle posture information and obtaining vehicle deviation information in combination with a preset path; step 3, performing transition adjustment on the deviation information; step 4, designing a linear extended state observer based on an iterative step size according to the third-order straight path tracking linearization model containing disturbances to observe the vehicle state and the total disturbance; step 5, designing a single parameter regulator containing disturbance feedforward compensation according to the observation value of the linear extended state observer The invention discloses a linear path tracking controller for wheeled agricultural machinery based on high-order anti-disturbance control theory, solves the problems of poor operation quality, slow online speed and easy overshoot in traditional methods under disturbance conditions, and designs a linear path tracking control system for agricultural machinery with fast online speed, strong robustness and high tracking accuracy based on the controller; in view of the problem that traditional linear and nonlinear controllers are complex in design and need to adjust multiple sets of control parameters, the design steps of the anti-disturbance control method for linear path tracking of wheeled agricultural machinery based on high-order anti-disturbance control theory are disclosed, and a single-parameter adjustment linear anti-disturbance control controller is adopted to greatly simplify the parameter adjustment process while ensuring the robust performance of the controller.

[0055] In this embodiment, the acquisition of vehicle parameters includes: acquiring the vehicle operating speed and wheelbase length, and establishing a third-order straight line path tracking linearization model containing disturbances based on the balance point.

[0056] In this embodiment, the construction of a third-order linear path tracking model containing disturbances includes: the kinematic model of the linear path tracking of agricultural machinery is:

[0057]

[0058] Where L is the vehicle wheelbase (m), ψ is the vehicle heading angle, V x is the vehicle's forward speed (m / s), x is the vehicle's position along the path (m), and y is the lateral deviation (m).

[0059] In this embodiment, the purpose of the straight path tracking control of agricultural machinery is to ensure that y→0 and ψ→0 at the same time. Therefore, it is only necessary to consider the kinematic equations of y and ψ and the internal and external disturbances during the operation process, and expand them at the equilibrium point (0, 0). Then the third-order linear path tracking model with disturbance is obtained:

[0060]

[0061] Where d(t) represents the internal and external disturbances of the system.

[0062] In this embodiment, the acquiring of the vehicle posture information and obtaining the deviation information of the vehicle in combination with the preset path includes: the navigation module acquiring the vehicle posture information according to the vehicle-mounted sensor and obtaining the deviation information of the vehicle in combination with the preset path.

[0063] In this embodiment, the transition adjustment of the deviation information includes: designing a linear differential tracker to perform transition adjustment on the deviation information instruction from the navigation module, coordinating the contradiction between rapidity and overshoot, and avoiding overshoot in the case of large initial deviation.

[0064] In this embodiment, the linear differential tracker includes: in view of the fact that the conventional control method directly obtains the deviation signal when the initial deviation is large, overshoot may occur easily, resulting in a slow online speed;

[0065]

[0066] Among them, r0 is the given input signal of the navigation module; r i＝(1，2，3，4) is the differential of the deviation command signal; R is the tracking speed adjustment factor; Formula (3) is used to perform transition adjustment on the deviation command sent by the navigation module, and the differential signals of the input signal are obtained at the same time.

[0067] In this embodiment, the design of a linear extended state observer based on an iterative step size according to a third-order straight line path tracking linearization model containing disturbances to observe the vehicle state and the total disturbance includes: in order to solve the cost increase problem caused by full-state feedback control, a linear extended state observer is used to estimate the vehicle state and disturbance during the operation process:

[0068]

[0069] Where h is the sampling step length; z i＝(1，2，3) is the vehicle status; x i＝(1，2，3) is the observed value; z4 is the observed value of internal disturbance and external disturbance during the operation; is the controller gain; u is the control input.

[0070] In this embodiment, the design of a single-parameter adjustable path tracking controller with disturbance feedforward compensation according to the observation value of the linear extended state observer includes: to achieve disturbance compensation and state feedback, a third-order single-parameter linear active disturbance rejection controller is designed:

[0071] u=k1e1+k2e2+k3e3+k4e4;

[0072] Among them, e i =r i -z i , i=1,2,3,4; k i, i=1,2,3,4 represents the controller gain;

[0073] Based on the parameter bandwidth theory, let where ω c It represents the bandwidth of the controller, thus realizing the single parameter adjustment of the controller, greatly reducing the difficulty of controller parameter tuning and facilitating the implementation of digital controller.

[0074] This embodiment uses MATLAB simulation tool to compare the method of the present invention with the full state feedback control method. Set the simulated agricultural machinery body wheelbase L = 1.5m, the operating speed V x =1m / s. The simulation results are as follows Figure 4 As shown, the left side is the result without applying disturbance. By comparison, it is found that the lateral deviation and heading deviation of the method in this embodiment converge quickly to the equilibrium point, while the traditional state feedback control method takes a long time to go online. The right side is the simulation result after applying disturbance. The results show that this method has a significant effect on suppressing disturbances, and the steady-state error accuracy is significantly better than the traditional control method.

[0075] Example 2

[0076] On the basis of Example 1, this Example 2 further provides a control system using the linear path tracking anti-disturbance control method of the wheeled agricultural machinery in Example 1, including: a posture sensor, wherein the posture sensor is suitable for collecting and obtaining the operating speed V of the vehicle x , and the current heading angle ψ of the vehicle, measuring the wheelbase length L of the agricultural machinery to establish a third-order straight line path tracking linearization model containing disturbances; an angle sensor, the angle sensor is suitable for obtaining the steering wheel angle of the vehicle; a navigation control module, the navigation control module is suitable for generating navigation path points according to the geographic information of the field, and obtaining the deviation information (x, y) of the vehicle according to the vehicle posture information and in combination with the preset path; a linear differential controller, the linear differential controller is suitable for transition adjustment of the deviation information instruction from the navigation module; a linear extended state observer, the linear extended state observer is suitable for observing the vehicle state and the total disturbance based on the iterative step size; a linear active disturbance rejection controller, the linear active disturbance rejection controller contains a single parameter adjustment of disturbance feedforward compensation; the posture sensor can obtain the vehicle's operating speed V x, and the current heading angle ψ of the vehicle. The wheelbase length L of the agricultural machinery is measured to establish a third-order linear path tracking linearization model containing disturbances. The navigation module obtains the vehicle posture information based on the on-board posture sensor and obtains the vehicle deviation information (x, y) in combination with the preset path, and passes this deviation information instruction to the linear differential tracker for transition adjustment. The linear extended state observer based on the iterative step observes the vehicle state and the total disturbance. The obtained observation values ​​are used for feedforward compensation of the linear active disturbance rejection controller. Finally, the output of the controller acts on the vehicle operation model to realize the active disturbance rejection function of the agricultural machinery linear tracking control.

[0077] In summary, the present invention obtains vehicle parameters to construct a third-order straight path tracking linearization model with disturbances; obtains vehicle posture information and obtains vehicle deviation information in combination with a preset path; performs transition adjustment on the deviation information; designs a linear extended state observer based on an iterative step according to the third-order straight path tracking linearization model with disturbances to observe the vehicle state and the total disturbance; designs a single-parameter adjustment path tracking controller with disturbance feedforward compensation according to the observation value of the linear extended state observer; solves the problems of poor operation quality, slow online speed and easy overshoot in the case of disturbances in traditional methods, and designs a straight path tracking control system for agricultural machinery with fast online speed, strong robustness and high tracking accuracy based on the controller; in view of the problem that traditional linear and nonlinear controllers are complex in design and need to adjust multiple sets of control parameters, the design steps of a straight path tracking anti-disturbance control method for wheeled agricultural machinery based on high-order anti-disturbance control theory are disclosed, and a single-parameter adjustment linear anti-disturbance controller is adopted to greatly simplify the parameter adjustment process while ensuring the robust performance of the controller.

[0078] In several embodiments provided in the present application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present invention. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0079] In addition, the functional modules in the various embodiments of the present invention may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

[0080] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0081] Based on the above ideal embodiments of the present invention, the relevant staff can make various changes and modifications without departing from the technical concept of the present invention through the above description. The technical scope of the present invention is not limited to the contents of the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A straight path tracking anti-disturbance control method for a wheeled agricultural machine, characterized in that: include: Obtain the parameters of the vehicle and construct a third-order linear path tracking model with disturbances; Obtain vehicle posture information and obtain vehicle deviation information based on the preset path; Transition adjustment of deviation information; According to the third-order straight line path tracking linearization model with disturbance, a linear extended state observer based on iterative step is designed to observe the vehicle state and total disturbance. A single-parameter adjustable path tracking controller with disturbance feedforward compensation is designed based on the observed values ​​of the linear extended state observer. The obtaining of vehicle parameters includes: Obtain vehicle operating speed and wheelbase length; The construction of a third-order straight line path tracking linearization model containing disturbances includes: The kinematic model of agricultural machinery linear path tracking is: Where L is the vehicle wheelbase, ψ is the vehicle heading angle, V x is the vehicle's forward speed, x is the vehicle's position along the path, and y is the lateral deviation; Expand at the equilibrium point (0, 0) and choose Then the third-order linear path tracking model with disturbance is obtained: Where d(t) represents the internal and external disturbances of the system; The transition adjustment of the deviation information includes: Design a linear differential tracker to perform transition adjustment on the deviation information command from the navigation module, coordinate the contradiction between rapidity and overshoot, and avoid overshoot in the case of large initial deviation; The linear differential tracker comprises: Among them, r0 is the given input signal of the navigation module; r i =(1, 2, 3, 4) are the differentials of the deviation command signal; R is the tracking speed adjustment factor; A linear differential tracker is used to perform transition adjustment on the deviation command sent by the navigation module, and at the same time, differential signals of various orders of the input signal are obtained; The design of a linear extended state observer based on an iterative step length according to a third-order straight line path tracking linearization model containing disturbance to observe the vehicle state and the total disturbance includes: Where h is the sampling step length; z i＝(1，2,3 ) is the vehicle status; x i＝(1，2，3) is the observed value; z4 is the observed value of internal disturbance and external disturbance during the operation; is the controller gain; u is the control input; The design of a single parameter adjustment path tracking controller with disturbance feedforward compensation according to the observation value of the linear extended state observer comprises: u=k1e1+k2e2+k3e3+k4e4; Among them, e i =r i -z i , i=1,2,3,4; k i , i=1,2,3,4 represents the controller gain; Based on the parameter bandwidth theory, let where w c Represents the bandwidth of the controller, thereby achieving single parameter adjustment of the controller.

2. The linear path tracking anti-disturbance control method for a wheeled agricultural machine according to claim 1, characterized in that: The step of acquiring the vehicle posture information and obtaining the vehicle deviation information in combination with the preset path includes: The navigation module obtains the vehicle's position information based on the on-board sensors and obtains the vehicle's deviation information based on the preset path.

3. A control system using the linear path tracking anti-disturbance control method of a wheeled agricultural machine as claimed in claim 1, characterized in that: include: A posture sensor, wherein the posture sensor is suitable for collecting the operating speed of the vehicle and the current heading angle of the vehicle; An angle sensor, wherein the angle sensor is suitable for obtaining a steering wheel angle of the vehicle; A navigation control module, wherein the navigation control module is adapted to generate navigation path points according to the field geographic information, and obtain the deviation information of the vehicle according to the vehicle posture information and in combination with the preset path; A linear differential controller, the linear differential controller being adapted to perform transition adjustment on the deviation information instruction from the navigation module; A linear extended state observer, wherein the linear extended state observer is suitable for observing the vehicle state and the total disturbance based on the linear extended state observer of the iteration step; A linear active disturbance rejection controller is provided, wherein the linear active disturbance rejection controller contains a single parameter adjustment of disturbance feedforward compensation.

Images