An improved pursuit algorithm 4WID high clearance sprayer trajectory tracking control method

Abstract

The application discloses a 4WID high-clearance sprayer track tracking control method for improving a Pursuit algorithm and belongs to the field of unmanned driving systems. The control method introduces a lateral error and a heading error into a 4WID pure tracking model, corrects RTK positioning errors, and designs an evaluation function to dynamically change a look-ahead distance, so that the problem that the look-ahead distance in a traditional pure tracking controller cannot be dynamically adjusted and leads to low tracking precision is solved, the navigation track tracking precision at a turning place is effectively improved, and the demand for high-precision operation in a paddy field environment can be met.

Description

Technical Field

[0001] This invention relates to unmanned driving system technology, specifically to a trajectory tracking control method for a 4WID high ground clearance sprayer based on an improved Pursuit algorithm. Background Technology

[0002] In the field of autonomous driving systems, agricultural robots have received widespread attention. The effectiveness of path tracking directly determines the quality of navigation control, and improving the performance and accuracy of the desired path tracking task is crucial for the overall control of agricultural robots.

[0003] Pure tracking has significant advantages due to its simple control. However, in agricultural applications, due to the uncertainty of environmental and terrain features, it suffers from problems such as inability to track in a timely manner, slow convergence speed, and low tracking accuracy, resulting in less than ideal performance and accuracy.

[0004] In response, many scholars have improved the pure tracking algorithm. Researchers have implemented dynamic adjustment of the look-ahead distance of the pure tracking model based on the ITAE optimization criterion, fuzzy algorithm, particle swarm optimization algorithm, BP neural network algorithm, and ant colony optimization algorithm, which effectively improves the tracking accuracy of agricultural machinery and reduces convergence time and tracking error. Summary of the Invention

[0005] This invention proposes an optimized trajectory tracking control algorithm for 4WID high-clearance sprayers based on Pursuit. The optimized algorithm incorporates lateral and heading errors into the 4WID pure tracking model, designs an evaluation function to dynamically change the forward look-ahead distance, and corrects RTK positioning errors, effectively improving the navigation trajectory tracking accuracy at turns.

[0006] The technical solution of this invention is: a trajectory tracking control method for a 4WID high ground clearance sprayer based on an improved Pursuit, comprising the following steps:

[0007] Step 1: Use a high-precision navigation and positioning system (RTK) to provide real-time, high-precision position, speed, and attitude navigation parameters for the sprayer.

[0008] Step 2: Establish the kinematic model and pure tracking model for the special chassis of the 4WID high ground clearance sprayer.

[0009] Step 3: Introduce the lateral error and heading error into the ideal pure tracking model to obtain the improved pure tracking model.

[0010] Step 4: Based on the improved pure tracking model, design an evaluation function to dynamically change the look-ahead distance to achieve path tracking.

[0011] Furthermore, step 1 specifically includes:

[0012] Step 1.1: The sprayer described in Step 1 employs a highly integrated GNSS / INS high-precision combined navigation system. Its matching GNSS high-precision positioning and orientation receiver has a built-in high-precision positioning and orientation board, which can quickly and accurately calculate the relative position information of the two antennas and the angle (azimuth) between the line connecting the phase centers of the two antennas and true north. At the same time, by receiving differential data from the reference station, real-time carrier phase differential positioning (RTK) can be achieved, providing the sprayer with centimeter-level high-precision position information.

[0013] Furthermore, step 2 specifically includes:

[0014] Step 2.1: First, establish the global coordinate system and the vehicle coordinate system. Based on geometric principles, establish the kinematic model of the sprayer. To facilitate algorithm design, the kinematic model of the sprayer is simplified as follows:

[0015]

[0016] Where, P = [xy θ] T Let (x, y) be the pose of the sprayer's center of mass in the global coordinate system, (x, y) be the coordinates of the vehicle's center of mass point O, θ be the heading of the sprayer's centerline relative to the inertial frame, L be the wheelbase of the sprayer's chassis, δ be the steering angle of the front and rear steering axes, and v be the velocity of the sprayer relative to the inertial frame.

[0017] Step 2.2: Establish a pure tracking model for the high ground clearance sprayer.

[0018] Step 2.2.1: In the navigation coordinate system, A represents the current rear wheel center position of the sprayer, C represents the sprayer coordinates of the preview point on the reference path, and R represents the turning radius. Set counterclockwise movement R>0, clockwise movement R<0, 2α represents the center angle, and L... d δ represents the forward sight distance, L represents the wheelbase of the sprayer chassis, and δ represents the steering angle of the front and rear steering axes of the sprayer.

[0019] Step 2.2.2, in ΔAOC, according to the Law of Sines, we get:

[0020]

[0021] Step 2.2.3, the steering angles in the four-wheel steering model have the following relationship:

[0022]

[0023] Step 2.2.4: Combine equations (2) and (3) to obtain the expression for the steering angle:

[0024]

[0025] Furthermore, step 3 specifically includes:

[0026] Step 3.1, (x i ,y i (x) is the coordinate of the target point. o ,y o ) is the coordinate of the rear wheel center, L d Let be the variables to be determined, satisfying the following geometric relationship:

[0027]

[0028] Step 3.2, L1 is the actual lateral error, α is the actual heading angle, and θ e For the heading error, the following geometric relationship must be satisfied:

[0029]

[0030]

[0031] Step 3.3: By simultaneously solving equations (5), (6), (7), and (4), we obtain the improved pure tracking model:

[0032]

[0033] Furthermore, step 4 specifically includes:

[0034] Step 4.1: Determine the forward-looking area based on the positional relationship between the current position of the sprayer and the reference path.

[0035] Step 4.2: Traverse the path points in the forward-looking region and substitute them into the improved pure tracking model.

[0036] Step 4.3: Predict the position of the sprayer, where Δt is the update time interval. Since the position data of the agricultural machinery is updated very quickly, it can be assumed that the speed and wheel rotation angle remain constant within Δt.

[0037]

[0038] Step 4.4: Correct the RTK positioning coordinates.

[0039] Step 4.4.1: When the sprayer is driving in a paddy field environment, the vehicle body often tilts, which means that the output RTK positioning coordinates are not the actual coordinates of the rear axle midpoint of the vehicle. Directly using the coordinate values ​​output by the positioning system as the vehicle position coordinates will result in a large error.

[0040] Step 4.4.2: Set the installation height of the positioning antenna to H, the roll angle to φ, the pitch angle to ψ, and the vehicle heading angle to θ. The corrected position coordinates of the sprayer are:

[0041]

[0042] Step 4.5: Design the evaluation function to obtain the optimal forward look distance.

[0043] Step 4.5.1, the expression for the lateral error is:

[0044]

[0045] Step 4.5.2, the heading error expression is:

[0046] θ e =θ-θ r (12)

[0047] Step 4.5.3, the evaluation function expression is:

[0048]

[0049] in, These are the predicted lateral error and predicted heading error for the next moment, respectively.

[0050] Step 4.5.4: Traverse the path points in the forward view area to obtain the maximum value A. max A max The distance from the corresponding path point to the sprayer is the optimal forward-looking distance.

[0051] The beneficial effects of this invention are as follows: This invention introduces lateral error and heading error into the 4WID pure tracking model to correct RTK positioning error, and designs an evaluation function to dynamically change the forward sight distance, which solves the problem of low tracking accuracy caused by the inability to dynamically adjust the forward sight distance in traditional pure tracking controllers. It effectively improves the tracking accuracy of navigation trajectory at turns and can meet the needs of high-precision operations in paddy field environments. Attached image description:

[0052] Figure 1 A simplified diagram of the kinematic model of the sprayer.

[0053] Figure 2 This is a four-wheel steering model for a sprayer.

[0054] Figure 3 To improve the pure tracking model.

[0055] Figure 4 This is a schematic diagram of RTK positioning error. Detailed implementation method:

[0056] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. The described examples are only some embodiments of the present invention, not all embodiments.

[0057] The specific implementation steps are as follows:

[0058] Step 1: Based on the 4WID high-clearance sprayer, real-time high-precision position, speed and attitude navigation parameters of the sprayer can be provided.

[0059] Step 1.1: The sprayer described in Step 1 employs a highly integrated GNSS / INS high-precision combined navigation system. Its matching GNSS high-precision positioning and orientation receiver has a built-in high-precision positioning and orientation board, which can quickly and accurately calculate the relative position information of the two antennas and the angle (azimuth) between the line connecting the phase centers of the two antennas and true north. At the same time, by receiving differential data from the reference station, real-time carrier phase differential positioning (RTK) can be achieved, providing the sprayer with centimeter-level high-precision position information.

[0060] Step 2: Establish and simplify the kinematic model of the 4WID high-clearance sprayer's unique chassis. This includes the following steps:

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

[0062]

[0063] Step 2.2: As Figure 2 As shown, a pure tracking model of a high ground clearance sprayer is established. This mainly includes the following steps:

[0064] Step 2.2.1: In the navigation coordinate system, A represents the current rear wheel center position of the sprayer, C represents the sprayer coordinates of the preview point on the reference path, and R represents the turning radius. Set counterclockwise movement R>0, clockwise movement R<0, 2α represents the center angle, and L... d δ represents the forward sight distance, L represents the wheelbase of the sprayer chassis, and δ represents the steering angle of the front and rear steering axes of the sprayer.

[0065] Step 2.2.2: In ΔAOC, according to the Law of Sines, we get:

[0066]

[0067] Step 2.2.3: The steering angles in the four-wheel steering model have the following relationship:

[0068]

[0069] Step 2.2.4: Combine equations (2) and (3) to obtain the expression for the steering angle:

[0070]

[0071] Step 3: As Figure 3 As shown, it includes the following steps:

[0072] Step 3.1, (x i ,y i (x) is the coordinate of the target point. o ,y o () represents the coordinates of the rear wheel center, R is the turning radius, and L is the turning radius. d Let be the variables to be determined, satisfying the following geometric relationship:

[0073]

[0074] Step 3.2, L1 is the actual lateral error, α is the actual heading angle, and θ e For the heading error, the following geometric relationship must be satisfied:

[0075]

[0076]

[0077] Step 3.3: By simultaneously solving equations (5), (6), (7), and (4), we obtain the improved pure tracking model:

[0078]

[0079] Step 4.3: Predict the position of the sprayer, where Δt is the update time interval. Since the position data of the agricultural machinery is updated very quickly, it can be assumed that the speed and wheel rotation angle remain constant within Δt.

[0080]

[0081] Step 4.4.2, as follows Figure 4 As shown, with the installation height of the positioning antenna set to H, the roll angle to φ, the pitch angle to ψ, and the vehicle heading angle to θ, the corrected position coordinates of the sprayer are:

[0082]

[0083] Step 4.5: Design the evaluation function to obtain the optimal forward look distance.

[0084] Step 4.5.1, the expression for the lateral error is:

[0085]

[0086] Step 4.5.2, the heading error expression is:

[0087] θ e =θ-θ r (12)

[0088] Step 4.5.3, the evaluation function expression is:

[0089]

[0090] in, These are the predicted lateral error and predicted heading error for the next moment, respectively.

[0091] Step 4.5.4: Traverse the path points in the forward view area to obtain the maximum value A. max A max The distance from the corresponding path point to the sprayer is the optimal forward-looking distance.

[0092] In summary, this invention introduces lateral and heading errors into the 4WID pure tracking model to correct RTK positioning errors. It designs an evaluation function to dynamically change the forward look-ahead distance, solving the problem of low tracking accuracy caused by the inability to dynamically adjust the forward look-ahead distance in traditional pure tracking controllers. This effectively improves the tracking accuracy of navigation trajectory at turns and can meet the needs of high-precision operations in paddy field environments.

[0093] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

Claims

1. A trajectory tracking control method for a 4WID high-clearance sprayer based on an improved Pursuit algorithm, characterized in that, Includes the following steps: Step 1: Use a high-precision navigation and positioning system (RTK) to provide real-time, high-precision position, speed, and attitude navigation parameters for the sprayer; Step 2: Establish the kinematic model and pure tracking model for the special chassis of the 4WID high ground clearance sprayer; Step 3: Introduce lateral error and heading error into the ideal pure tracking model to obtain an improved pure tracking model; Step 4: Based on the improved pure tracking model, design an evaluation function to dynamically change the forward look-ahead distance, realize path tracking, and verify through simulation. The specific process of step 4 is as follows: Step 4.1: Determine the forward-looking area based on the positional relationship between the current position of the sprayer and the reference path; Step 4.2: Traverse the path points in the forward-looking region and substitute them into the improved pure tracking model; Step 4.3, predict the location of the sprayer: （9）； in,( () represents the pose of the sprayer's center of mass in the global coordinate system at the next moment. (x, y) represents the pose of the sprayer's center of mass in the global coordinate system at the current moment, and (x, y) represents the center of mass of the vehicle body. Point coordinates, θ is the heading of the sprayer centerline relative to the inertial frame, Δt is the update time interval, L is the wheelbase of the sprayer chassis, δ is the turning angle of the sprayer's front and rear steering axes, and v is the speed of the sprayer relative to the inertial frame. Step 4.4, perform RTK positioning coordinate correction: (10)； in,( ) represents the corrected RTK positioning coordinates. ) represents the RTK positioning coordinates, H represents the installation height of the positioning antenna, φ represents the roll angle, ψ represents the pitch angle, and θ represents the heading of the sprayer's centerline relative to the inertial frame. Step 4.5: Design the evaluation function to obtain the optimal forward look distance; Step 4.5.1, the expression for the lateral error is: (11)； Step 4.5.2, the heading error expression is: (12)； in, For lateral error, For heading error, (x,y,θ) represents the current position information. , , (x, y) represents the target point location information, and (x, y) represents the vehicle's center of mass. Point coordinates, θ is the heading of the sprayer's centerline relative to the inertial frame; Step 4.5.3, the evaluation function expression is: (13)； in, For the evaluation function, , These are the predicted lateral error and predicted heading error for the next moment, respectively. Step 4.5.4: Traverse the path points in the forward view area to obtain the maximum value. , The distance from the corresponding path point to the sprayer is the optimal forward-looking distance.

2. The method according to claim 1, characterized in that: The specific process of step 2 is as follows: For the special chassis of the 4WID high ground clearance sprayer, its kinematic model is established and simplified: v (1)； in, Let (x, y) be the pose of the sprayer's center of mass in the global coordinate system, and (x, y) be the center of mass of the vehicle body. Point coordinates, θ is the heading of the sprayer centerline relative to the inertial frame, L is the wheelbase of the sprayer chassis, δ is the steering angle of the front and rear steering axes, and v is the velocity of the sprayer relative to the inertial frame. High-clearance sprayer model In the middle, according to the Law of Sines, we get: = (2)； In the four-wheel steering model, the steering angles have the following relationship: = （3）； Combining formulas (2) and (3), we obtain the expression for the steering angle: (4)； Where R represents the turning radius, 2 Indicates the central angle. The forward sight distance is indicated by L, and the wheelbase of the sprayer chassis is indicated by L. This indicates the steering angle of the front and rear steering axes of the sprayer.

3. The method according to claim 2, characterized in that: The specific process of step 3 is as follows: In the improved pure tracking model, the following geometric relationships exist: = （5）； (6)； (7)； Combining equations (5), (6), (7), and (4), we obtain: = (8)； Where d is the lateral error, ( ) are the coordinates of the target point, ( () represents the coordinates of the rear wheel center, and R is the turning radius. It is a variable to be determined. This represents the actual lateral error. This is the actual heading angle. This represents the heading error.

Images