CN-116149189-B - 4WID high-clearance sprayer track tracking control method for improving Pursuit algorithm

Abstract

The invention discloses a 4WID high-clearance sprayer track tracking control method for improving a Pursuit algorithm, and belongs to the field of unmanned systems. According to the control method, the transverse error and the heading error are introduced into the 4WID pure tracking model, the RTK positioning error is corrected, the forward looking distance is dynamically changed by designing the evaluation function, the problem that the forward looking distance in the traditional pure tracking controller cannot be dynamically adjusted, so that the tracking precision is low is solved, the tracking precision of the navigation track at the turning position is effectively improved, and the requirement of high-precision operation under the paddy field environment can be met.

Inventors

• SHEN YUE
• Zhao sha

Assignees

• 江苏大学

Dates

Publication Date

20260512

Application Date

20230224

Claims (3)

1. 1. The track tracking control method for the 4WID high-clearance sprayer with improved Pursuit algorithm is characterized by comprising the following steps: step 1, providing real-time high-precision position, speed and gesture navigation parameters of a sprayer by adopting a high-precision navigation positioning system RTK; Step 2, aiming at a special walking chassis of the 4WID high-clearance sprayer, establishing a kinematic model and a pure tracking model of the special walking chassis; step 3, introducing the transverse error and the course error into the pure tracking model in an ideal state to obtain an improved pure tracking model; step 4, based on the improved pure tracking model, designing an evaluation function to dynamically change the forward looking distance so as to realize path tracking and simulation verification; The specific process of the step 4 is as follows: Step 4.1, determining a forward looking area according to the position relation between the current position of the sprayer and a reference path; Step 4.2, traversing path points in the front area, and substituting an improved pure tracking model; step 4.3, predicting the position of the sprayer: (9); Wherein, the method comprises the following steps of ) The pose of the mass center of the sprayer at the next moment under the global coordinate system is shown as the following formula ) The pose of the mass center of the sprayer at the current moment under the global coordinate system is (x, y) the mass center of the vehicle body The point coordinates, theta is the heading of the central line of the sprayer relative to an inertial system, delta t is an updating time interval, L is the wheelbase of the chassis of the sprayer, delta is the steering angle of the front and rear steering shafts of the sprayer, and v is the speed of the sprayer relative to the inertial system; Step 4.4, carrying out RTK positioning coordinate correction: (10); Wherein, the method comprises the following steps of ) To correct RTK positioning coordinates ) The method is characterized in that the method is used for positioning RTK positioning coordinates, H is the installation height of a positioning antenna, phi is a roll angle, phi is a pitch angle, and theta is the heading of a central line of the sprayer relative to an inertial system; step 4.5, designing an evaluation function to obtain an optimal forward looking distance; step 4.5.1, the lateral error expression is: (11); and 4.5.2, the heading error expression is: (12); Wherein, the In the event of a lateral error, The current position information of (x, y, theta) is heading error, (-) , , ) Is the position information of the target point, (x, y) is the mass center of the vehicle body The point coordinates, theta, are the heading of the central line of the sprayer relative to an inertial system; Step 4.5.3, the evaluation function expression is: (13); Wherein, the In order to evaluate the function of the device, , The predicted transverse error and the predicted heading error at the next moment are respectively; Step 4.5.4, traversing the waypoints in the forward region to obtain a maximum value , The distance from the corresponding path point to the sprayer is the optimal forward looking distance.
2. 2. The method according to claim 1, wherein the specific process of the step 2 is as follows: aiming at a special chassis of the 4WID high-clearance sprayer, a kinematic model is built and simplified: v (1); Wherein, the Is the pose of the mass center of the sprayer under the global coordinate system, and (x, y) is the mass center of the vehicle body The point coordinates, θ, are the heading of the central line of the sprayer relative to the inertial system, L is the wheelbase of the chassis of the sprayer, δ is the steering angle of the front and rear steering shafts, and v is the speed of the sprayer relative to the inertial system; Sprayer model in high ground clearance According to sine theorem: = (2); the steering angle in the four-wheel steering model has the following relationship: = (3); The simultaneous expression (2) and the expression (3) obtain a steering angle expression: (4); Wherein R represents a turning radius, 2 The center angle is indicated as such, Indicating the forward viewing distance, L indicating the wheelbase of the sprayer chassis, The steering angle of the front and rear steering shafts of the sprayer is shown.
3. 3. The method according to claim 2, wherein the step 3 comprises the following steps: in the improved pure tracking model, the following geometrical relationships exist: = (5); (6); (7); the simultaneous equations (5), (6), (7) and (4) result: = (8); Wherein d is a transverse error, (-) ) Is the coordinates of the target point [ ] ) Is the coordinates of the center of the rear wheel, R is the turning radius, Is a variable to be determined and is, In order to be a practical lateral error, As the actual course angle of the heading, Is heading error.

Description

4WID high-clearance sprayer track tracking control method for improving Pursuit algorithm Technical Field The invention relates to unmanned system technology, in particular to a 4WID high-clearance sprayer track tracking control method for improving a Pursuit algorithm. Background In the field of unmanned systems, agricultural robots are receiving a great deal of attention. The effect of path tracking directly determines the advantages and disadvantages of navigation control, improves the performance and the precision of expected path tracking tasks, and is very important for the overall control of the agricultural robot. Pure tracking is of great advantage because of its simple control. However, in agricultural applications, due to uncertainty of environmental and topographical features, it has problems of inability to track in time, slow convergence speed, low tracking accuracy, and unsatisfactory performance and accuracy. In this regard, many scholars have improved on pure tracking algorithms. Researchers realize dynamic adjustment of the forward looking distance of the pure tracking model based on ITAE optimization criteria, fuzzy algorithm, particle swarm optimization algorithm, BP neural network algorithm and ant colony optimization algorithm, thereby effectively improving the precision of agricultural machinery tracking and reducing convergence time and tracking error. Disclosure of Invention The invention provides a 4WID high-clearance sprayer track tracking control optimization algorithm based on a Pursuis. The optimized algorithm introduces the transverse error and the heading error into a 4WID pure tracking model, designs an evaluation function to dynamically change the forward looking distance, corrects the RTK positioning error, and effectively improves the tracking precision of the navigation track at the turning position. The technical scheme of the invention is that the track tracking control method of the 4WID high-clearance sprayer based on the improved Pursuis comprises the following steps: and step 1, providing real-time high-precision position, speed and gesture navigation parameters of the sprayer by adopting a high-precision navigation positioning system RTK. And 2, establishing a kinematic model and a pure tracking model of the special walking chassis of the 4WID high-clearance sprayer. And step 3, introducing the transverse error and the course error into the pure tracking model under an ideal state to obtain an improved pure tracking model. And 4, dynamically changing the forward looking distance by designing an evaluation function based on the improved pure tracking model to realize path tracking. Further, the step 1 specifically includes: Step 1.1, the sprayer in step 1 adopts a highly integrated GNSS/INS high-precision integrated navigation system, and a built-in high-precision positioning and orientation board card of a matched GNSS high-precision positioning and orientation receiver can rapidly and accurately calculate the relative position information of two antennas and the included angle (azimuth angle) between the phase center connecting line of the two antennas and true north. Meanwhile, by receiving the differential data of the reference station, real-time carrier phase differential positioning (RTK) can be realized, and centimeter-level high-precision position information is provided for the sprayer. Further, the step 2 specifically includes: Step 2.1, firstly, establishing a global coordinate system and a vehicle body coordinate system, establishing a sprayer kinematic model based on a geometric principle, and simplifying the sprayer kinematic model into a convenient design algorithm: Wherein P= [ x y theta ] T is the pose of the mass center of the sprayer under the global coordinate system, (x, y) is the coordinate of the point O of the mass center of the vehicle body, theta is the course of the central line of the sprayer relative to the inertial system, L is the wheelbase of the chassis of the sprayer, delta is the steering angle of the front steering shaft and the rear steering shaft, and v is the speed of the sprayer relative to the inertial system. And 2.2, establishing a pure tracking model of the high-clearance sprayer. 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. Setting a counter-clockwise movement R >0, a clockwise movement R <0,2 alpha represents a central angle, L d represents a forward looking distance, L represents an wheelbase of a sprayer chassis, and delta represents a steering angle of a front steering shaft and a rear steering shaft of the sprayer. Step 2.2.2, in Δaoc, according to the sine theorem: Step 2.2.3, the steering angle in the four-wheel steering model has the following relation: Step 2.2.4, combining equation (2) with equation (3) to obtain a steering angle expres