A Multi-Agricultural-Machine Cooperative Operation Trajectory Tracking Control Method

Abstract

The present invention discloses a multi - agricultural - machine collaborative operation trajectory tracking control method. A multi - agricultural - machine collaborative operation sliding - mode trajectory tracking controller is designed based on the total repulsive force function of the agricultural machines, the estimated value of the agricultural - machine - matching non - linear disturbance, the estimated value of the agricultural - machine - non - matching non - linear disturbance, and the position and speed errors of trajectory tracking. According to the inverse kinematic model of the agricultural machines, the acceleration to be controlled by the agricultural machines is converted into the agricultural - machine chassis control parameters, and the sliding - mode trajectory tracking controllers of each agricultural machine control the agricultural machines to complete collaborative operations according to the planned operation trajectories based on the above - mentioned control parameters. The present invention realizes high - precision trajectory tracking control for multi - agricultural - machine collaborative operations under the constraint of the planned trajectory, and at the same time avoids the collision risks between machines and obstacles.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle trajectory tracking control, and particularly relates to a multi-agricultural machinery cooperative operation trajectory tracking control method. Background Technique

[0002] With the gradual maturity and application of the agricultural machinery automatic driving technology based on the satellite navigation system, the automation level and working efficiency of agricultural machinery have been greatly improved. In recent years, the agriculture in China has shown an intensive and large-scale development trend, and new concepts such as "intelligent agriculture" and "unmanned farm" have emerged. And due to the limitation of the short operation time window (tight stubble coordination), it is often necessary for multiple identical or different agricultural machines to work continuously in the farmland at the same time. Therefore, the cooperative formation operation control of multiple agricultural machines in the farmland has become a new research direction.

[0003] Since the farmland environment is usually very harsh, there are also errors in the mechanical structure and control system, and the trajectory tracking control system of agricultural machinery is affected by various disturbances most of the time, such as parameter uncertainties, unmodeled dynamics, and external load disturbances, etc. These various disturbances can be classified as matched nonlinear disturbances and unmatched nonlinear disturbances. For the matched nonlinear disturbances and unmatched nonlinear disturbances in the agricultural machinery operation system, relevant scholars have proposed methods such as finite-time disturbance observers and sliding mode observers to estimate such disturbances simultaneously, but these methods are all realized based on the real-time detected position and speed information of agricultural machinery. The position information of agricultural machinery can be detected by the satellite navigation system, but the speed information of agricultural machinery (especially tracked vehicles) is generally difficult to be accurately detected by sensors. Therefore, these methods are currently only limited to simulation analysis or research on wheeled vehicles. And in the design of agricultural machinery trajectory tracking control, usually only the error between the position information of agricultural machinery and the planned trajectory is considered to realize trajectory tracking control. If higher dynamic and static performance is to be obtained, the speed information of agricultural machinery also needs to be obtained in real time during trajectory tracking. In addition, there are static and dynamic obstacles in the farmland, and collisions may also occur between agricultural machines. Therefore, based on the position information of the multi-agricultural machinery operation system, a trajectory tracking control method that can simultaneously meet the functions of multi-agricultural machinery cooperative formation, disturbance estimation and compensation, obstacle avoidance (collision avoidance), etc. is developed. Summary of the Invention

[0004] Aiming at the deficiencies existing in the prior art, the present invention provides a multi-agricultural machinery cooperative operation trajectory tracking control method, which solves the problems such as low accuracy of agricultural machinery trajectory tracking control and collision risk in the case of multi-agricultural machinery cooperative operation.

[0005] The present invention realizes the above technical purpose through the following technical means.

[0006] A multi-agricultural machinery cooperative operation trajectory tracking control method:

[0007] Design a multi - agricultural - machine cooperative operation sliding - mode trajectory tracking controller based on the total repulsive force function of the agricultural machine, the estimated value of the agricultural - machine - matching nonlinear perturbation, the estimated value of the agricultural - machine - mismatching nonlinear perturbation, and the position and velocity errors of trajectory tracking:

[0008]

[0009] Where: a represents the acceleration that the agricultural machine needs to control, S represents the sliding - mode surface of the multi - agricultural - machine cooperative operation system, and represents the set of local position tracking errors and δ x = x - x d , δ x represents the position deviation of all agricultural machines, x represents the position of the agricultural machine, x d represents the planned reference trajectory of the agricultural machine, L represents the Laplacian matrix of the communication topology directed graph among multiple agricultural machines in the multi - agricultural - machine cooperative operation system, B is a non - negative diagonal matrix with at least one positive element, I n is the n - dimensional identity matrix, represents the Kronecker inner product, represents the set of local velocity tracking errors and δ v represents the velocity deviation of all agricultural machines, represents the planned reference velocity of the agricultural machine, is the estimated value of the agricultural - machine velocity v, represents the set of local mismatching nonlinear perturbations, is the estimated value of the agricultural - machine mismatching nonlinear perturbation d; λ = diag{λ1, λ2,..., λ n} represents a parameter with all elements greater than 0, and λ1, λ2,..., λ n respectively represent the parameters greater than 0 selected for constructing the sliding - mode surface in the trajectory tracking controllers of the 1st to nth agricultural machines; the vector ξ = [ζ1, ζ2,..., ζ n T , the vector ξ = [ξ1, ζ2,..., ξ n T , sgn() represents the sign function, represents the planned reference acceleration of the agricultural machine, is the estimated value of the agricultural - machine - matching nonlinear perturbation w, is the first - order derivative of, and F represents the total repulsive force function of the agricultural machine;

[0010] According to the inverse kinematic model of the agricultural machine, convert the α into the control parameters of the agricultural - machine chassis, and the sliding - mode trajectory tracking controllers of each agricultural machine control the agricultural machine to complete cooperative operations according to the control parameters along the planned operation trajectory.

[0011] ​​Further technical solution, the total repulsive force function of the agricultural machine Among them, the total repulsive force function of the i-th agricultural machine is expressed as F i =[f i T , 0] T , and n is the number of agricultural machines, n o is the number of obstacles, F Mij represents the repulsive force function between the i-th agricultural machine and the j-th agricultural machine, F Oik represents the repulsive force function between the i-th agricultural machine and the k-th obstacle.

[0012] Even further technical solution, the repulsive force function between the i-th agricultural machine and the j-th agricultural machine is:

[0013]

[0014] Among them: R ij represents the regional radius within which the i-th agricultural machine can detect the presence of the j-th agricultural machine, r ij represents the minimum safety distance between the i-th agricultural machine and the j-th agricultural machine, p mi is the position coordinate of the i-th agricultural machine, p mj is the position coordinate of the j-th agricultural machine, ||p mi -p mj || represents the distance between the i-th agricultural machine and the j-th agricultural machine.

[0015] Even further technical solution, the repulsive force function between the i-th agricultural machine and the k-th obstacle is:

[0016]

[0017] Among them: R ik represents the regional radius within which the i-th agricultural machine can detect the presence of the k-th obstacle, r ik represents the minimum safety distance between the i-th agricultural machine and the k-th obstacle, p ok is the position coordinate of the k-th obstacle, ||p mi -p ok || represents the distance between the i-th agricultural machine and the k-th obstacle.

[0018] Further technical solution, the estimated value of the agricultural machine matching the non-linear disturbance satisfies:

[0019]

[0020] Among them: is the estimated value of the agricultural machine position x, μ, α1, α2, and α3 are all parameters in the extended state observer, is The first derivative of is The first derivative of is The first derivative of

[0021] For a further technical solution, the estimated value of the unmatched non - linear perturbation of the agricultural machinery satisfies:

[0022]

[0023] Where: is the estimated value of the auxiliary variable n d and β is the tuning parameter. is The first derivative of

[0024] For a further technical solution, considering the matched non - linear perturbation and the unmatched non - linear perturbation, the dynamic model of multi - agricultural - machinery cooperative operation is expressed as:

[0025]

[0026] Where: is the first derivative of x, is the first derivative of v.

[0027] For a further technical solution, the Laplacian matrix \(L = D - A\), where \(A=[a ij \) represents the adjacency matrix with weights, and the degree matrix \(D = diag\{d m1 ,d m2 ,\cdots,d mn \}\).

[0028] The beneficial effects of the present invention are as follows: Based only on the position information of the agricultural machinery, the present invention respectively realizes the estimation of the matched and unmatched non - linear perturbations and the estimation of the operation speed information of the agricultural machinery through a cascaded observer; constructs a repulsive force function between agricultural machinery and agricultural machinery, and between agricultural machinery and obstacles through the artificial potential field method, and designs a multi - agricultural - machinery cooperative operation sliding - mode trajectory tracking controller based on the position and speed deviations of the agricultural machinery from the planned trajectory according to the position information of the agricultural machinery and the estimated speed information, the total repulsive force function of the agricultural machinery, the estimated value of the matched non - linear perturbation of the agricultural machinery, the estimated value of the unmatched non - linear perturbation of the agricultural machinery, and the position and speed errors of trajectory tracking, and compensates the estimated perturbation value into it, realizing high - precision trajectory tracking control of multi - agricultural - machinery cooperative operation under the constraint of the planned trajectory, and at the same time avoiding the collision risk between machines and obstacles; the present invention solves the problem of trajectory tracking control for multi - agricultural - machinery cooperative operation, and has the advantages of strong environmental adaptability, high reliability, good stability, etc. Description of the Drawings

[0029] Figure 1 It is the trajectory tracking control block diagram of the i-th agricultural machine in the multi-agricultural machine collaborative operation system of the present invention;

[0030] Figure 2 It is the communication topology directed graph of 3 crawler combine harvesters in the present invention;

[0031] Figure 3 It is the operation trajectory diagram of the collaborative operation system of 3 crawler combine harvesters planned in the present invention;

[0032] Figure 4 It is the schematic diagram of the headland alternating steering of 3 crawler combine harvesters in the present invention;

[0033] Figure 5 It is the planar motion sketch of the i-th crawler combine harvester in the present invention;

[0034] Figure 6 It is the tracking control trajectory diagram of 3 crawler combine harvesters in the present invention;

[0035] Figure 7(a) is the left and right track control speed diagram of crawler combine harvester 1 in the present invention;

[0036] Figure 7(b) is the left and right track control speed diagram of crawler combine harvester 2 in the present invention;

[0037] Figure 7(c) is the left and right track control speed diagram of crawler combine harvester 3 in the present invention;

[0038] Figure 8(a) is the trajectory tracking error diagram of crawler combine harvester 1 in the present invention;

[0039] Figure 8(b) is the trajectory tracking error diagram of crawler combine harvester 2 in the present invention;

[0040] Figure 8(c) is the trajectory tracking error diagram of crawler combine harvester 3 in the present invention;

[0041] Figure 9(a) is the cumulative trajectory tracking error diagram of crawler combine harvester 1 before and after disturbance compensation in the present invention;

[0042] Figure 9(b) is the cumulative trajectory tracking error diagram of crawler combine harvester 2 before and after disturbance compensation in the present invention;

[0043] Figure 9(c) is the cumulative trajectory tracking error diagram of crawler combine harvester 3 before and after disturbance compensation in the present invention;

[0044] Figure 10(a) is the matching nonlinear disturbance estimation result diagram of crawler combine harvester 1 in the present invention;

[0045] Figure 10(b) shows the matching non - linear disturbance estimation result diagram of the crawler combine harvester 2 in the present invention;

[0046] Figure 10(c) shows the matching non - linear disturbance estimation result diagram of the crawler combine harvester 3 in the present invention;

[0047] Figure 11(a) shows the non - matching non - linear disturbance estimation result diagram of the crawler combine harvester 1 in the present invention;

[0048] Figure 11(b) shows the non - matching non - linear disturbance estimation result diagram of the crawler combine harvester 2 in the present invention;

[0049] Figure 11(c) shows the non - matching non - linear disturbance estimation result diagram of the crawler combine harvester 3 in the present invention. Detailed implementation manners

[0050] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, but the protection scope of the present invention is not limited thereto.

[0051] The trajectory tracking control block diagram of the i - th agricultural machine in the multi - agricultural - machine collaborative operation system is as Figure 1 shown, Figure 1 The meanings of the parameters in di are as follows: x represents the reference trajectory of the i - th agricultural machine planned, ok represents the reference speed of the i - th agricultural machine planned; p ok represents the position information of the k - th obstacle, r i represents the position of the i - th agricultural machine, represents the speed of the i - th agricultural machine estimated by the cascade observer of the present invention; represents the matching non - linear disturbance of the i - th agricultural machine estimated by the cascade observer of the present invention, represents the non - matching non - linear disturbance of the i - th agricultural machine estimated by the cascade observer of the present invention; δ xi represents the position deviation of the i - th agricultural machine, δ vi represents the speed deviation of the i - th agricultural machine, δ x1 ,.., δ xi-1 , δ xi+1 ,...,, δ xn represents the position deviations of other agricultural machines except the i - th agricultural machine, δ v1 ,...,, δ vi-1 , δ vi+1 ,...,, δ vn represents the speed deviations of other agricultural machines except the i - th agricultural machine, represents the estimated value of the non - matching non - linear disturbance of other agricultural machines except the i - th agricultural machine; e xi represents the local position tracking error of the i - th agricultural machine, evi denotes the local speed tracking error of the \(i\)-th agricultural machine, denotes the estimated local mismatched nonlinear disturbance of the \(i\)-th agricultural machine; \(F\) i denotes the total repulsive force function of the \(i\)-th agricultural machine, \(a\) i denotes the acceleration that the \(i\)-th agricultural machine needs to control output by the sliding mode trajectory tracking controller.

[0052] A method for trajectory tracking control of multi-agricultural machine cooperative operation includes the following steps:

[0053] Step (1), constructing the total repulsive force function of the agricultural machine: Based on the artificial potential field method, establish the repulsive force potential field functions between agricultural machines and between agricultural machines and obstacles respectively, and take the negative gradient of them to obtain the corresponding repulsive force functions, and merge the repulsive force functions between agricultural machines and between agricultural machines and obstacles, and add the heading dimension to obtain the total repulsive force function of the agricultural machine.

[0054] Based on the artificial potential field method, establish the repulsive force potential field functions between the \(i\)-th agricultural machine and the \(j\)-th agricultural machine, and between the \(i\)-th agricultural machine and the \(k\)-th obstacle, and take the negative gradient of them to obtain the corresponding repulsive force functions, specifically:

[0055]

[0056]

[0057] where, \(p\) mi = [x xi , x yi T is the position coordinate of the \(i\)-th agricultural machine, \(x\) xi represents the position coordinate of the \(i\)-th agricultural machine in the \(x\)-axis direction, \(x\) yi represents the position coordinate of the \(i\)-th agricultural machine in the \(y\)-axis direction; \(p\) mj = [x xj , x yj T is the position coordinate of the \(j\)-th agricultural machine, \(x\) xj represents the position coordinate of the \(j\)-th agricultural machine in the \(x\)-axis direction, \(x\) yj represents the position coordinate of the \(j\)-th agricultural machine in the \(y\)-axis direction; \(p\) ok = [x xk , x yk T is the position coordinate of the \(k\)-th obstacle, \(x\) xk represents the position coordinate of the \(k\)-th obstacle in the \(x\)-axis direction, \(x\) yk represents the position coordinate of the \(k\)-th obstacle in the \(y\)-axis direction; \(\vert\vert p\) mi - p mj \vert\vert\) represents the distance between the \(i\)-th agricultural machine and the \(j\)-th agricultural machine, \(\vert\vert p\) mi - p​​​ok || represents the distance between the i-th agricultural machine and the k-th obstacle; r ij represents the minimum safe distance between the i-th agricultural machine and the j-th agricultural machine (which needs to be greater than the operation radius of the j-th agricultural machine), r ik represents the minimum safe distance between the i-th agricultural machine and the k-th obstacle (which needs to be greater than the radius r ok ) of the k-th obstacle; R ij represents the regional radius within which the i-th agricultural machine can detect the presence of the j-th agricultural machine, R ik represents the regional radius within which the i-th agricultural machine can detect the presence of the k-th obstacle; F Mij represents the repulsive force function between the i-th agricultural machine and the j-th agricultural machine, FO ik represents the repulsive force function between the i-th agricultural machine and the k-th obstacle; the superscript T in the formula represents the transpose of the vector.

[0058] Assuming that the repulsive force function of the i-th agricultural machine is 0 in the course of heading latitude, the total repulsive force function of the i-th agricultural machine can be expressed as:

[0059] F i = [f i T , 0]T (3)

[0060] Among them, n is the number of agricultural machines, n o is the number of obstacles.

[0061] Therefore, the total repulsive force function of n agricultural machines

[0062] Step (2), construct the dynamic model of multi-agricultural machine collaborative operation: regard each agricultural machine in the collaborative operation as a massless point, and consider the existing matching and non-matching nonlinear disturbances in the system, and use the second-order dynamic model to describe the state of the agricultural machines in the collaborative operation system.

[0063] In the collaborative operation, considering the existing matching and non-matching nonlinear disturbances during the operation of the i-th agricultural machine and regarding it as a massless point, the second-order dynamic model of the i-th agricultural machine can be expressed as:

[0064]

[0065] Among them, the pose vector x i = [x xi , x yi , θ hi T θ hi represents the heading of the i-th agricultural machine; the velocity vector v i = [v xi ​, v yi , ω hi T , v xi represents the velocity of the i-th agricultural machine in the x-axis direction, v yi represents the velocity of the i-th agricultural machine in the y-axis direction, ω hi represents the angular velocity of the i-th agricultural machine; the acceleration vector a of the i-th agricultural machine i = [a xi , a yi , α hi T , a xi represents the acceleration of the i-th agricultural machine in the x-axis direction, a yi represents the acceleration of the i-th agricultural machine in the y-axis direction, α hi represents the angular acceleration of the i-th agricultural machine; the mismatched non-linear disturbance vector d i = [d xi , d yi , d hi T, d xi represents the mismatched non-linear disturbance of the i-th agricultural machine in the x-axis direction, d yi represents the mismatched non-linear disturbance of the i-th agricultural machine in the y-axis direction, d hi represents the mismatched non-linear disturbance of the angular velocity of the i-th agricultural machine; the matched non-linear disturbance vector w i = [w xi , w yi , w hi T , w xi represents the matched non-linear disturbance of the i-th agricultural machine in the x-axis direction, w yi represents the matched non-linear disturbance of the i-th agricultural machine in the y-axis direction, w hi represents the matched non-linear disturbance of the angular acceleration of the i-th agricultural machine.

[0066] Therefore, the dynamic model of multi-agricultural machine collaborative operation can be expressed as:

[0067]

[0068] Among them, the position of the agricultural machine The speed of the agricultural machine The acceleration of the agricultural machine Mismatched non-linear disturbance Matched non-linear disturbance

[0069] ​​​Step (3), construct a cascaded observer for the matching and mismatching nonlinear disturbances of agricultural machinery: Input the agricultural machinery speed information estimated by the extended state observer into the nonlinear disturbance observer as known parameters, and then estimate the mismatching nonlinear disturbance in the agricultural machinery collaborative operation system through the constructed nonlinear disturbance observer; similarly, input the mismatching nonlinear disturbance estimated by the nonlinear disturbance observer into the extended state observer as known parameters, and then estimate the matching nonlinear disturbance and the agricultural machinery speed information in the agricultural machinery collaborative operation system through the constructed extended state observer.

[0070] The nonlinear disturbance observer for the mismatching nonlinear disturbance d can be expressed as:

[0071]

[0072] where, is the estimated value of the agricultural machinery speed v estimated by the following constructed extended state observer, is the estimated value of the auxiliary variable n d of, is the estimated value of the agricultural machinery mismatching nonlinear disturbance d, and β is a tuning parameter and greater than 0.

[0073] Taking the matching nonlinear disturbance w as the extended state variable, an extended state observer for the matching nonlinear disturbance can be constructed according to the dynamic model of the agricultural machinery collaborative operation system in Equation (5):

[0074]

[0075] where, is the estimated value of the agricultural machinery mismatching nonlinear disturbance d estimated by the above constructed nonlinear disturbance observer, is the estimated value of the agricultural machinery position x, is the estimated value of the agricultural machinery speed v, is the estimated value of the agricultural machinery matching nonlinear disturbance w, and the parameters μ, α1, α2, and α3 in the extended state observer are all greater than 0.

[0076] Step (4), construct a multi-agricultural machinery collaborative operation trajectory tracking controller: Based on the multi-agent theory, construct the Laplacian matrix of the agricultural machinery collaborative operation system, and design an observer-based multi-agricultural machinery collaborative operation sliding mode trajectory tracking controller according to the repulsive force function, the estimated values of the matching and mismatching nonlinear disturbances, and the position and speed errors of the trajectory tracking.

[0077] Assume that the multi-agricultural machinery collaborative operation system consists of n agricultural machineries, and the communication topology digraph G = {V, E, A} among them, where V = {υ1, υ2,..., υ n} represents the set of all nodes (i.e., agricultural machineries), Denote the set of all edges (i.e., the communication links between agricultural machines), and \(A = [a_{ij}]\) represents the adjacency matrix with weights. The degree matrix of graph \(G\) can be expressed as \(D=\text{diag}\{d m1 ,d m2 ,\cdots,d mn \}\), where the element The Laplacian matrix \(L\) of graph \(G\) is \(L = D - A\).

[0078] According to the sliding mode variable structure theory, the sliding surface of the multi - agricultural - machine cooperative operation system is selected as:

[0079]

[0080] where, denotes the Kronecker inner product, \(I n \) is the \(n\) - dimensional identity matrix, \(S\) represents the sliding surface of the multi - agricultural - machine cooperative operation system; \(\lambda=\text{diag}\{\lambda_1,\lambda_2,\cdots,\lambda n \}\) represents the parameter with all elements greater than 0, and \(\lambda_1,\lambda_2,\cdots,\lambda n \) respectively represent the parameters greater than 0 selected for constructing the sliding surface in the trajectory tracking controllers of the 1st to \(n\)th agricultural machines; denotes the set of local position tracking errors and \(\delta x =x - x d \) (\(\delta x \) represents the position deviation of all agricultural machines), \(x d \) represents the planned reference trajectory of the agricultural machine, denotes the set of local velocity tracking errors and \) (\(\delta v \) represents the velocity deviation of all agricultural machines), represents the planned reference velocity of the agricultural machine, denotes the set of estimated local mismatched nonlinear disturbances, \(B\) is a non - negative diagonal matrix with at least one positive element (\(B=\text{diag}\{b_1,b_2,\cdots,b n \}\), \(b n \) is a user - defined parameter).

[0081] Select the following exponential reaching law:

[0082]

[0083] where, \(\text{sgn}()\) represents the sign function, \(\xi=[\zeta_1,\zeta_2,\cdots,\zeta n T ,\xi=[\xi_1,\xi_2,\cdots,\xi n T , and all elements in vector \(\zeta\) and vector \(\xi\) are greater than 0.

[0084] ​​Based on the non - linear disturbance estimation value of the combined cascade observer (non - linear disturbance observer + extended state observer) and the repulsive force function for collision avoidance and obstacle avoidance, the sliding - mode trajectory tracking controller for multi - agricultural - machine cooperative operation based on the observer is designed as follows:

[0085]

[0086] Among them, represents the planned reference acceleration of the agricultural machine.

[0087] According to the type of agricultural machine, the corresponding inverse kinematics model is established. The acceleration control quantity shown in Equation (10) can be converted into the corresponding agricultural - machine chassis control parameters (for example, for a tracked chassis, it is the speeds of the left and right tracks; for a wheeled chassis, it is the speed and the front - wheel steering angle). Then, the sliding - mode trajectory tracking controller of each agricultural machine controls the agricultural machine to complete the cooperative operation according to the planned operation trajectory according to the control parameters (the specific process is the prior art).

[0088] Next, the effectiveness of the multi - agricultural - machine cooperative operation trajectory tracking control method of the present invention is verified through simulation.

[0089] Suppose there are 3 tracked combine harvesters with the same structural parameters in the rectangular farmland cooperative operation system. The communication topology directed graph of the 3 tracked combine harvesters is as Figure 2 shown. The cutting width w of the tracked combine harvester h = 2m, the distance between the centers of the two tracks w b = 1.5m, and the speed range of the left and right tracks is - 1.5 to 1.5m / s. The planned operation trajectory of the tracked combine harvester cooperative operation system is as Figure 3 shown, and the head - land turning of the tracked combine harvester is realized by the way of alternating turning, as Figure 4 shown.

[0090] The plane motion sketch of the i - th tracked combine harvester in the global XOY coordinate system is as Figure 5 shown. xoy is the local coordinate system fixed on the tracked combine harvester, the x - axis is the forward direction of the tracked combine harvester, and θ hi is the heading angle of the i - th tracked combine harvester. In the global XOY coordinate system, the inverse motion model of the i - th tracked combine harvester without considering the influence of slip factors can be expressed as:

[0091]

[0092] Among them, N -1 represents the inverse matrix of matrix N, M + represents the Moore - Penrose generalized inverse matrix of matrix M, ai represents the acceleration control quantity output by the sliding mode trajectory tracking controller of the i-th agricultural machine, is the integral of the acceleration a i with respect to time; v Li represents the speed of the left track of the agricultural machine, v Ri represents the speed of the right track of the agricultural machine.

[0093] The initial states of 3 tracked combine harvesters are respectively x 10 = [3, 14, π / 2] T , x 20 = [3, 11, π / 2] T and x 30 = [3, 8, π / 2] T , assuming that the mismatched nonlinear disturbance and matched nonlinear disturbance of the i-th tracked combine harvester are as shown in formulas (12) and (13). The cascade observer (formulas (6) and (7)) described in the present invention is used to estimate the matched and mismatched nonlinear disturbances of the tracked combine harvester, and in order to ensure the stability of the system, the estimated value of the cascade observer is limited to -0.1 to 0.1.

[0094]

[0095]

[0096] The trajectory tracking control results of the cooperative operation system composed of 3 tracked combine harvesters are as Figure 6 shown. The actual operation trajectory of tracked combine harvester 1 is represented by a dotted line, the actual operation trajectory of tracked combine harvester 2 is represented by a dash-dotted line, the actual operation trajectory of tracked combine harvester 3 is represented by a dashed line, the planned operation trajectories of 3 tracked combine harvesters are all represented by solid lines, and the moving trajectory of the dynamic obstacle is represented by a dashed line. It can be seen from this that the multi-agricultural machine cooperative operation trajectory tracking control method of the present invention can achieve the trajectory tracking control of a group of 3 tracked combine harvesters, and can successfully avoid dynamic obstacles and static obstacles in the farmland environment. In addition, the tracking trajectories of tracked combine harvester 1 (dotted line) and tracked combine harvester 3 (dashed line) will deviate from the planned trajectory during the turning process at the field head. This is because during the turning process at the field head, the distance between tracked combine harvester 1 and tracked combine harvester 3 is too close, and the anti-collision repulsive force function will make them move away from each other. The cooperative operation trajectory tracking control, obstacle avoidance and anti-collision objectives of the present invention can be successfully achieved simultaneously. The left and right track control speeds corresponding to 3 tracked combine harvesters are shown in Figures 7(a), (b), (c). The trajectory tracking errors of the cooperative operation system of 3 tracked combine harvesters are shown in Figures 8(a), (b), (c).

[0097] The cumulative trajectory tracking errors of the trajectory tracking control of the cooperative operation system of three crawler combine harvesters before and after disturbance compensation are shown in Figures 9(a), (b), and (c). It can be seen from this that during the straight-line driving stage, the cumulative tracking error of the trajectory tracking control without disturbance compensation will gradually increase; on the contrary, except during the anti-collision and obstacle avoidance processes, the cumulative tracking error after disturbance compensation hardly increases, indicating that the method of the present invention has better trajectory tracking control performance. The matching non-linear disturbances and non-matching non-linear disturbances estimated by the cascade observer described in the present invention for the three crawler combine harvesters are shown in Figures 10(a), (b), (c) and 11(a), (b), (c) respectively, indicating that the cascade observer of the present invention can successfully estimate the matching and non-matching non-linear disturbances.

[0098] The described embodiments are the preferred embodiments of the present invention, but the present invention is not limited to the above embodiments. Without departing from the essential content of the present invention, any obvious improvements, substitutions or variations that those skilled in the art can make all fall within the protection scope of the present invention.

Claims

1. A multi - agricultural - machine cooperative operation trajectory tracking control method, characterized in that: Based on the total repulsive force function of the agricultural machines, the estimated value of the agricultural - machine - matching non - linear disturbance, the estimated value of the agricultural - machine - mismatching non - linear disturbance, and the position and velocity errors of the trajectory tracking, a multi - agricultural - machine cooperative operation sliding - mode trajectory tracking controller is designed: Where: a represents the acceleration that the agricultural machinery needs to control, S represents the sliding surface of the multi - agricultural - machinery collaborative operation system, and represents the set of local position tracking errors and δ x = x - x d , δ x represents the position deviation of all agricultural machinery, x represents the position of the agricultural machinery, and x d represents the planned reference trajectory of the agricultural machinery. L represents the Laplacian matrix of the communication topology directed graph among multiple agricultural machinery in the multi - agricultural - machinery collaborative operation system. B is a non - negative diagonal matrix with at least one positive element. I n is the n - dimensional identity matrix, represents the Kronecker inner product, represents the set of local velocity tracking errors and δ v represents the velocity deviation of all agricultural machinery, represents the planned reference velocity of the agricultural machinery, is the estimated value of the agricultural machinery velocity v, represents the set of local mismatched non - linear disturbances, is the estimated value of the agricultural machinery mismatched non - linear disturbance d; λ = diag{λ1, λ2, …, λ n} represents a parameter with all elements greater than 0. λ1, λ2, …, λ n respectively represent the parameters greater than 0 selected for constructing the sliding surface in the trajectory tracking controllers of the 1st to nth agricultural machinery; the vector ζ = [ζ1, ζ2, …, ζ n T , the vector ξ = [ξ1, ξ2, …, ξ n T , sgn() represents the sign function, represents the planned reference acceleration of the agricultural machinery, is the estimated value of the agricultural machinery matched non - linear disturbance w, is the first - order derivative of, and F represents the total repulsive force function of the agricultural machinery;​​ According to the inverse kinematic model of the agricultural machine, the a is converted into the agricultural - machine chassis control parameters, and the sliding - mode trajectory tracking controllers of each agricultural machine control the agricultural machines to complete the cooperative operation according to the planned operation trajectory based on the control parameters.

2. The multi - agricultural - machine cooperative operation trajectory tracking control method according to claim 1, characterized in that The total repulsive force function of the agricultural machinery Among them, the total repulsive force function of the i-th agricultural machinery is expressed as F i =[f i T , 0] T , and n is the number of agricultural machinery, n o is the number of obstacles, F Mij represents the repulsive force function between the i-th agricultural machinery and the j-th agricultural machinery, F Oik represents the repulsive force function between the i-th agricultural machinery and the k-th obstacle.

3. The multi - agricultural - machine collaborative operation trajectory tracking control method according to claim 2, characterized in that, The repulsive force function between the i - th agricultural machine and the j - th agricultural machine is: Where: R ij represents the area radius within which the i-th agricultural machine can detect the presence of the j-th agricultural machine, r ij represents the minimum safety distance between the i-th agricultural machine and the j-th agricultural machine, p mi is the position coordinate of the i-th agricultural machine, p mj is the position coordinate of the j-th agricultural machine, ||p mi -p mj || represents the distance between the i-th agricultural machine and the j-th agricultural machine.

4. The multi-agricultural machine collaborative operation trajectory tracking control method according to claim 3, wherein, The repulsive force function between the i - th agricultural machine and the k - th obstacle is: Where: R ik represents the area radius within which the i-th agricultural machine can detect the presence of the k-th obstacle, r ik represents the minimum safety distance between the i-th agricultural machine and the k-th obstacle, p ok is the position coordinate of the k-th obstacle, ||p mi -p ok || represents the distance between the i-th agricultural machine and the k-th obstacle.

5. The multi-agricultural machine collaborative operation trajectory tracking control method according to claim 1, wherein, The estimated value of the agricultural - machine - matching non - linear disturbance satisfies: Wherein: is the estimated value of the position x of the agricultural machinery, and μ, α1, α2, and α3 are all parameters in the extended state observer, is the first derivative of, is the first derivative of, is the first derivative of.

6. The multi - agricultural - machine collaborative operation trajectory tracking control method according to claim 1, wherein, The estimated value of the agricultural - machine - mismatching non - linear disturbance satisfies: Wherein: is the estimated value of the auxiliary variable n, β is the tuning parameter, d and is the first derivative of.

7. The multi - agricultural - machine collaborative operation trajectory tracking control method according to claim 1, wherein, Considering the matching non - linear disturbance and the mismatching non - linear disturbance, the dynamic model of the multi - agricultural - machine cooperative operation is expressed as: Wherein: is the first derivative of x, is the first derivative of v.

8. The multi - agricultural - machine collaborative operation trajectory tracking control method according to claim 1, characterized in that, The Laplacian matrix L = D - A, where A = [a ij represents the adjacency matrix with weights, and the degree matrix D = diag{d m1 , d m2 , …, d mn},

Images