Self-propelled agricultural machine path tracking adaptive feedforward composite control method and system

Abstract

The application discloses a self-propelled agricultural machine path tracking adaptive feedforward composite control method and system. The method comprises the following steps: collecting the steering angle response data of a steering system, identifying and establishing a dynamic model of the steering system containing a second-order over-damped model; obtaining a desired path and a current pose to calculate a lateral error, dynamically adjusting the preview distance of a pure tracking algorithm based on the lateral error, calculating a pure tracking geometric tracking angle by using the adjusted preview distance to obtain a target steering angle; and constructing a feedforward controller based on the dynamic model, taking the target steering angle as an input to calculate a steering angle control quantity after feedforward compensation and outputting the same. The second-order over-damped model is constructed through system identification, adaptive preview pure tracking feedback is combined with dynamic feedforward compensation, the problems of large-mass agricultural machine steering response delay and nonlinear hysteresis are effectively solved, and the path tracking precision and stability are improved.

Description

Technical Field

[0001] This invention relates to the field of automatic navigation and control technology for agricultural machinery, and more specifically to a self-propelled agricultural machinery path tracking adaptive feedforward composite control method and system. Background Technology

[0002] Large agricultural machinery, such as self-propelled straw and plastic film recycling machines, exhibits characteristics like response delay and nonlinear hysteresis during path tracking due to their large mass, size, and multi-system coupling. Traditional Pure Pursuit (PP) algorithms, employing a fixed preview distance, present a trade-off between response sensitivity and tracking accuracy: too short a preview distance results in sensitive but oscillating vehicle response, while too long a preview distance leads to a smooth response but decreased tracking accuracy. Furthermore, pure pursuit algorithms, based on geometric model-driven feedforward control, lack compensation for system dynamics (such as steering inertia, friction, and delay), making it difficult to meet the demands of high-precision path tracking.

[0003] In existing technologies, some solutions use a first-order system model to approximate steering dynamics, but they cannot accurately describe the "S"-shaped inflection point characteristics and overdamping characteristics in the steering response. Therefore, there is an urgent need for a path tracking method that can accurately model the dynamic characteristics of the steering system and realize feedforward-feedback composite control. Summary of the Invention

[0004] In view of the above problems, the present invention is proposed to provide a self-propelled agricultural machinery path tracking adaptive feedforward composite control method and system that overcomes or at least partially solves the above problems.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] 1. A self-propelled agricultural machinery path tracking adaptive feedforward composite control method, characterized by comprising the following steps:

[0007] S1. Collect the steering angle response data of the steering system, and establish a dynamic model of the steering system based on the steering angle response data. The dynamic model includes a second-order overdamped model characterizing the dynamic characteristics of the steering system.

[0008] S2. Obtain the desired path and current pose, calculate the lateral error, dynamically adjust the pre-aiming distance of the pure tracking algorithm based on the lateral error, and use the adjusted pre-aiming distance to calculate the pure tracking geometric tracking angle to obtain the target turning angle;

[0009] S3. Construct a feedforward controller based on the dynamic model. The feedforward controller takes the target rotation angle as input and calculates the rotation angle control quantity after feedforward compensation based on the target rotation angle.

[0010] S4. Output the feedforward compensated angle control quantity to the steering actuator.

[0011] Preferably, the collected corner response data is processed by removing outliers using the 3σ criterion, filling in missing values ​​using linear interpolation, and smoothing using a Savitzky-Golay filter.

[0012] Preferably, the dynamic model further includes a continuous friction model for compensating for nonlinear friction, wherein the continuous friction model is:

[0013]

[0014] Among them, f c f is the Coulomb friction coefficient. v The coefficient of viscous friction is... The angular velocity of the target turning angle. It is a smooth approximation of the sign function.

[0015] Preferably, the transfer function of the second-order overdamped model is in the form of:

[0016]

[0017] Where K is the steady-state gain, T d ω is the delay time. n Let be the natural frequency, ζ be the damping ratio satisfying ζ>1, and s be the Laplace operator. For delayed terms; It is a unit step function.

[0018] Preferably, the parameters of the dynamic model are identified using the nonlinear least squares method, with the goal of minimizing the squared error between the actual turning angle output and the predicted output of the dynamic model. The optimal parameters are solved iteratively using the Levenberg-Marquardt algorithm. The parameters are also identified independently for the left-turn and right-turn conditions.

[0019] Preferably, the feedforward controller is constructed based on the inverse model of the dynamic model, and uses model parameters that are independently identified for left turns and right turns respectively. The output feedforward compensated steering angle control quantity satisfies:

[0020]

[0021] Where, δ ff K represents the angle control value after feedforward compensation. ff For the inverse model gain, K ff =1 / K, used to cancel the amplification effect, where K is the steady-state gain; Turn at the target corner; The target turning angular velocity is calculated using a difference approximation. The unit is ° / s, where T S It is the control cycle; The target steering angle acceleration is calculated using second-order difference. Unit: ° / s 2 .

[0022] Preferably, when identifying the parameters of the dynamic model, lower and upper bound constraints are set independently for the left turn and right turn conditions to limit the physical reasonable range of the parameters.

[0023] Preferably, in the step of dynamically adjusting the aiming distance based on lateral error, the following adaptive scheduling strategy is adopted:

[0024]

[0025] in, The adjusted aiming distance. For lateral error; Used as a baseline aiming distance; For error adaptive aiming components; The vehicle's speed; This is the speed proportionality coefficient.

[0026] Preferably, after calculating the pure tracking geometric angle, the proportional feedback of the lateral error is superimposed on the geometric tracking angle to obtain the target turning angle; the superimposed geometric tracking angle satisfy:

[0027]

[0028] in, The preset lateral error gain coefficient has a value range of 0.1 to 0.3. Represents the original geometric tracking angle; This indicates lateral error.

[0029] Based on the same inventive concept, this invention also discloses a self-propelled agricultural machinery path tracking adaptive feedforward composite control system, used to execute any of the above-mentioned self-propelled agricultural machinery path tracking adaptive feedforward composite control methods, including:

[0030] The data acquisition module is used to collect steering angle response data of the steering system;

[0031] The model identification module is used to establish a dynamic model of the steering system based on the steering angle response data. The dynamic model includes a second-order overdamped model.

[0032] The feedback control module is used to acquire the desired path and current pose, calculate the lateral error, dynamically adjust the pre-aiming distance of the pure tracking algorithm based on the lateral error, and calculate the pure tracking geometric tracking angle using the adjusted pre-aiming distance to obtain the target turning angle.

[0033] The feedforward control module is used to calculate the feedforward compensated angle control quantity based on the inverse model of the dynamic model, with the target angle as input.

[0034] The output module is used to output the feedforward compensated steering angle control quantity to the steering actuator.

[0035] The beneficial effects of the above-described technical solutions provided in the embodiments of the present invention include at least the following:

[0036] By collecting actual steering response data, a dynamic model of the steering system including a second-order overdamped model was established. Compared with the traditional first-order model, the steering prediction accuracy was greatly improved. The root mean square error of the left turn condition was reduced from 3.834° to 0.872°, and the right turn condition was reduced from 3.183° to 0.812°. At the same time, the continuous friction model further suppressed the steady-state error and low-speed crawling phenomenon.

[0037] During path tracking, the aiming distance of the pure tracking algorithm is dynamically adjusted based on the magnitude of the lateral error, enabling the vehicle to quickly correct itself when the error is large and maintain stable driving when the error is small. A feedforward compensation is applied to the target steering angle using an established dynamic model, effectively eliminating the inertia and lag of the steering system. This is then combined with adaptive pure tracking feedback to eliminate lateral deviation, forming a feedforward-feedback composite control. Real-vehicle test results show that the mean absolute value of the lateral error (MAE) of this method is reduced to 0.0264 meters, a decrease of more than 41% compared to the traditional pure tracking algorithm. Attached Figure Description

[0038] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0039] Figure 1 This is a flowchart of a self-propelled agricultural machinery path tracking adaptive feedforward composite control method provided in an embodiment of the present invention; Figure 2 This is a comparison chart of the prediction effects of the identification model provided in the embodiments of the present invention; Figure 3 This is a comparison diagram of the straight path tracking trajectory provided in the embodiment of the present invention using the A_PP algorithm; Figure 4This is a diagram showing the lateral error curve of the A_PP algorithm for straight path tracking provided in this embodiment of the invention. Figure 5 This is a comparison diagram of the straight path tracking trajectory provided in the embodiment of the present invention using the FF_A_PP algorithm; Figure 6 This is a diagram showing the lateral error curve of the FF_A_PP algorithm for straight path tracking provided in this embodiment of the invention. Figure 7 This is a block diagram of a self-propelled agricultural machinery path tracking adaptive feedforward composite control system provided in an embodiment of the present invention. Detailed Implementation

[0040] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0041] like Figure 1 As shown in the figure, this invention discloses a self-propelled agricultural machinery path tracking adaptive feedforward composite control method, characterized by the following steps:

[0042] S1. Collect the steering system's angle response data and establish a dynamic model of the steering system based on the angle response data. The dynamic model includes a second-order overdamped model that characterizes the dynamic characteristics of the steering system.

[0043] S2. Obtain the desired path and current pose, calculate the lateral error, dynamically adjust the pre-aiming distance of the pure tracking algorithm based on the lateral error, and use the adjusted pre-aiming distance to calculate the pure tracking geometric tracking angle to obtain the target turning angle;

[0044] S3. Construct a feedforward controller based on the dynamic model. The feedforward controller takes the target angle as input and calculates the angle control quantity after feedforward compensation based on the target angle.

[0045] S4. Output the feedforward compensated angle control quantity to the steering actuator.

[0046] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. This embodiment is described using a self-propelled straw and residual film recycling machine as an example, but the application of the present invention is not limited thereto.

[0047] S101, Data Acquisition and Preprocessing

[0048] Using a self-propelled straw and plastic film recycling machine as the subject, a step response experiment with multiple speed gradients was conducted. Four characteristic speed points of 0.5, 1, 2, and 3 m / s were selected, and a ±10° steering angle step input excitation signal was used to test both left-turn and right-turn conditions. The vehicle speed, steering angle command, and actual steering angle output were recorded simultaneously at a frequency of 100 Hz. Each condition was tested three times to eliminate random errors.

[0049] For the collected corner time series data, outliers are removed using the 3σ criterion, missing values ​​are filled in using linear interpolation, and Savitzky-Golay (SG) filters (window length 21, 3rd order polynomial) are used for smoothing to remove environmental noise. A custom program is used to unify the sampling frequency to solve the problem of non-uniform timestamps in the original data, and step detection is used to automatically identify the step change moments in the input signal.

[0050] S201, Steering System Modeling

[0051] Through time-domain analysis of the measured step response curve at the turning angle, the system exhibits typical second-order dynamic characteristics: the response curve is "S"-shaped, that is, the rate of change is high in the initial stage (rapid dynamics), and gradually approaches steady state after the inflection point (slow convergence), while the response of the first-order system is a monotonic exponential change and lacks inflection point characteristics.

[0052] The damping characteristics of a second-order system are determined by the damping ratio ζ. When 0 < ζ < 1, it is underdamped, meaning the response exhibits oscillations and overshoot; when ζ = 1, it is critically damped, exhibiting the fastest monotonic change without overshoot; when ζ > 1, it is overdamped, exhibiting a slow monotonic change without overshoot. Based on the system's dynamic characteristics, there is no significant oscillation throughout the entire process (overshoot < 2%), and the system's velocity decreases significantly as it approaches steady state in the later stages, consistent with overdamped characteristics. Therefore, a second-order overdamped system model is adopted, with the transfer function as follows:

[0053]

[0054] Where K is the steady-state gain, the ratio of the output steady-state value to the input step amplitude, reflecting the system's amplification capability of the input; T d ω is the input-output delay time, the time delay from a change in input to the system's initial response, measured in seconds; s is the Laplace variable; ω n The natural frequency, ζ, determines the frequency of the system's free oscillation, measured in rad / s; ζ is the damping ratio, dimensionless; u(tT) d ) is a unit step function.

[0055] Based on the physical characteristics of the vehicle steering system, a second-order system model is used to characterize the system's fundamental dynamic properties, such as inertial mass, elastic restoring force, and viscous damping. These parameters are obtained through step response experiments to achieve dynamic feedforward. Considering that the nonlinear friction effect present in the actual system can lead to steady-state error and low-speed creeping, a continuous friction model is introduced for compensation. Dynamic feedforward eliminates phase lag, and friction compensation suppresses steady-state error.

[0056]

[0057] In the formula, f c The coefficient of friction is the Coulomb friction coefficient, calibrated through steady-state error at low speeds (<0.5° / s), and the amplitude of static friction force is used to compensate for the dead zone effect of the steering system; f v It is the coefficient of viscous friction, which is proportional to velocity and is calibrated through a uniform velocity experiment; The angular velocity of the desired (target) turning angle, in ° / s; function Used for smoothing approximate sign function , where 0.1 is a smoothing coefficient, which can avoid numerical instability caused by abrupt changes in the function near zero velocity.

[0058] S301. System parameter identification based on least squares method

[0059] The model is fitted using a nonlinear least squares method (fitting the optimal parameters by minimizing the squared error between the actual data and the model output), with the following objective function:

[0060]

[0061] Among them, y act (t i ) represents t i Real-time measured rotation angle output, unit °; y mod (t i ) represents the output predicted by the model, calculated from the identified parameters and the step response formula, in degrees.

[0062] The optimization algorithm is based on the MATLAB nonlinear least squares solver lsqcurvefit and employs the Levenberg-Marquardt (LM) algorithm for iterative solution. This algorithm introduces a damping factor μ to adaptively adjust the step size, smoothly switching between gradient descent and Gauss-Newton methods. In the early stages of iteration or when the residuals are large, μ is increased to dominate gradient descent and ensure global convergence; as the optimal solution approaches, μ is decreased to dominate Gauss-Newton and achieve second-order accelerated convergence. Furthermore, the algorithm utilizes Jacobian matrix regularization to address ill-conditioned numerical problems of the Hessian approximation matrix in singular or near-singular states.

[0063] Passing through the lower bound l b and the upper boundary u b Physically reasonable parameter constraints are applied to limit the acceptable range. Due to the inconsistent dynamic response characteristics of left and right turns, exhibiting asymmetric properties, left and right turns are identified independently. The parameter vector is x = [K, ζ, ω]. n T d The specific values ​​of the upper and lower bounds are shown in Table 1, where δ d Turn the corner towards your target.

[0064] Table 1 Steering System Parameter Identification Parameter Settings

[0065] <![CDATA[Initial value x₀]]> <![CDATA[[δ d ,1.3,1.8,0.1]]]> <![CDATA[[δ d ,1.2,2.0,0.08]]]> <![CDATA[lower bound l b > <![CDATA[[0.95δ d ,1.1,1.5,0.05]]]> <![CDATA[[0.95δ d ,1.1,1.8,0.03]]]> <![CDATA[upper bound u b > <![CDATA[[1.05δ d ,1.5,2.2,0.3]]]> <![CDATA[[1.05δ d ,1.4,2.5,0.15]]]>

[0066] The lsim() function was used to perform time-domain simulations on the identified parameters, and the differences between the actual data and the model output were compared. A step-by-step identification strategy was adopted to ensure parameter convergence. The parameter identification results of the first-order and second-order overdamped system models are shown in Table 2. In the friction model, the Coulomb friction coefficient is 0.098 and the viscous friction coefficient is 0.050.

[0067] Table 2. Steering System Parameter Identification Results

[0068] To verify that the system is a second-order system model, a first-order system was used as a control group during parameter identification. The analytical solutions of the transfer function and step response of the first-order delayed system are as follows:

[0069]

[0070] Where τ is the time constant, the time required for the system to reach 63.2% of its steady state, characterizing the response speed, in seconds.

[0071] Prediction is performed using the parameter identification results, and the prediction effect is as follows: Figure 2 As shown in Figure 2, the prediction effects of the first-order model, the second-order overdamped model, and the second-order overdamped combined model with friction compensation are compared. The figure includes three sub-figures: left turn fitting verification (a), right turn fitting verification (b), and model prediction accuracy (c). It can be seen from the figure that the prediction curves of the second-order overdamped model and the combined model can closely follow the trend of the measured steering angle and accurately reproduce the typical "S"-shaped overdamped response characteristics of the steering system. However, the prediction curve of the first-order model deviates significantly from the measured value and cannot accurately depict the steering dynamic process. At the same time, the combined model with friction compensation has a better overall fitting degree and higher prediction accuracy than the pure second-order overdamped model.

[0072] S104, Composite Controller Setup

[0073] Based on the system identification results, a composite controller was built. This controller achieves phase lead compensation by integrating the desired steering angle and its first and second derivatives through dynamic feedforward terms based on the system inverse model, thereby eliminating system inertial delay and dynamic lag. Simultaneously, a continuous friction model is introduced for nonlinear friction compensation to suppress steady-state errors and improve nonlinear characteristics during low-speed and steering transitions. The controller can also switch model parameters according to the steering direction (left / right turn) to more accurately compensate for the actual dynamic characteristics of the system.

[0074]

[0075] Where, δ ff K represents the angle control value after feedforward compensation. ff For the inverse model gain, K ff =1 / K, used to cancel the amplification effect, where K is the steady-state gain; Turn at the target corner; The target turning angular velocity is calculated using a difference approximation. The unit is ° / s, where T S It is the control cycle; The target steering angle acceleration is calculated using second-order difference. Unit: ° / s 2 .

[0076] By inputting the left-turn and right-turn identification parameters respectively, the feedforward controllers for left turns and right turns can be obtained as follows:

[0077]

[0078] Where the target rotation angle δ d This is obtained through a path tracing algorithm, which is a non-linear pure tracking algorithm, as detailed below:

[0079] (1) Gain scheduling adaptive preview based on lateral error

[0080] The core objective of path tracking is to enable the vehicle to travel precisely along the reference trajectory and minimize the lateral error e. lat Traditional PP algorithms use a fixed pre-aiming distance L. d This presents a contradiction, L d Too short a length will result in a more sensitive vehicle response, but it is prone to oscillation; L d Too long a length results in a smoother vehicle response, but reduces tracking accuracy; therefore, a length based on e is considered. lat Dynamically adjust L d , when e lat When large, shorten L d This allows the vehicle to steer more aggressively to reduce errors; when e lat Hours, extend L dThis ensures the vehicle maintains a smooth driving motion. Combined with the control concept of "gain scheduling," which adjusts the system's current state (e... lat Adjust controller parameters (L) d An adaptive pre-aiming distance strategy is introduced, and a smoothing function is defined to dynamically adjust L. d The formula is as follows:

[0081]

[0082] in, The adjusted aiming distance. For lateral error; Used as a baseline aiming distance; For error adaptive aiming components; The vehicle's speed; This is the speed proportionality coefficient.

[0083] (2) Closed-loop control with lateral error feedback

[0084] Traditional Proportional Tracking (PP) algorithms are feedforward control based on geometric models, primarily relying on the geometric relationship between the vehicle and the path for calculations, and lack adaptability to actual tracking errors. When model errors or external disturbances exist, their tracking performance is significantly affected. Therefore, a geometric angle compensation strategy is proposed, which involves adding proportional feedback k of the lateral error to the geometric angle α. lat ·e lat The formula is as follows:

[0085]

[0086] Where, k lat This is the gain coefficient after experimental tuning, with a value ranging from 0.1 to 0.3; Represents the original geometric tracking angle; This indicates lateral error.

[0087] By dynamically adjusting the geometric tracking angle at the forward viewpoint based on lateral error, an adaptive correction mechanism is introduced into the system, enabling the vehicle to more actively eliminate path deviations. This compensation mechanism can correct the geometric model parameters in real time based on the current error, thereby suppressing error accumulation, accelerating convergence, and effectively improving the system's response speed and steady-state accuracy to path tracking deviations. To prevent anomalies during the tracking process, an upper and lower limit protection mechanism for the pre-aiming distance is set. The algorithm will be referred to as A_PP below.

[0088] To verify the performance of the above algorithm, a real-vehicle test was conducted, with the vehicle running straight without load at a speed of 1 m / s. Based on the test results of the PP algorithm, L was set... d =10m, error gain coefficient k lat The values ​​were 0.1, 0.2, and 0.3 respectively. The experimental results are as follows: Figure 3 As shown in Table 3, when k lat When k = 0.2, the tracking performance is optimal, with MAE of 0.0375m, SD of 0.0106m, and MAX of 0.0672m. Therefore, k is ultimately set to... lat It is 0.2. Compared to the PP algorithm (L... d When the value is 10m, the MAE is 0.0640m, the SD is 0.0293m, and the Max is 0.1698m. The MAE, SD, and Max decreased by 41.41%, 63.82%, and 60.42%, respectively.

[0089] Table 3. Statistics of Lateral Error in Linear Tracking of A_PP Algorithm

[0090] <![CDATA[k lat1 =0.1]]> 0.0458 0.0119 0.0819 <![CDATA[k lat2 =0.2]]> 0.0375 0.0106 0.0672 <![CDATA[k lat2 =0.3]]> 0.0412 0.0141 0.1027

[0091] In summary, compared to PP, A_PP, characterized by adaptive preview and error compensation, maintains a relatively simple structure with only a small increase in computational load. Adaptive preview changes the control strategy from a look-ahead perspective, enabling the vehicle to respond quickly when large errors occur, thus preventing further error amplification. Simultaneously, proportional feedback of lateral error is added to the geometric angle α to correct for the current error. The combination of these two approaches enhances performance. However, in practical applications, the improvement is limited due to the reliance on geometric models and the lack of dynamic characteristic compensation, resulting in only a limited improvement in tracking accuracy.

[0092] Dynamic compensation and adaptive correction (hereinafter referred to as FF_A_PP) are performed on the basis of A_PP to achieve path tracking control that is more in line with the characteristics of real vehicles. FF_A_PP adds dynamic feedforward compensation based on system identification parameters to A_PP to improve response speed and reduce phase lag and steady-state error caused by system inertia, friction, etc.

[0093] The test site and design are consistent with the A_PP algorithm, and the real vehicle tracking effect is as follows. Figure 4 As shown in Table 4, the MAE of the three sets of experiments were 0.0264, 0.0289, and 0.0273m, respectively; the SD were 0.0121, 0.0105, and 0.0139m, respectively; and the MAX were 0.0612, 0.0542, and 0.0649m, respectively. The differences in tracking performance were not significant. Comparing the worst result of the three sets with the best result of the A_PP algorithm (MAE of 0.0375m, SD of 0.0106m, and MAX of 0.0672m), the MAE decreased by 22.93%, the SD remained basically the same, indicating that both algorithms have good stability, and the MAX decreased by 19.35%.

[0094] Table 4. Statistics of Lateral Errors in FF_A_PP Algorithm Tracking

[0095] 1 0.0264 0.0121 0.0612 2 0.0289 0.0105 0.0542 3 0.0273 0.0139 0.0649

[0096] Comparing the path tracking performance of the three algorithms PP, A_PP, and FF_A_PP, FF_A_PP showed the best overall tracking performance, with an optimal MAE of 0.264m and an SD of 0.0121m. Steady-state accuracy was significantly improved, and overshoot was reduced. This indicates that the composite control method proposed in this invention significantly improves path tracking accuracy and stability, while reducing overshoot.

[0097] like Figure 5 As shown, based on the same inventive concept, this invention also discloses a self-propelled agricultural machinery path tracking adaptive feedforward composite control system, used to execute any of the above-mentioned self-propelled agricultural machinery path tracking adaptive feedforward composite control methods, including:

[0098] The data acquisition module is used to collect steering angle response data of the steering system;

[0099] The model identification module is used to establish a dynamic model of the steering system based on the steering angle response data. The dynamic model includes a second-order overdamped model.

[0100] The feedback control module is used to acquire the desired path and current pose, calculate the lateral error, dynamically adjust the pre-aiming distance of the pure tracking algorithm based on the lateral error, and calculate the pure tracking geometric tracking angle using the adjusted pre-aiming distance to obtain the target turning angle.

[0101] The feedforward control module is used to calculate the feedforward compensated angle control quantity based on the inverse model of the dynamic model, with the target angle as input.

[0102] The output module is used to output the feedforward compensated angle control quantity to the steering actuator.

[0103] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0104] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A self-propelled agricultural machinery path tracking adaptive feedforward composite control method, characterized in that, Includes the following steps: S1. Collect the steering angle response data of the steering system, and establish a dynamic model of the steering system based on the steering angle response data. The dynamic model includes a second-order overdamped model characterizing the dynamic characteristics of the steering system. S2. Obtain the desired path and current pose, calculate the lateral error, dynamically adjust the pre-aiming distance of the pure tracking algorithm based on the lateral error, and use the adjusted pre-aiming distance to calculate the pure tracking geometric tracking angle to obtain the target turning angle; S3. Construct a feedforward controller based on the dynamic model. The feedforward controller takes the target rotation angle as input and calculates the rotation angle control quantity after feedforward compensation based on the target rotation angle. S4. Output the feedforward compensated angle control quantity to the steering actuator.

2. The method according to claim 1, characterized in that, Outliers were removed from the collected corner response data using the 3σ criterion, missing values ​​were filled in using linear interpolation, and the data was smoothed using a Savitzky-Golay filter.

3. The method according to claim 1, characterized in that, The dynamic model also includes a continuous friction model for compensating for nonlinear friction, wherein the continuous friction model is: ； Among them, f c f is the Coulomb friction coefficient. v The coefficient of viscous friction, The angular velocity of the target turning angle. It is a smooth approximation of the sign function.

4. The method according to claim 1, characterized in that, The transfer function form of the second-order overdamped model is as follows: ； Where K is the steady-state gain, T d ω is the delay time. n Let be the natural frequency, ζ be the damping ratio satisfying ζ>1, and s be the Laplace operator. For delayed terms; It is a unit step function.

5. The method according to claim 1, characterized in that, The parameters of the dynamic model are identified using the nonlinear least squares method. The goal is to minimize the squared error between the actual turning angle output and the predicted output of the dynamic model. The optimal parameters are solved iteratively using the Levenberg-Marquardt algorithm. The parameters are identified independently for the left-turn and right-turn conditions.

6. The method according to claim 5, characterized in that, The feedforward controller is constructed based on the inverse model of the dynamic model, and uses model parameters that are independently identified for left turns and right turns respectively. The output feedforward compensated steering angle control quantity satisfies: ； Where, δ ff K represents the angle control value after feedforward compensation. ff For the inverse model gain, K ff =1 / K, used to cancel the amplification effect, where K is the steady-state gain; Turn at the target corner; The target turning angular velocity is calculated using a difference approximation. The unit is ° / s, where T S It is a control cycle; The target steering angle acceleration is calculated using second-order difference. Unit: ° / s 2 .

7. The method according to claim 5, characterized in that, When identifying the parameters of the dynamic model, lower and upper bound constraints are set independently for the left turn and right turn conditions to limit the physical reasonable range of the parameters.

8. The method according to claim 1, characterized in that, In the step of dynamically adjusting the aiming distance based on lateral error, the following adaptive scheduling strategy is adopted: ； in, The adjusted aiming distance. For lateral error; Used as a baseline aiming distance; For error adaptive aiming components; The vehicle's speed; This is the speed proportionality coefficient.

9. The method according to claim 8, characterized in that, After calculating the pure tracking geometric tracking angle, the proportional feedback of the lateral error is also superimposed on the geometric tracking angle to obtain the target turning angle; Superimposed geometric tracking angle satisfy: ； in, The preset lateral error gain coefficient has a value range of 0.1 to 0.

3. Represents the original geometric tracking angle; This indicates lateral error.

10. A self-propelled agricultural machinery path tracking adaptive feedforward composite control system, used to execute the tracking control method as described in any one of claims 1-9, characterized in that, include: The data acquisition module is used to collect steering angle response data of the steering system; The model identification module is used to establish a dynamic model of the steering system based on the steering angle response data. The dynamic model includes a second-order overdamped model. The feedback control module is used to acquire the desired path and current pose, calculate the lateral error, dynamically adjust the pre-aiming distance of the pure tracking algorithm based on the lateral error, and calculate the pure tracking geometric tracking angle using the adjusted pre-aiming distance to obtain the target turning angle. The feedforward control module is used to calculate the feedforward compensated angle control quantity based on the inverse model of the dynamic model and with the target angle as input. The output module is used to output the feedforward compensated angle control quantity to the steering actuator.

Images