Work vehicle path tracking control method, device, electronic device, and storage medium

Abstract

The application relates to a work vehicle path tracking control method and device, an electronic device and a storage medium, the method comprising the following steps: after a work vehicle is connected to a tracking path, a navigation system is started; a GPS antenna determines a frequency sending discrete path point coordinate set S; a discrete target path trajectory point is loaded, and the nearest discrete path point S to a current vehicle position is searched i ; a filtering process is performed on a point set near the discrete path point S i ; a path curvature at the path point after the filtering process is obtained; a vehicle local coordinate system is rotated and translated; current wheel rotation angles are estimated according to a vehicle driving trajectory and a vehicle state space model; and the work vehicle path is tracked and controlled according to the path curvature. The application solves the problem of low accuracy and stability of the tracking control of the work vehicle in the prior art.

Description

Technical Field

[0001] This application relates to the field of automatic control, and in particular to a method, apparatus, electronic device, and storage medium for path tracking control of a work vehicle. Background Technology

[0002] Agricultural vehicles encompass common agricultural vehicles as well as specialized vehicles used in farmland environments. Automated agricultural vehicles help alleviate the intensity of manual labor, reduce input costs, and improve profitability. Significant progress has been made in the autonomous navigation of agricultural vehicles over the past two decades. The ultimate goal for agricultural vehicles is to develop autonomous vehicles or field robots for agricultural operations.

[0003] Currently, the instability of GPS signals in farmland environments is limited by the number and location of base stations and interference from external environmental factors. When operating in remote suburban farmland and other similar locations, the stability of GPS signal acquisition for farmland vehicles is worse than that for general urban vehicles. Using centimeter-level GPS antenna equipment and fixing the antenna equipment to an unobstructed position on the roof of farmland vehicles can improve the accuracy of GPS signals. However, data points collected due to environmental disturbances and noise interference will seriously affect the accuracy and stability of path tracking.

[0004] Agricultural vehicles, due to their operational needs, generally need to traverse entire fields, such as planting, tilling, spraying, harvesting, and sampling. These operations require agricultural vehicles to travel on evenly spaced paths. As a result, the path tracking process of unmanned agricultural vehicles operating in this environment mainly involves straight lines and U-turns at the field edges. The curvature of the tracked path varies greatly, and the existing kinematic-based feedforward control technology struggles to balance the accuracy and stability of the tracking control.

[0005] There is currently no effective solution to the problem of low accuracy and stability in the tracking and control of operating vehicles in existing technologies. Summary of the Invention

[0006] This embodiment provides a method, apparatus, electronic device, and storage medium for tracking and controlling the path of a work vehicle, in order to solve the problems of low accuracy and stability in the tracking and control of work vehicles in the prior art.

[0007] Firstly, this embodiment provides a method for controlling the path tracking of a work vehicle, the method comprising:

[0008] After the work vehicle is put into online tracking, the navigation system is turned on;

[0009] The GPS antenna determines the frequency to transmit the discrete path point coordinate set S;

[0010] Load discrete target path trajectory points and find the discrete path point S closest to the current vehicle position. i Among them, S i Let be any point within the coordinate set S;

[0011] For discrete path points S i Filter the nearby point set;

[0012] Find the path curvature at the path points after filtering;

[0013] Rotate and translate the vehicle's local coordinate system;

[0014] Estimate the current wheel angle based on the vehicle's driving trajectory and the vehicle's state space model;

[0015] The path of the work vehicle is tracked and controlled based on the path curvature.

[0016] In some embodiments, the tracking control of the work vehicle path based on path curvature includes:

[0017] When the path curvature is less than 0.1, the operation vehicle path is tracked and controlled using a pure tracking model based on lateral deviation.

[0018] In some embodiments, the tracking control of the work vehicle path based on path curvature includes:

[0019] When the path curvature is greater than or equal to 0.1, the path of the work vehicle is tracked and controlled using a heading angle-based feedback control model.

[0020] In some of these embodiments, the work vehicle is a front-and-rear axle wheeled vehicle.

[0021] In some embodiments, the method further includes: when the vehicle is tracking a path, finding the next path trajectory point to be tracked, selecting the road point closest to the vehicle's position as the target road point, continuously updating the vehicle's position coordinates during the tracking process of each path trajectory point, continuously discarding the tracked path trajectory points, and calculating the next road point closest to the vehicle's position as the target trajectory point.

[0022] In some of these embodiments, the discrete path points S i Filtering of nearby point sets includes:

[0023] Using the Kalman filter algorithm to analyze discrete path points S i The nearby point set is filtered.

[0024] In some embodiments, calculating the path curvature at the filtered path points includes:

[0025] Use the least squares method to find the path curvature at path points after Kalman filtering.

[0026] Secondly, this embodiment provides a work vehicle path tracking control device, the device comprising:

[0027] The activation module is used to activate the navigation system after the work vehicle is online and tracking the route.

[0028] The transmitting module is used by the GPS antenna to determine the frequency and transmit the discrete path point coordinate set S;

[0029] The loading module is used to load discrete target path trajectory points and find the discrete path point S closest to the current vehicle position. i Among them, S i Let be any point within the coordinate set S;

[0030] The filtering module is used to filter discrete path points S. i Filter the nearby point set;

[0031] The calculation module is used to calculate the path curvature at path points after filtering.

[0032] The coordinate transformation module is used to rotate and translate the vehicle's local coordinate system;

[0033] The estimation module is used to estimate the current wheel angle based on the vehicle's driving trajectory and the vehicle state space model;

[0034] The tracking module is used to track and control the path of the work vehicle based on the path curvature.

[0035] Thirdly, this embodiment provides an electronic device including a memory and a processor, wherein the memory stores a computer program and the processor is configured to run the computer program to perform the work vehicle path tracking control method described in the first aspect.

[0036] Fourthly, this embodiment provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the operation vehicle path tracking control method described in the first aspect.

[0037] Compared with the prior art, the method, device, electronic device and storage medium for path tracking control of agricultural vehicles provided in this embodiment are applicable to common wheeled agricultural vehicles. The model is simple and reliable. This path tracking method based on solving the problem of large curvature variation range of the tracking path is widely applicable to the tracking path in common agricultural operations. It can improve the error caused by unstable GPS signals in agricultural environments, while also improving the problem that traditional single path tracking control methods are difficult to balance tracking accuracy and tracking control stability in the path tracking environment of agricultural vehicles.

[0038] The principle of this application is as follows: This invention utilizes Kalman filtering to process discrete GPS data, filtering out cluttered data caused by external factors such as environmental noise and instantaneous anomalies in GPS antenna signals. Then, the least squares method is used to perform curve fitting on the GPS data to obtain the curvature of the real-time tracking path. A pure tracking model based on lateral error and a method incorporating heading error weight control are used to track paths with different curvature ranges. This solves the problem of path tracking error caused by factors such as antenna signals in agricultural operation vehicles and the problem of insufficient control stability caused by the large range of curvature changes in the driving path of agricultural operation vehicles.

[0039] In summary, the present invention has the following advantages:

[0040] 1. By using Kalman filtering to preprocess GPS data for tracking paths, noise is eliminated, which improves the tracking error problem caused by factors such as antenna signals in agricultural vehicles and enhances tracking accuracy.

[0041] 2. By rotating and translating the vehicle's local coordinate system, the tracking effect is ensured to be accurate when the vehicle uses its own path tracking control model. The current wheel angle is estimated based on the vehicle's driving trajectory and the vehicle's state space model, which solves the problem of difficulty in determining the initial wheel angle.

[0042] 3. Use the least squares method to find the path points S after Kalman filtering. i Path curvature ρ at i By using a pure tracking model based on lateral error and incorporating a method based on heading error weight control for path tracking within different curvature ranges, the problem of path tracking error caused by factors such as antenna signals and insufficient control stability caused by the large range of curvature variation in the driving path of agricultural vehicles is solved. This suppresses system oscillations caused by understeer and overshoot, ultimately achieving vehicle path tracking in agricultural operation environments.

[0043] Details of one or more embodiments of this application are set forth in the following drawings and description to make other features, objects and advantages of this application more readily apparent. Attached Figure Description

[0044] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings:

[0045] Figure 1 This is a hardware structure block diagram of a terminal that executes the operation vehicle path tracking control method according to the embodiments of this application;

[0046] Figure 2 This is a flowchart of the operation vehicle path tracking control method according to an embodiment of this application;

[0047] Figure 3 This application describes the Kalman filter algorithm flow and principle in its embodiments.

[0048] Figure 4 This is a schematic diagram of the rotation and translation of the planar coordinate system according to an embodiment of this application;

[0049] Figure 5 This is a schematic diagram of a pure tracking model in an embodiment of this application;

[0050] Figure 6 This is a schematic diagram of a two-wheeled vehicle model according to an embodiment of this application;

[0051] Figure 7 This is a schematic diagram of a heading angle feedback control model according to an embodiment of this application. Detailed Implementation

[0052] To better understand the purpose, technical solution, and advantages of this application, the application is described and illustrated below in conjunction with the accompanying drawings and embodiments.

[0053] Unless otherwise defined, the technical or scientific terms used in this application shall have the general meaning as understood by one of ordinary skill in the art to which this application pertains. Words such as “a,” “an,” “an,” “the,” “the,” and “these,” used in this application, do not indicate quantitative limitation and may be singular or plural. The terms “comprising,” “including,” “having,” and any variations thereof used in this application are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or device that comprises a series of steps or modules (units) is not limited to the listed steps or modules (units) but may include steps or modules (units) not listed, or may include other steps or modules (units) inherent to such processes, methods, products, or devices. The terms “connected,” “linked,” and “coupled,” used in this application, are not limited to physical or mechanical connections but may include electrical connections, whether direct or indirect. The term “multiple” used in this application refers to two or more. The "and / or" operator describes the relationship between related objects, indicating that three relationships can exist. For example, "A and / or B" can represent three cases: A alone, A and B simultaneously, and B alone. Typically, the character " / " indicates that the objects before and after it are in an "or" relationship. The terms "first," "second," and "third," etc., used in this application are merely for distinguishing similar objects and do not represent a specific ordering of the objects.

[0054] The method embodiments provided in this example can be executed on a terminal, computer, or similar computing device. For example, it can run on a terminal. Figure 1 This is a hardware structure block diagram of a terminal executing the operation vehicle path tracking control method of the embodiments of this application. For example... Figure 1 As shown, a terminal may include one or more ( Figure 1 Only one is shown in the diagram. A processor 102 and a memory 104 for storing data are also included. The processor 102 may be, but is not limited to, a microprocessor (MCU) or a programmable logic device (FPGA). The terminal may also include a transmission device 106 for communication functions and an input / output device 108. Those skilled in the art will understand that… Figure 1 The structure shown is for illustrative purposes only and does not limit the structure of the terminal described above. For example, the terminal may also include components that are larger than... Figure 1 The more or fewer components shown, or having the same Figure 1 The different configurations shown are illustrated.

[0055] The memory 104 can be used to store computer programs, such as application software programs and modules, like the computer program corresponding to the work vehicle path tracking control method in this embodiment. The processor 102 executes various functional applications and data processing by running the computer program stored in the memory 104, thereby implementing the above-described method. The memory 104 may include high-speed random access memory, and may also include non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some instances, the memory 104 may further include memory remotely located relative to the processor 102, and these remote memories can be connected to the terminal via a network. Examples of such networks include, but are not limited to, the Internet, corporate intranets, local area networks, mobile communication networks, and combinations thereof.

[0056] The transmission device 106 is used to receive or send data via a network. This network includes a wireless network provided by the terminal's communication provider. In one example, the transmission device 106 includes a Network Interface Controller (NIC), which can connect to other network devices via a base station to communicate with the Internet. In another example, the transmission device 106 can be a Radio Frequency (RF) module used for wireless communication with the Internet.

[0057] The overall design concept of this application is as follows: Considering that agricultural vehicles often involve repeated straight-line driving and U-turns across the entire field, Kalman filtering is used to preprocess the GPS data of the tracking path to eliminate noise. Based on the preprocessed path tracking points, the real-time curvature of the path is calculated. For straight-line tracking and U-turn tracking, where the curvature range is large, a pure tracking model with lateral error feedback is used to control the tracking of sections with low curvature. This model is simple and provides accurate tracking even under low-speed agricultural vehicle operation conditions. By using weighted control of lateral and heading error feedback to track sections with high curvature, high tracking accuracy is ensured while suppressing system oscillations caused by understeer and overshoot, ultimately achieving vehicle path tracking in agricultural operation environments.

[0058] This embodiment provides a method for path tracking control of work vehicles. Figure 2 This is a flowchart of the operation vehicle path tracking control method according to an embodiment of this application, as shown below. Figure 2 As shown, the process includes the following steps:

[0059] Step S201: After the work vehicle goes online to track the route, the navigation system is turned on.

[0060] Specifically, after the farmland operation vehicle goes online to track the route, it activates the navigation system, which includes a GPS antenna, angle sensor, steering wheel motor, and controller.

[0061] Step S202: The GPS antenna determines the frequency and transmits the discrete path point coordinate set S.

[0062] Specifically, GPS antennas have centimeter-level accuracy. The Global Positioning System uses WGS84 as its reference coordinate system, with the Earth's central mass as the origin, a reference ellipsoid, a standard coordinate system, altitude data, and a geoid. WGS84 latitude and longitude information is based on the Earth's spherical coordinate system, which is inconvenient for calculating distances during path tracking. Therefore, WGS84 coordinates are converted to planar coordinates. The commonly used coordinate system is the Universal Transverse Mercator (UTM) projection coordinate system. The UTM projection coordinate system is divided into 60 regions, like an orange, and each of these regions is flattened. The coordinate set S is also based on a planar rectangular coordinate system. The coordinate set received by the GPS antenna in the WGS84 geodetic coordinate system needs to be converted to the UTM projection coordinate system to transform the geodetic global coordinate system into a vehicle coordinate system used for path tracking.

[0063] Step S203: Load discrete target path trajectory points and find the path point S closest to the current vehicle position. i Among them, S i Let be any point within the coordinate set S.

[0064] Specifically, when the vehicle is tracking a path, it finds the next path trajectory point to be tracked, selects the road point closest to the vehicle's position as the target road point, and continuously updates the vehicle's position coordinates during the tracking process of each path trajectory point, while continuously discarding the tracked path trajectory points and calculating the next road point closest to the vehicle's position as the target trajectory point.

[0065] Step S204: Use the Kalman filter algorithm to process the discrete path points S i Filtering of nearby point sets.

[0066] Specifically, the Kalman filter algorithm process and principle are as follows: Figure 3 As shown, the process and principle of filtering the point set near the discrete path point Si are as follows: First, initialize the parameters and define the state information. Then, prior predictions are made for the system, as follows:

[0067]

[0068]

[0069] Next, the system status is updated, as follows:

[0070]

[0071]

[0072]

[0073] The update module iterates the updated data to the new state parameters for the current moment and proceeds to the next round of prediction and update until the system terminates. In the above equation, the subscript "k" represents the value at any given time, and the superscript "-" represents the predicted value. The vehicle location information collected in real time by the GPS antenna is based on a fixed timestamp, with each frame having an equal time interval. The longitude and latitude information collected in real time by the GPS antenna are used as the input state parameters for the Kalman filter algorithm. It is the initial state parameter The covariance matrix, which represents the value of this frame. The relationships between the state parameters are defined, where A and C are the state transition matrices for the corresponding state parameters. The system state parameters are updated at each time step. It is a priori estimate at any given time, providing an estimate of the system state. It is the accurate value of the system state at any given time. It is obtained by weighting and gaining the prior estimate of the system state and the correction for the error between it and the sensor observation. It is the prediction covariance matrix at any given time, representing the prediction covariance matrix for each frame. The relationship between state parameters, P k It is its updated accurate value; Q and R are measures of uncertainty in the process of predicting the system state, generally such as environmental noise; K k It is the Kalman gain, which is the weighted term of the latitude and longitude information collected by the sensor and the error of the prior estimate.

[0074] Based on the initial values ​​of the system state parameters, the system state is predicted. The parameters are then updated based on the predicted state values. For example, during path tracking, the state changes in each frame. After each prediction is completed, it is necessary to correct the state based on the observed values. The corrected state value is then used as the accurate value to predict the state value of the next frame.

[0075] The purpose of Kalman gain is to reduce the variance of the optimal estimate. It acts as a weighting term, coordinating the error weights between the estimate and the observation. This is the core function of Kalman filtering.

[0076] Step S205: Use the least squares method to find the path points S after Kalman filtering. i The path curvature at that point.

[0077] Specifically, the path point S after Kalman filtering is obtained using the least squares method. i Path curvature ρ at i Choose the polynomial y = a1x m +…+a m x+a m+1 Where m=3, linear fitting is performed, and 7 path points Si (2m+1) are selected, which are (x i-3 ,y i-3 ), (x i-2 ,y i-2 ), (x i-1 ,y i-1 ), (x i ,y i ), (x i+1 ,y i+1 ), (x i+2 ,y i+2 ), (x i+3 ,y i+3 ), where i>3; for the polynomial y=a1x 3 +a2x 2 +a3x+a4, from Nx=b, where the least squares principle is needed to find an approximate solution. pass have to

[0078] in:

[0079] Therefore, the fitted curve y = a is obtained. 1m x 3 +a 2m x 2 +a 3m x+a 4m ,

[0080] According to the curvature formula Point S can be obtained i curvature ρ i .

[0081] Step S206: Rotate and translate the vehicle's local coordinate system.

[0082] Specifically, in step S203, since the pure tracking model mentioned above requires the vehicle to be at the origin of the plane coordinate system and the vehicle direction to be parallel to the y-axis, the wheel rotation angle δ needs to be obtained in the coordinate system after rotation and translation.

[0083] A schematic diagram of rotation and translation of a planar coordinate system is shown below. Figure 4 As shown, let the coordinates of point P in the plane coordinate system XOY be (x...). P ,yP If point P rotates by θ degrees and translates from the origin to O′(X), then... O′ ,Y O′ The coordinates in the coordinate system X′O′Y′ after () are:

[0084]

[0085] Step S207: Estimate the current wheel angle based on the vehicle's driving trajectory and the vehicle state space model.

[0086] Specifically, since it's necessary to control the wheels to turn a certain angle, this can only be achieved by turning the steering wheel a certain angle, requiring knowledge of the initial wheel angle. The following estimates the current wheel angle based on the vehicle's trajectory:

[0087] The state-space model of the two-wheeled vehicle is as follows:

[0088]

[0089] Where (x, y) represents the coordinates of the center of the rear wheel axle in the planar coordinate system, v represents the speed of the vehicle in the direction of travel, and l is the distance between the front and rear wheel axles. It is the vehicle's yaw angle (the angle between the vehicle's direction of travel and the x-axis of a Cartesian coordinate system).

[0090] From the above formula, we can obtain

[0091]

[0092] Discretizing it yields

[0093]

[0094]

[0095] Step S208: Track and control the path of the work vehicle according to the path curvature.

[0096] Specifically, when the path curvature ρ i When <0.1, a pure tracking model based on lateral bias is used for tracking. The pure tracking model tracks as follows: Figure 5 As shown.

[0097] When the vehicle is at the origin of the plane coordinate system and its direction is parallel to the y-axis, the trajectory the vehicle will travel and the turning radius R satisfy the following relationship:

[0098]

[0099] Solving the above system of equations, we get...

[0100]

[0101] like Figure 6 The two-wheeled vehicle model, derived from the simplified kinematic two-wheeled vehicle model based on the Ackermann steering principle, shows that:

[0102]

[0103] Where l is the distance between the front and rear axles of the vehicle, and δ is the front wheel steering angle. Based on the calculation, we can obtain that if we want the car's trajectory to pass through point (x... t ,y t If the front wheel turns, then the steering angle of the wheel is:

[0104]

[0105] When the path curvature ρ i When ≥0.1, based on the control of the pure tracking model based on lateral deviation, a control weight term based on heading deviation is added, and the heading angle feedback control model is as follows: Figure 7 As shown, when the vehicle is at the origin of the plane coordinate system and its direction is parallel to the y-axis, the trajectory of the vehicle and the turning radius R satisfy the following relationship:

[0106] When the path curvature ρ i When the value is ≥0.1, a control weight based on the heading deviation is added on the basis of the lateral deviation control to solve the problem that the single feedforward control cannot maintain the stability of the control after the system disturbance is enhanced due to the increased curvature of the tracking path.

[0107] By using feedback control based on the vehicle's heading angle deviation, the system's stability is greatly enhanced.

[0108] From the heading angle deviation feedback control model, we get: e δ =δ y -α

[0109] Among them, e δ It is the vehicle heading angle tracking error; δ y α is the target heading angle of the vehicle relative to the next tracking waypoint; α is the heading angle of the current vehicle position.

[0110] The feedback control increment for the front wheel steering angle using PID control is:

[0111]

[0112] Among them, K p K i K d These are the gain coefficients for the proportional gain, integral gain, and differential gain, respectively.

[0113] When the curvature and curvature variation of the planned tracking path are large, the tracking accuracy and stability of the control system are significantly improved after the weight of vehicle heading angle feedback control is added.

[0114] In summary, combining control based on lateral deviation and heading deviation, the front wheel steering angle ε of the vehicle at this time is:

[0115]

[0116] Among them, K l K is a weighting coefficient based on lateral deviation control. h It is a weighting coefficient based on heading deviation control.

[0117] Step S209: Determine whether the distance between the vehicle and the target point is less than the preset distance. If yes, end the vehicle path tracking control. If no, proceed to step S203.

[0118] Specifically, it determines whether the distance between the vehicle and the target point is less than a preset distance x. s If yes, then terminate the vehicle path tracking control; otherwise, proceed to step S203.

[0119] In this embodiment, Kalman filtering is used to preprocess the GPS data of the tracking path trajectory to eliminate noise, improve the path tracking error problem caused by factors such as antenna signals of agricultural operation vehicles, and improve tracking accuracy. By rotating and translating the vehicle's local coordinate system, the tracking effect is ensured to be accurate when the vehicle uses its own path tracking control model. The current wheel angle is estimated based on the vehicle's driving trajectory and the vehicle's state space model, solving the problem of difficulty in determining the initial wheel angle. The path point S after Kalman filtering is calculated using the least squares method. i Path curvature ρ at i By using a pure tracking model based on lateral error and incorporating a method based on heading error weight control for path tracking within different curvature ranges, the problem of path tracking error caused by factors such as antenna signals and insufficient control stability caused by the large range of curvature variation in the driving path of agricultural vehicles is solved. This suppresses system oscillations caused by understeer and overshoot, ultimately achieving vehicle path tracking in agricultural operation environments.

[0120] It should be noted that the steps shown in the above process or in the flowchart of the accompanying figures can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0121] This embodiment also provides a work vehicle path tracking control device, which is used to implement the above embodiments and preferred embodiments; details already described will not be repeated. The terms "module," "unit," "subunit," etc., used below refer to combinations of software and / or hardware that achieve a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated.

[0122] The device includes:

[0123] The activation module is used to activate the navigation system after the work vehicle is online and tracking the route.

[0124] The transmitting module is used by the GPS antenna to determine the frequency and transmit the discrete path point coordinate set S;

[0125] The loading module is used to load discrete target path trajectory points and find the discrete path point S closest to the current vehicle position. i Among them, S i Let be any point within the coordinate set S;

[0126] The filtering module is used to filter discrete path points S. i Filter the nearby point set;

[0127] The calculation module is used to calculate the path curvature at path points after filtering.

[0128] The coordinate transformation module is used to rotate and translate the vehicle's local coordinate system;

[0129] The estimation module is used to estimate the current wheel angle based on the vehicle's driving trajectory and the vehicle state space model;

[0130] The tracking module is used to track and control the path of the work vehicle based on the path curvature.

[0131] It should be noted that the above modules can be functional modules or program modules, and can be implemented through software or hardware. For modules implemented through hardware, the above modules can reside in the same processor; or the above modules can be located in different processors in any combination.

[0132] This embodiment also provides an electronic device including a memory and a processor, the memory storing a computer program and the processor being configured to run the computer program to perform the steps in any of the above method embodiments.

[0133] Optionally, the electronic device may further include a transmission device and an input / output device, wherein the transmission device is connected to the processor and the input / output device is connected to the processor.

[0134] Optionally, in this embodiment, the processor can be configured to perform the following steps via a computer program:

[0135] S1, After the work vehicle goes online to track the route, the navigation system is turned on;

[0136] S2, GPS antenna determines the frequency to transmit discrete path point coordinate set S;

[0137] S3, load discrete target path trajectory points, and find the discrete path point S closest to the current vehicle position. i Among them, S i Let be any point within the coordinate set S;

[0138] S4, for discrete path points S i Filter the nearby point set;

[0139] S5, find the path curvature at the path points after filtering;

[0140] S6, rotate and translate the vehicle's local coordinate system;

[0141] S7 estimates the current wheel angle based on the vehicle's driving trajectory and the vehicle state space model;

[0142] S8 tracks and controls the path of the work vehicle based on the path curvature.

[0143] It should be noted that the specific examples in this embodiment can refer to the examples described in the above embodiments and optional implementations, and will not be repeated in this embodiment.

[0144] Furthermore, in conjunction with the vehicle path tracking control method provided in the above embodiments, this embodiment can also provide a storage medium for implementation. The storage medium stores a computer program; when executed by a processor, the computer program implements the steps of any of the vehicle path tracking control methods in the above embodiments.

[0145] It should be understood that the specific embodiments described herein are merely illustrative of the application and not intended to limit it. All other embodiments derived by those skilled in the art based on the embodiments provided in this application without inventive effort are within the scope of protection of this application.

[0146] Obviously, the accompanying drawings are merely some examples or embodiments of this application. Those skilled in the art can apply this application to other similar situations based on these drawings without any creative effort. Furthermore, it is understood that although the work done in this development process may be complex and lengthy, for those skilled in the art, certain design, manufacturing, or production modifications made based on the technical content disclosed in this application are merely conventional technical means and should not be considered as insufficient disclosure of this application.

[0147] The term "embodiment" in this application refers to a specific feature, structure, or characteristic described in connection with an embodiment that may be included in at least one embodiment of this application. The appearance of this phrase in various places in the specification does not necessarily imply the same embodiment, nor does it imply that it is mutually exclusive with or independent of other embodiments. It will be clearly or implicitly understood by those skilled in the art that the embodiments described in this application may be combined with other embodiments without conflict.

[0148] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of patent protection. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the appended claims.

Claims

1. A work vehicle path tracking control method characterized by, The method comprises: After the work vehicle is on-line to track the path, the navigation system is started; GPS antenna determines frequency sending discrete path point coordinate set ; load discrete target path trajectory point, find the nearest discrete path point from the current vehicle position ; wherein, is any point in the coordinate set ; For discrete path points Filter the nearby point set; The path curvature at the path point after filtering is calculated; The vehicle local coordinate system is rotated and translated; The current wheel rotation angle is estimated according to the vehicle driving track and the vehicle state space model; The work vehicle path is tracked according to the path curvature; The work vehicle path is tracked according to the path curvature, which comprises: When the path curvature is less than 0.1, a pure pursuit model based on lateral deviation is used to track control the work vehicle path; in particular, when the vehicle is at the origin of the plane coordinate system and the vehicle direction is parallel to the y-axis, the trajectory to be traveled by the vehicle and the turning radius satisfy the following relationship: The above equation groups are solved simultaneously According to the two-wheeled vehicle model, a kinematic two-wheeled vehicle model simplified from the Ackerman steering principle can be known: wherein is the distance between the front and rear axles of the vehicle, is the front wheel steering angle, which is obtained from the calculation, if the trajectory of the car passes through the point then the front wheel steering angle is: When the path curvature is greater than or equal to 0.1, the path of the work vehicle is tracked and controlled by using a heading angle feedback control model; specifically, on the basis of tracking control of a pure pursuit model based on lateral deviation, a control weight term based on heading deviation is added, when the vehicle is at the origin of the plane coordinate system and the direction of the vehicle is parallel to the y-axis, the trajectory to be traveled by the vehicle and the turning radius satisfy the following relationship: From the course angle deviation feedback control model, we have: wherein, is a vehicle heading angle tracking error; is a vehicle target heading angle relative to a next tracking waypoint; is a current vehicle position heading angle; The feedback control increment of the front wheel steering angle of the vehicle using PID control is: wherein , , are the gain coefficients of the proportional, integral and derivative parts, respectively; Based on lateral deviation and based on heading deviation control, at this time the vehicle front wheel steering angle is: wherein, is a weight coefficient based on lateral deviation control, is a weight coefficient based on heading deviation control.

2. The work vehicle path following control method of claim 1, wherein, The work vehicle is a front and rear axle wheel type vehicle.

3. The work vehicle path following control method of claim 1, wherein, The method further comprises: when the vehicle is tracking the path, the next path track point to be tracked is searched, the path point closest to the vehicle body position is selected as the target path point, the vehicle position coordinates are updated in each path track point tracking process, the tracked path track points are discarded, and the next path point closest to the vehicle body position is calculated as the target track point.

4. The work vehicle path following control method of claim 1, wherein, The pair of discrete path points The filtering processing on the nearby point set includes: Utilizing a Kalman filter algorithm to filter a set of discrete path points in the vicinity.

5. The work vehicle path following control method of claim 4, wherein, The path curvature at the path point after filtering is calculated, which comprises: The path curvature at the path point after Kalman filtering is calculated by using the least square method.

6. A work vehicle path following control device characterized by, The device comprises: The starting module is configured to start the navigation system after the work vehicle is on-line to track the path; The sending module is used for the GPS antenna to determine the frequency sending discrete path point coordinate set ; A loading module is configured to load discrete target path trajectory points, find the closest discrete path point to the current vehicle position ; wherein, is an arbitrary point within the coordinate set ​ The filtering module is used for discrete path points. Filter the nearby point set; The calculation module is configured to calculate the path curvature at the path point after filtering; The coordinate conversion module is configured to rotate and translate the vehicle local coordinate system; The estimation module is configured to estimate the current wheel rotation angle according to the vehicle driving track and the vehicle state space model; The tracking module is configured to track the work vehicle path according to the path curvature; The work vehicle path is tracked according to the path curvature, which comprises: When the path curvature is less than 0.1, a pure pursuit model based on lateral deviation is used to track the path of the work vehicle; in particular, when the vehicle is at the origin of the plane coordinate system and the direction of the vehicle is parallel to the y-axis, the trajectory to be traveled by the vehicle and the turning radius satisfy the following relationship: Solving the above set of equations, we get According to the two-wheeled vehicle model, a kinematic two-wheeled vehicle model simplified from the Ackerman steering principle can be known: wherein is the distance between the front and rear axles of the vehicle, is the front wheel steering angle, which is obtained from the calculation, if the trajectory of the car passes through the point then the front wheel steering angle is: When the path curvature is greater than or equal to 0.1, the path of the work vehicle is tracked and controlled by using a heading angle feedback control model; specifically, on the basis of tracking control of a pure pursuit model based on lateral deviation, a control weight term based on heading deviation is added, when the vehicle is at the origin of the plane coordinate system and the direction of the vehicle is parallel to the y-axis, the trajectory to be traveled by the vehicle and the turning radius satisfy the following relationship: From the course angle deviation feedback control model, we have: wherein, is a vehicle heading angle tracking error; is a vehicle target heading angle relative to a next tracking waypoint; is a current vehicle position heading angle; The feedback control increment of the PID control for the vehicle front wheel rotation angle is: wherein , , are the gain coefficients of the proportional, integral and derivative parts, respectively; Based on lateral deviation and based on heading deviation control, at this time the vehicle front wheel steering angle is: wherein, is a weight coefficient based on lateral deviation control, is a weight coefficient based on heading deviation control. 7.An electronic device comprising a memory and a processor, the electronic device characterized by, The memory stores a computer program, and the processor is configured to run the computer program to execute the work vehicle path tracking control method in any one of claims 1 to 5.

8. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the steps of the work vehicle path tracking control method in any one of claims 1 to 5.

Images