Trajectory tracking control method of four-wheel independent drive 4WID self-steering high-clearance sprayer

Abstract

The invention discloses a trajectory tracking control method for a four-wheel independent drive 4WID self-steering high-ground-clearance sprayer, and the method mainly comprises the following four steps: 1, employing a highly-integrated high-precision integrated navigation system for the 4WID self-steering high-ground-clearance sprayer in the embodiment, receiving the differential data of a reference station, and carrying out the tracking control of the four-wheel independent drive 4WID self-steering high-ground-clearance sprayer; real-time carrier phase differential positioning can be realized, and centimeter-level high-precision position information is provided for the spraying machine; 2, aiming at a special walking chassis of the 4WID self-steering high-ground-clearance spraying machine, establishing a kinematic model of the special walking chassis and simplifying the kinematic model into a two-wheeled vehicle model; and step 3, establishing a pose error model of trajectory tracking on the basis of the kinematic model of the two-wheeled vehicle, and gradually designing a trajectory tracking control law of the sprayer on the basis of a backstepping control method. According to the method, a Backstepping nonlinear control method is adopted, the transverse deviation, the longitudinal deviation and the course angle can be accurately controlled at the same time, and the path tracking precision of the spraying machine in the operation process is remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the field of automatic navigation control of agricultural plant protection machinery, and in particular relates to a trajectory tracking control method of a four-wheel independently driven 4WID self-steering high-ground clearance sprayer, belonging to the field of automatic navigation of agricultural machinery. Background Art

[0002] With the continuous development of modern agriculture, automated navigation technology for agricultural machinery has been widely used in agricultural production processes such as sowing, fertilizing, spraying, and harvesting. In automated sprayer navigation systems, the two core technologies are sprayer posture measurement and navigation trajectory tracking control. Current sprayer navigation systems generally use path tracking control algorithms that only consider lateral control, enabling independent navigation of agricultural machinery. However, with the increasing development of collaborative navigation operations involving multiple agricultural machinery, lateral control is no longer the sole requirement. To ensure a safe distance between agricultural machinery during collaborative operations, longitudinal control is essential. Trajectory tracking control ensures high lateral and longitudinal control accuracy, ensuring the safety of multi-machine collaborative operations and significantly improving agricultural production efficiency, playing a significant role in achieving intelligent agriculture.

[0003] As a nonholonomic system, the sprayer kinematic model cannot be designed using a stable static feedback law. However, the Backstepping control method, an important nonlinear control technique, is not only a widely used control method but also a cutting-edge topic in adaptive control theory and application. The Backstepping control method offers unique advantages in addressing nonlinear control problems. Its basic concept is to decompose a complex nonlinear system into subsystems that do not exceed the system order. Partial Lyapunov functions are then designed for each subsystem, ensuring a certain degree of convergence. A virtual control law is then obtained for each subsystem. In the design of the next subsystem, the virtual control law of the previous subsystem is used as the tracking target for this subsystem. Similar to the design of the previous subsystem, the virtual control law for this subsystem is obtained; and so on, ultimately, the actual control law for the entire closed-loop system is obtained. Summary of the Invention

[0004] The sprayer navigation trajectory tracking control method primarily involves a control approach based on the sprayer's kinematic model. Due to the complexity of the sprayer model, the uncertainty of the operating environment, and the sprayer load, the sprayer navigation control system is a complex and uncertain system. To address this issue, a backstepping control method is used to gradually design control laws for lateral deviation and longitudinal deviation-heading deviation. Virtual control laws are designed for each subsystem individually to ensure convergence, ultimately yielding the actual control law for the entire system.

[0005] The purpose of the present invention is to overcome the problem that current path tracking control cannot ensure that multiple agricultural machines maintain a safe longitudinal distance when operating, and to provide a 4WID (Four-Wheel Independent Drive, 4WID) self-steering high-clearance sprayer trajectory tracking control method with simple design method and strong robustness.

[0006] The purpose of the present invention is achieved through the following technical solutions:

[0007] The present invention discloses a trajectory tracking control method for a four-wheel independent drive (4WID) self-steering high-ground clearance sprayer, comprising the following steps:

[0008] Step 1: The 4WID self-steering high-clearance sprayer uses a highly integrated, high-precision combined navigation system. By receiving differential data from the base station, it can achieve real-time carrier phase differential positioning, providing centimeter-level high-precision position information for the sprayer.

[0009] Step 2: For the special walking chassis of the 4WID self-steering high-ground clearance sprayer, establish its kinematic model and simplify it into a two-wheeled vehicle model.

[0010] In step 2.1, the global coordinate system and the vehicle coordinate system are first established. The sprayer kinematic model is established based on geometric principles. To simplify the design of the trajectory tracking control algorithm, the sprayer kinematic model is simplified as follows:

[0011]

[0012] Step 3: Based on the kinematic model of the two-wheeled vehicle, a posture error model for trajectory tracking is established and the trajectory tracking control law of the sprayer is gradually designed based on the Backstepping method.

[0013] Step 3.1, in the sprayer posture error diagram, L is the wheelbase of the sprayer chassis; D is the track width of the sprayer chassis; δ is the steering angle of the front and rear steering axes; O is the center of mass of the sprayer chassis; O r is the target center of mass of the sprayer; (x, y) is the coordinate of the center of mass of the sprayer chassis relative to the inertial coordinate system, (x r ,y r ) is the coordinate of the sprayer target center of mass relative to the inertial coordinate system, (x e ,y e ) are the longitudinal and lateral errors of the sprayer relative to its own coordinate system, c(s) is the curvature of the target path, s is the curvilinear coordinate of the target center of mass along the reference path from the initial position, θ is the heading of the sprayer centerline relative to the inertial system, and θ r is the target heading of the sprayer; θ e is the deviation of heading; v is the speed of the sprayer relative to the inertial system, v ris the target velocity; P = [xy θ] T .

[0014] Step 3.2, in the global coordinate system, the sprayer changes from the position [xy θ] T Move to pose [x r y r θ r ] T , the coordinates of the sprayer in the vehicle body coordinate system are: P e =[x e y e θ e ] T , where θ e =θ r -θ.

[0015] Step 3.3, according to the coordinate transformation formula, the error equation describing the moving posture can be obtained as:

[0016]

[0017] In step 3.4, based on the established posture error differential equation, the trajectory tracking control goal of the sprayer kinematic model is to design a suitable control law so that the tracking error converges to zero:

[0018] In step 3.5, the pose error differential equation can be obtained by combining equations (1) and (2):

[0019]

[0020] Step 3.6: Design the trajectory tracking control algorithm based on the posture error differential equation established in step 3.5

[0021] Step 3.7, select the positive definite Lyapunov function of the first step as:

[0022]

[0023] Step 3.8, take the derivative of both sides of equation (4) with respect to time and obtain:

[0024]

[0025] Step 3.9: Substitute equation (3) into equation (5) to obtain:

[0026]

[0027] Step 3.10, select u1 = sinθ e is the virtual input of the first step, to ensure Negative definite, select the expected value u of u1 1d for:

[0028]

[0029] Step 3.11, to ensure Negative, select the sprayer speed as:

[0030] v=k x x e +v r cosθ e (8)

[0031] Step 3.12, k in equation (7) and equation (8) y With k x is any positive real number.

[0032] Step 3.13, when u1 can accurately and quickly track u 1d When , the longitudinal error and the lateral error will converge to 0.

[0033] Step 3.14, define u1 and u 1d The difference is:

[0034]

[0035] Step 3.15, construct the positive definite Lyapunov function of the second step as:

[0036]

[0037] Step 3.16, it is obvious that V2 is positive definite. Taking the derivative of both sides of equation (10) with respect to time, we get:

[0038]

[0039] Step 3.17: Substitute equations (3), (6), and (9) into equation (11) to obtain:

[0040]

[0041] Step 3.18: Substitute equations (7) and (8) into equation (12) to obtain:

[0042]

[0043] Step 3.19, to ensure Negative definite, choose ω as:

[0044]

[0045] Step 3.20, k in formula (14) u is any positive constant.

[0046] Step 3.21, the control law of the sprayer motion posture error model is obtained from equations (8) and (14):

[0047] Beneficial effects: Combined with the important invention points of each claim, the functions, effects, and technical problems solved by each invention point are supplemented in detail, which is conducive to subsequent authorization 1. The present invention has the following beneficial effects: High control accuracy: The present invention adopts the Backstepping nonlinear control method, which can accurately control the lateral deviation, longitudinal deviation and heading angle at the same time, significantly improving the path tracking accuracy of the sprayer during operation. 2. Supporting multi-machine collaborative operation: The present invention not only realizes lateral control, but also introduces longitudinal control design, providing a longitudinal safety distance control method for the collaborative operation of multiple agricultural machines, which is the key basis for realizing multi-machine collaborative operation. 3. The algorithm is lightweight and easy to deploy: The control structure of the present invention is clear, the algorithm used has low computational complexity, and can be directly deployed in existing agricultural machinery control terminals (such as PLCs and embedded controllers), with good engineering feasibility. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 This is the kinematic model of the 4WID self-steering high-clearance sprayer.

[0049] Figure 2 This is the trajectory tracking pose error map of the sprayer. DETAILED DESCRIPTION

[0050] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. The described examples are only part of the embodiments of the present invention, not all the embodiments.

[0051] The specific implementation steps are as follows:

[0052] Step 1: This embodiment is based on a 4WID self-steering high-clearance sprayer. The sprayer uses a highly integrated GNSS / INS high-precision combined navigation system. By receiving differential data from the reference station, it can achieve real-time carrier phase differential positioning (RTK), providing centimeter-level high-precision position information for the sprayer.

[0053] Step 2: Build a kinematic model for the 4WID self-steering high-clearance sprayer's special walking chassis and simplify it into a two-wheeled vehicle model. This includes the following steps:

[0054] Step 2.1, as Figure 1As shown, firstly, the global coordinate system and the vehicle coordinate system are established, and the sprayer kinematic model is established based on the geometric principle. To facilitate the design of the algorithm, the sprayer kinematic model is simplified as follows:

[0055]

[0056] Step 3, such as Figure 2 As shown in the figure, a trajectory tracking posture error model is established based on the kinematic model of the two-wheeled vehicle, and the trajectory tracking control law of the sprayer is gradually designed based on the backstepping method. The main steps include:

[0057] Step 3.1, in the sprayer posture error diagram, L is the wheelbase of the sprayer chassis; D is the track width of the sprayer chassis; δ is the steering angle of the front and rear steering axes; O is the center of mass of the sprayer chassis; O r is the target center of mass of the sprayer; (x, y) is the coordinate of the center of mass of the sprayer chassis relative to the inertial coordinate system, (x r ,y r ) is the coordinate of the sprayer target center of mass relative to the inertial coordinate system, (x e ,y e ) are the longitudinal and lateral errors of the sprayer relative to its own coordinate system, c(s) is the curvature of the target path, s is the curvilinear coordinate of the target center of mass along the reference path from the initial position, θ is the heading of the sprayer centerline relative to the inertial system, and θ r is the target heading of the sprayer; θ e is the deviation of heading; v is the speed of the sprayer relative to the inertial system, v r is the target speed; P = [x yθ] T .

[0058] Step 3.2, in the global coordinate system, the sprayer changes from the position [xy θ] T Move to pose [x r y r θ r ] T , the error of the sprayer in the vehicle body coordinate system is: P e =[x e y e θ e ] T , where θ e =θ r -θ.

[0059] Step 3.3, according to the coordinate transformation formula, the error equation describing the moving posture can be obtained as:

[0060]

[0061] In step 3.4, based on the above-established posture error differential equation, the trajectory tracking control goal of the sprayer kinematic model is to design a suitable control law so that the tracking error converges to zero:

[0062] In step 3.5, the pose error differential equation can be obtained by combining equations (1) and (2):

[0063]

[0064] Step 3.6: Design the trajectory tracking control algorithm based on the posture error differential equation established in step 3.5:

[0065] Step 3.7, select the positive definite Lyapunov function of the first step as:

[0066]

[0067] Step 3.8, take the derivative of both sides of equation (4) with respect to time and obtain:

[0068]

[0069] Step 3.9: Substitute equation (3) into equation (5) to obtain:

[0070]

[0071] Step 3.10, select u1 = sinθ e is the virtual input of the first step, to ensure Negative definite, select the expected value u of u1 1d for:

[0072]

[0073] Step 3.11, to ensure that V1 is negative, select the sprayer speed as:

[0074] v=k x x e +v r cosθ e (8)

[0075] Step 3.12, k in equation (7) and equation (8) y With k x is any positive real number.

[0076] Step 3.13, when u1 can accurately and quickly track u 1d When , the longitudinal error and the lateral error will converge to 0.

[0077] Step 3.14, define u1 and u 1d The difference is:

[0078]

[0079] Step 3.15, construct the positive definite Lyapunov function of the second step as:

[0080]

[0081] Step 3.16, it is obvious that V2 is positive definite. Taking the derivative of both sides of equation (10) with respect to time, we get:

[0082]

[0083] Step 3.17: Substitute equations (3), (6), and (9) into equation (11) to obtain:

[0084]

[0085] Step 3.18: Substitute equations (7) and (8) into equation (12) to obtain:

[0086]

[0087] Step 3.19, to ensure Negative definite, choose ω as:

[0088]

[0089] Step 3.20, k in formula (14) u is any positive constant.

[0090] Step 3.21, the control law of the sprayer motion posture error model is obtained from equations (8) and (14):

[0091]

[0092] The above is only a specific implementation scheme of the present invention, but the scope of protection of the present invention is not limited thereto. Any technician familiar with this technical field, within the technical scope of the present invention, can make equivalent replacements or changes based on the technical scheme and inventive concept of the present invention, which should be covered by the scope of protection of the present invention.

Claims

1. A trajectory tracking control method for a four-wheel independent drive (4WID) self-steering high-clearance sprayer, the method mainly comprising the following four steps: Step 1: A highly integrated, high-precision combined navigation system is constructed using a 4WID self-steering high-clearance sprayer. By receiving differential data from the base station, real-time carrier phase differential positioning is achieved, providing centimeter-level high-precision position information for the sprayer. Step 2: A kinematic model of the 4WID self-steering high-clearance sprayer's special walking chassis is established and simplified into a two-wheeled vehicle model. Step 3: A trajectory tracking posture error model is established based on the two-wheeled vehicle kinematic model, and the trajectory tracking control law of the sprayer is gradually designed based on the backstepping control method.

2. The method according to claim 1, characterized in that: The step 2 specifically includes: For the special walking chassis of the 4WID self-steering high-clearance sprayer, its kinematic model is established and simplified into a two-wheeled vehicle model: Where P = [x yθ] T It is defined as the sprayer state quantity, (x, y) is the coordinate of the sprayer chassis center of mass, θ is the sprayer heading, v is the sprayer speed, δ is the steering angle of the sprayer steering axis, and L is the sprayer chassis wheelbase.

3. The method according to claim 1, wherein: The step 3 specifically includes: Establish the sprayer trajectory tracking posture error map: D is the wheelbase of the sprayer chassis; O is the center of mass of the sprayer chassis; O r is the target center of mass of the sprayer; (x r ,y r ) is the coordinate of the sprayer target center of mass relative to the inertial coordinate system, (x e ,y e ) are the longitudinal and lateral errors of the sprayer relative to its own coordinate system, c(s) is the curvature of the target path, s is the curvilinear coordinate of the target centroid along the reference path from the initial position, and θ r is the target heading of the sprayer; θ e is the deviation of heading; v r The target speed P = [x yθ] T , in the global coordinate system, the sprayer changes from the position [x yθ] T Move to pose [x r y r θ r ] T , the error vector of the sprayer in the vehicle body coordinate system is: P e =[x e y e θ e ] T , where θ e =θ r -θ; According to the coordinate transformation formula, the error equation describing the moving posture can be obtained as: The simultaneous equations (1) and (2) can be used to obtain the posture error differential equation: in, is the sprayer angular velocity; Based on the above-established posture error differential equation, the trajectory tracking control goal of the sprayer kinematic model is to design a suitable control law so that the tracking error converges to zero: The design steps of the trajectory tracking control algorithm based on the Backstepping method are as follows: The positive definite Lyapunov function (Lyapunov) of the first step is selected as V1: The time derivatives of both sides of formula (4) are: Substituting (3) into (5) we get: Select u1 = sinθ e As the first step of virtual input, to ensure Negative definite, select the expected value u of u1 1d for: To ensure Negative, select the sprayer speed as: v=k x x e +v r cosθ e (8) In formula (7) and formula (8), k y With k x is any positive real number; When u1 can accurately and quickly track u 1d When , the longitudinal error and the lateral error will converge to 0; Define u1 and u 1d The difference is: Select the Lyapunov function of the second step as V2: Obviously V2 is positive definite. Taking the derivative of both sides of Equation (10) with respect to time, we can get: Substituting equations (3), (6) and (9) into equation (11), we obtain: Substituting equations (7) and (8) into equation (12), we get: To ensure Negative definite, choose ω as: In formula (14), k u is any positive real number; The control law of the sprayer motion posture error model is obtained from equations (8) and (14):

Images