Agricultural machine automatic driving trajectory control method for large-curvature agricultural machine operation path

Abstract

A kind of agricultural machine automatic driving trajectory control method for agricultural machine large curvature operation path, by obtaining agricultural machine operation path discrete operation path point set and after being preprocessed, establish cubic spline curve with path cumulative arc length as parameter, generate continuous smooth reference path;According to the maximum steering angle or minimum turning radius of agricultural machine wheel base, the local curvature of reference path is judged and corrected;Again according to the local curvature of the modified reference path, agricultural machine operation speed and control period, the reference path is adaptively resampled and the reference heading angle and local curvature corresponding to each sampling point are calculated;Finally, according to the current position, current speed, local curvature and lateral error of agricultural machine, vehicle running angular velocity and linear velocity are dynamically generated, and the sampling point of reference path, reference heading angle, local curvature, nearest path point, angular velocity and linear velocity are output to agricultural machine automatic driving control module to realize trajectory control.The present application can control agricultural machine to obtain more stable tracking reference information under the scene of head turning, boundary operation and large curvature curve path tracking, improve the smoothness of path tracking and operation accuracy.

Description

Technical Field

[0001] This invention relates to a technology in the field of autonomous driving, specifically a trajectory control method for autonomous driving of agricultural machinery on high curvature operation paths. Background Technology

[0002] In hilly and mountainous areas, there are many irregularly shaped farmlands, and the operation paths often have large curvatures. Currently, the commonly used method is to use broken line fitting to connect the paths as a tracking reference. However, this method is prone to problems such as sudden changes in heading angle, curvature, and unstable forward sight point when turning at the edge of the field, working at the boundary, and on sections with large curvatures. This results in discontinuous steering of agricultural machinery, increased tracking errors, and decreased ride smoothness, making it difficult to meet the requirements for centimeter-level unmanned operation trajectory tracking of agricultural machinery. Summary of the Invention

[0003] To address the aforementioned shortcomings of existing technologies, this invention proposes a trajectory control method for automated driving of agricultural machinery on high-curvature operating paths. This method can control agricultural machinery to obtain more stable tracking reference information in scenarios such as turning at the edge of the field, boundary operations, and tracking paths on high-curvature curves, thereby improving the smoothness of path tracking and the accuracy of operation.

[0004] This invention is achieved through the following technical solution:

[0005] This invention relates to a trajectory control method for automated driving of agricultural machinery on high-curvature operating paths. The method involves acquiring a discrete set of operating path points and preprocessing them to establish a cubic spline curve using the cumulative arc length of the path as a parameter, generating a continuous and smooth reference path. The local curvature of the reference path is constrained and corrected based on the machine's wheelbase, maximum steering angle, or minimum turning radius. Then, the reference path is adaptively resampled based on the corrected local curvature, the machine's operating speed, and the control cycle, and the reference heading angle and local curvature corresponding to each sampling point are calculated. Finally, the vehicle's angular velocity and linear velocity are dynamically generated based on the machine's current position, current speed, local curvature, and lateral error. The sampling points, reference heading angle, local curvature, nearest path point, angular velocity, and linear velocity of the reference path are output to the automated driving control module to achieve trajectory control.

[0006] The aforementioned acquisition of the discrete operation path point set for agricultural machinery operation paths refers to: based on the discrete operation path points of the agricultural machinery operation path to be tracked, receiving path spatial information obtained through GNSS / RTK acquisition, manual calibration, map planning, or operation boundary extraction, and forming an initial operation path control sequence for curve fitting, specifically: ,in: , Let represent the coordinates of the i-th path point in the planar coordinate system.

[0007] The preprocessing includes: sorting discrete path points, removing duplicate points, identifying distance anomalies, and identifying heading abrupt change points. When the distance between adjacent path points is less than a preset minimum distance threshold, duplicate points are removed; when the distance between adjacent path points is greater than a preset maximum distance threshold, point supplementation is performed between adjacent path points; when the heading angle change of an adjacent path segment corresponding to a path point exceeds a preset threshold, it is identified as an RTK sampling jitter point or an abnormal acquisition point, and is replaced, smoothed, or removed using adjacent valid path points to ensure that the input path point sequence meets the curve fitting requirements.

[0008] The continuous and smooth reference path is obtained by fitting a vehicle path spline curve, specifically by converting discrete operation path points into parameterized path points using the cumulative arc length s of the path as the curve parameter. ,in: ; Establish cubic spline functions for x(s) and y(s) respectively to obtain the continuous reference path: This cubic spline function satisfies positional continuity, first derivative continuity, and second derivative continuity at the connection points of adjacent path segments, and can be used to represent loop paths.

[0009] The aforementioned constraint judgment and correction refers to: calculating the maximum permissible path curvature of the agricultural machinery based on the machine's wheelbase, maximum front wheel steering angle, and operational safety requirements. and the local curvature of the fitted path ,when Greater than When the curvature of the local path segment exceeds a preset safety curvature threshold, path point densification, endpoint tangential adjustment, or local smoothing correction are performed to ensure that the corrected reference path curvature meets the steering capability of the agricultural machinery. Specifically, the wheelbase of the agricultural machinery is L, and the maximum front wheel steering angle is... , Let be the curvature at the j-th reference path point, used to characterize the degree of path curvature.

[0010] The adaptive resampling refers to: adaptively adjusting the sampling interval according to the local curvature of the fitted reference path, and resampling the fitted reference path according to the adaptive sampling interval to obtain a reference path point sequence that can be used for agricultural machinery automatic driving.

[0011] The adaptive adjustment of the sampling interval refers to the following: for straight road sections or road sections with small curvature, a larger sampling interval is used to reduce the number of path points and computational load; for road sections with large curvature, corner bends, or sharp bends at boundaries, a smaller sampling interval is used to improve the reference point density and curve path tracking accuracy. Specifically, the sampling interval... ,in: Let j be the sampling interval near the j-th sampling point. The preset maximum sampling interval, For reference path in the 1st Local curvature near each sampling point, This is the curvature adjustment coefficient.

[0012] The reference heading angle is calculated based on the tangential direction of the resampled path point, specifically as follows: ,in: Let be the expected heading angle corresponding to the j-th reference path point, used to provide directional reference for the automatic driving of agricultural machinery.

[0013] The local curvature is used to characterize the degree of curvature of the reference path at that location, and can be further used for forward distance adjustment, speed constraints, and steering control quantity generation, specifically: .

[0014] The vehicle angular velocity is generated using curvature feedforward and smooth sliding mode tracking control, and is obtained in the following way:

[0015] Step 1) Search for the nearest path point in the resampled reference path point sequence based on the current location of the agricultural machinery. Based on the agricultural machinery's operating speed and control cycle, several path points after the nearest point are selected as real-time tracking reference points, where: the current position of the agricultural machinery at any time k. ;

[0016] Step 2) Based on the current position of the agricultural machinery, the nearest path point, the forward target point, and the reference heading angle, calculate the lateral error and heading error of the agricultural machinery relative to the reference path: , ,in: For lateral error, To compensate for heading errors by curvature, Let be the angle normalization function. For heading angle, Curvature direction compensation coefficient , For curvature, For curvature-heading compensation system.

[0017] The linear velocity of the vehicle is dynamically determined based on the average curvature of the reference path ahead of the agricultural machinery, the current lateral error, and the original heading error.

[0018] When the curvature of the reference path ahead increases, the lateral error increases, or the heading error increases, the target linear velocity is reduced; when the curvature of the reference path ahead is small and the tracking error is small, the target linear velocity is increased, specifically: ,in: For the target linear velocity, Basic task speed, The average curvature within a preset path range in front of the agricultural machinery. This is the original heading error. , and These are the speed adjustment coefficients corresponding to curvature, lateral error, and heading error, respectively. and These are the minimum and maximum permissible linear speeds for agricultural machinery, respectively.

[0019] Step 3) After completing the nearest path point search and calculating the lateral error and curvature-compensated heading error, the original target angular velocity is generated using curvature feedforward and smooth sliding mode tracking control. The curvature feedforward angular velocity is calculated based on the current speed of the agricultural machinery and the average signed curvature of the reference path ahead: Construct a sliding mode surface to compensate for heading and lateral errors based on curvature: Combining curvature-feedforward angular velocity, curvature-compensated heading error feedback, lateral error feedback, and smoothing sliding mode compensation term, the original target angular velocity is generated: ,in: This is the current speed of the agricultural machinery. The average signed curvature within the preset path range in front of the agricultural machinery. For curvature feedforward angular velocity, Let λ be the sliding surface, and λ be the lateral error conversion coefficient. For heading error feedback coefficient, This is the lateral error feedback coefficient. To smooth the sliding mode gain, These are the boundary layer parameters.

[0020] Technical effect

[0021] This invention integrates agricultural machinery steering curvature constraints into the reference path generation and tracking control process. It adaptively adjusts the path sampling density based on local curvature and combines the average signed curvature of the preceding path, lateral error, and curvature-compensated heading error to collaboratively generate target linear velocity and target angular velocity. This reduces sudden steering changes and tracking errors in agricultural machinery on high-curvature road sections, and improves the smoothness of trajectory tracking and operational accuracy. Attached Figure Description

[0022] Figure 1 This is a flowchart of the present invention;

[0023] Figure 2 This is a schematic diagram illustrating the calculation of the closest point, forward view point, lateral error, and heading error of the agricultural machinery relative to the reference path in this invention.

[0024] Figure 3 This is a schematic diagram illustrating the experimental results of agricultural machinery curve path tracking in this invention. Detailed Implementation

[0025] like Figure 1 As shown in the figure, this embodiment relates to a method for automatic trajectory control of agricultural machinery operating on high curvature paths, including:

[0026] Step 1: Collect agricultural machinery operation path points using GNSS / RTK equipment, or manually input discrete operation path points via a control terminal. Remove duplicate points, identify outliers, and rearrange the path points to obtain a discrete path point sequence that meets the curve fitting requirements.

[0027] Step 2: Using the cumulative arc length of the path as a parameter, establish cubic spline functions for the x and y coordinates of the path points respectively to generate a continuous and smooth reference path. This reference path satisfies positional continuity, first derivative continuity, and second derivative continuity at the connection points of path segments.

[0028] Step 3: Adaptively adjust the sampling interval based on the local curvature of the reference path, and resample the reference path to generate a reference path point sequence with dense sampling on high curvature road sections and relatively sparse sampling on low curvature road sections. At the same time, calculate the reference heading angle and local curvature corresponding to each reference path point.

[0029] Step 4: During the automatic driving of the agricultural machinery, the current position, heading angle, and speed of the agricultural machinery are acquired in real time. Based on the current position, the nearest path point is searched in the reference path point sequence, and the forward sight distance is dynamically determined by combining the local curvature near the nearest path point and the current lateral error. When the agricultural machinery enters a bend at the edge of the field, a sharp bend at the boundary, or a path segment with large curvature, the forward sight distance is automatically reduced and the accuracy of the forward sight target point update is improved; when the agricultural machinery is on a straight section or a path segment with small curvature, the forward sight distance is appropriately increased to improve the smoothness of driving.

[0030] Step 5: Calculate the lateral error and heading error based on the current position of the agricultural machinery, the nearest path point, and the reference heading angle, and output the reference path point, reference heading angle, local curvature, nearest point index, forward target point, lateral error, and heading error to the agricultural machinery automatic driving control module.

[0031] Following specific experiments, a high-curvature path tracking test was conducted on a real vehicle test platform equipped with a GNSS / RTK positioning device, a heading angle detection device, and a differential drive actuator. The test reference path was generated from discrete path points collected on-site through cumulative chord length parameterization, two-dimensional cubic spline fitting, curvature constraint correction, and curvature adaptive resampling. The path included straight sections, continuous curved sections, and high-curvature turning sections. During the test, the target linear velocity ranged from 0.200 to 0.436 m / s, the GNSS / RTK effective position update frequency was approximately 10 Hz, and the RTK fixed solution was maintained throughout the test. The maximum absolute value of the average curvature ahead of the reference path was... The corresponding equivalent turning radius is approximately 1.66 m. The test lasted approximately 91.90 s, during which the vehicle continuously tracked the reference path for approximately 28.01 m. The test results show that the root mean square error of the vehicle's lateral error is 0.0419 m, the average absolute lateral error is 0.0337 m, and the maximum absolute lateral error is 0.1105 m. After removing the first 3 seconds of start-up and initial adjustment phase, the root mean square error of the heading error is approximately 4.55°, and the maximum absolute heading error is 11.70°. This demonstrates that the method of the present invention can achieve an average absolute value of curvature of approximately 1.66 m. Achieve continuous and stable trajectory tracking under conditions of large curvature operating paths.

[0032] like Figure 2 As shown, the agricultural machinery is currently located at point [location missing]. The nearest path point on the reference path is determined by local nearest point search. And select the forward target point along the reference path direction. Current location of agricultural machinery Relative to the nearest path point The projected distance in the normal direction of the reference path constitutes the lateral error. Current heading angle of agricultural machinery Reference heading angle at the nearest waypoint The included angle between them constitutes the heading error. By continuously updating the nearest path point and the forward-looking target point, tracking reference information is provided for the subsequent calculation of the target's linear velocity and angular velocity.

[0033] like Figure 3 As shown, by using the reference path generated by this invention, agricultural machinery can obtain a smoother target trajectory and continuous heading angle reference during curved path tracking, reducing sudden steering changes caused by discrete path point polyline connections, and improving the stability and operational accuracy of curved path tracking.

[0034] Compared with existing technologies, this invention utilizes cumulative chord length parameterized two-dimensional cubic spline fitting, agricultural machinery steering curvature constraints, and curvature adaptive resampling to create a reference path with continuous position and heading and smooth curvature changes in the discrete operation path. Furthermore, it coordinates the generation of target linear velocity and target angular velocity based on the curvature of the preceding path and tracking errors. In real-vehicle testing, the vehicle continuously completed approximately 28.01 m of trajectory tracking on the reference path, which included continuous curves and large-curvature turns. The maximum absolute value of the average curvature of the preceding reference path was [missing value]. The root mean square error of the lateral direction was 0.0419 m, and the mean absolute lateral error was 0.0337 m. The experimental results show that this invention can maintain continuous and stable trajectory tracking under conditions of large curvature operating paths, and matches the path generation results with the actual steering capability of the agricultural machinery.

[0035] The above-described specific implementations can be partially adjusted by those skilled in the art in different ways without departing from the principles and purpose of the present invention. The scope of protection of the present invention is defined by the claims and is not limited to the above-described specific implementations. All implementation schemes within the scope of the claims are bound by the present invention.

Claims

1. A method for trajectory control of automated driving of agricultural machinery for large curvature operation paths, characterized in that, By acquiring and preprocessing the discrete set of operation path points for agricultural machinery, a cubic spline curve is established using the cumulative arc length of the path as a parameter to generate a continuous and smooth reference path. The local curvature of the reference path is constrained and corrected based on the wheelbase, maximum steering angle, or minimum turning radius of the agricultural machinery. Then, the reference path is adaptively resampled based on the corrected local curvature, the agricultural machinery's operating speed, and the control cycle, and the reference heading angle and local curvature corresponding to each sampling point are calculated. Finally, the vehicle's angular velocity and linear velocity are dynamically generated based on the current position, current speed, local curvature, and lateral error of the agricultural machinery. The sampling points, reference heading angle, local curvature, nearest path point, angular velocity, and linear velocity of the reference path are output to the agricultural machinery automatic driving control module to achieve trajectory control.

2. The method for automatic trajectory control of agricultural machinery for large curvature operation paths according to claim 1, characterized in that, The aforementioned acquisition of the discrete operation path point set for agricultural machinery operation paths refers to: based on the discrete operation path points of the agricultural machinery operation path to be tracked, receiving path spatial information obtained through GNSS / RTK acquisition, manual calibration, map planning, or operation boundary extraction, and forming an initial operation path control sequence for curve fitting, specifically: ,in: , Let represent the coordinates of the i-th path point in the planar coordinate system.

3. The method for automatic trajectory control of agricultural machinery for large curvature operation paths according to claim 1 or 2, characterized in that, The preprocessing includes: sorting discrete path points, removing duplicate points, judging abnormal distance points, and judging abrupt changes in heading. When the distance between adjacent path points is less than a preset minimum distance threshold, duplicate points are removed. When the distance between adjacent path points is greater than a preset maximum distance threshold, point supplementation is performed between adjacent path points. When the heading angle change of an adjacent path segment corresponding to a certain path point exceeds a preset threshold, it is determined as an RTK sampling jitter point or an abnormal acquisition point, and is replaced, smoothed, or removed using adjacent valid path points to ensure that the input path point sequence meets the curve fitting requirements.

4. The method for automatic trajectory control of agricultural machinery for large curvature operation paths according to claim 1, characterized in that, The continuous and smooth reference path is obtained by fitting a vehicle path spline curve, specifically by converting discrete operation path points into parameterized path points using the cumulative arc length s of the path as the curve parameter. ,in: ; Establish cubic spline functions for x(s) and y(s) respectively to obtain the continuous reference path: This cubic spline function satisfies positional continuity, first derivative continuity, and second derivative continuity at the connection points of adjacent path segments, and can be used to represent loop paths.

5. The method for automatic trajectory control of agricultural machinery for large curvature operation paths according to claim 1, characterized in that, The aforementioned constraint judgment and correction refers to: calculating the maximum permissible path curvature of the agricultural machinery based on the machine's wheelbase, maximum front wheel steering angle, and operational safety requirements. and the local curvature of the fitted path ,when Greater than When the curvature of the local path segment exceeds a preset safety curvature threshold, path point densification, endpoint tangential adjustment, or local smoothing correction are performed to ensure that the corrected reference path curvature meets the steering capability of the agricultural machinery. Specifically, the wheelbase of the agricultural machinery is L, and the maximum front wheel steering angle is... , Let be the curvature at the j-th reference path point, used to characterize the degree of path curvature.

6. The method for automatic trajectory control of agricultural machinery for large curvature operation paths according to claim 1, characterized in that, The adaptive resampling refers to: adaptively adjusting the sampling interval according to the local curvature of the fitted reference path, and resampling the fitted reference path according to the adaptive sampling interval to obtain a reference path point sequence that can be used for agricultural machinery automatic driving.

7. The method for automatic trajectory control of agricultural machinery for large curvature operation paths according to claim 6, characterized in that, The adaptive adjustment of the sampling interval refers to the following: for straight road sections or road sections with small curvature, a larger sampling interval is used to reduce the number of path points and computational load; for road sections with large curvature, corner bends, or sharp bends at boundaries, a smaller sampling interval is used to improve the reference point density and curve path tracking accuracy. Specifically, the sampling interval... ,in: Let j be the sampling interval near the j-th sampling point. The preset maximum sampling interval, For reference path in the 1st Local curvature near each sampling point, This is the curvature adjustment coefficient.

8. The method for automatic trajectory control of agricultural machinery for large curvature operation paths according to claim 1, characterized in that, The reference heading angle is calculated based on the tangential direction of the resampled path point, specifically as follows: ,in: The expected heading angle corresponding to the j-th reference path point is used to provide directional reference for the automatic driving of agricultural machinery; The local curvature is used to characterize the degree of curvature of the reference path at that location, and can be further used for forward distance adjustment, speed constraints, and steering control quantity generation, specifically: .

9. The method for automatic trajectory control of agricultural machinery for large curvature operation paths according to claim 1, characterized in that, The vehicle's angular velocity is generated using curvature feedforward and smooth sliding mode tracking control, and is obtained in the following way: Step 1) Search for the nearest path point in the resampled reference path point sequence based on the current location of the agricultural machinery. Based on the agricultural machinery's operating speed and control cycle, several path points after the nearest point are selected as real-time tracking reference points, where: the current position of the agricultural machinery at any time k. ; Step 2) Based on the current position of the agricultural machinery, the nearest path point, the forward target point, and the reference heading angle, calculate the lateral error and heading error of the agricultural machinery relative to the reference path: , ,in: For lateral error, To compensate for heading errors by curvature, Let be the angle normalization function. For heading angle, Curvature direction compensation coefficient , For curvature, For curvature-heading compensation system; Step 3) After completing the nearest path point search and calculating the lateral error and curvature compensation heading error, the original target angular velocity is generated using curvature feedforward and smooth sliding mode tracking control. The curvature feedforward angular velocity is calculated based on the current speed of the agricultural machinery and the average signed curvature of the reference path ahead. Construct a sliding mode surface to compensate for heading and lateral errors based on curvature: Combining curvature-feedforward angular velocity, curvature-compensated heading error feedback, lateral error feedback, and smoothing sliding mode compensation term, the original target angular velocity is generated: ,in: This is the current speed of the agricultural machinery. The average signed curvature within the preset path range in front of the agricultural machinery. For curvature feedforward angular velocity, Let λ be the sliding surface, and λ be the lateral error conversion coefficient. For heading error feedback coefficient, This is the lateral error feedback coefficient. To smooth the sliding mode gain, These are the boundary layer parameters.

10. The method for automatic trajectory control of agricultural machinery for large curvature operation paths according to claim 1, characterized in that: The linear velocity of the vehicle is dynamically determined based on the average curvature of the reference path ahead of the agricultural machinery, the current lateral error, and the original heading error. When the curvature of the reference path ahead increases, the lateral error increases, or the heading error increases, the target linear velocity is reduced; when the curvature of the reference path ahead is small and the tracking error is small, the target linear velocity is increased, specifically: ,in: For the target linear velocity, Basic task speed, The average curvature within a preset path range in front of the agricultural machinery. This is the original heading error. , and These are the speed adjustment coefficients corresponding to curvature, lateral error, and heading error, respectively. and These are the minimum and maximum permissible linear speeds for agricultural machinery, respectively.

Images