Pure tracking control method and system for orchard tracked vehicle

Abstract

The invention provides an orchard tracked vehicle pure tracking control method and system, and the method comprises the steps: constructing a geometry-vector dynamic coupling model based on path geometric features and vector field analysis, quantifying local steering characteristics through a direction change sensing mechanism, introducing a curvature factor to represent path accumulated deformation, inhibiting noise interference in combination with a direction consistency correction term, improving curvature estimation of a three-point arc method, and generating multi-scale curvature characteristic data; based on a tracked vehicle kinematics model, analyzing a coupling relationship between a transverse tracking error and a course tracking error, and generating coupling error state data; and inputting the multi-scale curvature characteristic data and the coupling error state data into a backstepping controller, analyzing and exporting a dynamic look-ahead distance explicit expression through a simultaneous kinematics equation and a backstepping control equation, generating a self-adaptive angular velocity control instruction, and outputting the self-adaptive angular velocity control instruction to an execution mechanism to drive a vehicle to steer. And adaptive optimization of the look-ahead distance along with the path curvature, the vehicle state and the tracking error is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of automatic navigation technology of agricultural machinery, and particularly relates to a pure tracking control method and system for orchard tracked vehicles. BACKGROUND

[0002] High-precision path tracking technology is the core and key to realize autonomous operation of agricultural machinery in complex environments such as orchards and fields, and its performance directly determines the refinement of operation quality and the economy of operation efficiency, and is an important embodiment of the intelligent level of smart agricultural equipment.

[0003] Among numerous path tracking control algorithms, the pure tracking algorithm is widely used in the navigation control of wheeled and tracked agricultural machinery due to its intuitive model, clear physical meaning of parameters, and light computing burden. The core idea of the algorithm is based on the "preview" mechanism, that is, the vehicle is controlled to move towards a "preview point" on the reference path in front, and the entire path is tracked by constantly tracking the moving preview point. The preview distance is the most critical parameter of the algorithm, which defines the distance of the preview point and directly affects the control performance of the algorithm: a shorter preview distance can improve the tracking accuracy of curved paths but easily causes system oscillation; a longer preview distance can ensure the stability of straight-line travel but reduces the tracking accuracy and response speed to curved paths.

[0004] However, the inherent limitations of the traditional pure tracking algorithm exposed in actual agricultural applications seriously restrict the further improvement of its performance. The primary limitation is reflected in the setting of the preview distance. Existing methods mostly set the preview distance as a fixed value, or only bind it with the longitudinal speed of the vehicle in a simple linear or nonlinear manner. This static or semi-static parameter strategy cannot adapt to the complex and variable working environment. For example, when the vehicle has a large lateral deviation or heading deviation due to ground slippage, positioning delay, etc., the fixed preview distance cannot accelerate the deviation correction, resulting in slow convergence; when the path curvature changes dramatically (such as encountering sharp turns or consecutive S-shaped curves), the fixed preview distance cannot perceive the change in path shape in advance and adjust in time, which easily leads to tracking overshoot, cutting curves, or even instability, seriously affecting the precision and safety of working in complex orchard inter-row operations.

[0005] Another more fundamental limitation stems from its core input-the accuracy and robustness of the calculation of path curvature. Traditional methods generally use the geometric principle of determining a circle based on three points to estimate the curvature (three-point circular arc method). This method can work under ideal conditions where the path points are uniform and smooth, but in engineering practice, due to unstable GNSS signals, discontinuous path planner output, and other factors, the reference path point sequence obtained is often discrete, noisy, or even non-uniform. In the face of such "ill-conditioned" paths, the classic three-point method becomes extremely fragile: it is extremely sensitive to noise and abnormal spacing between path points, and even a small coordinate disturbance can cause the calculated curvature value to jump dramatically, resulting in calculated results that are completely contrary to physical facts (such as infinite curvature). This unreliable, noisy curvature signal directly as input to the controller, will send the wrong steering instructions to the actuator, seriously damaging the control stability of the system, and becoming a bottleneck for improving tracking performance.

[0006] In summary, the prior art is difficult to solve the two coupled problems of "unreliable curvature estimation under complex paths" and "poor adaptive ability of look-ahead distance in dynamic environment" at the same time. Therefore, developing a pure tracking control method that can deeply integrate path geometric features to achieve high-robustness curvature estimation, and dynamically coupling path features, real-time state of tracked vehicles, and tracking errors, to achieve online adaptive optimization of look-ahead distance, is of great significance to significantly improve the path tracking accuracy, stability, and robustness of electric orchard tracked vehicles in complex unstructured environments, and has become a key problem that needs to be broken through in the development of the field. SUMMARY

[0007] To this end, an object of the present application is to propose an orchard tracked vehicle pure tracking control method and system to solve the problems mentioned in the background art and overcome the deficiencies in the prior art.

[0008] To achieve the above-mentioned object, the present application provides an orchard tracked vehicle pure tracking control method, comprising:

[0009] Based on the sequence of reference path points in the orchard, the three-point circular arc curvature estimation method is optimized by integrating the local direction change, bending degree, and direction consistency features of the path, to generate noise-resistant multi-scale curvature feature data;

[0010] Based on the real-time pose of the vehicle and the reference path, the lateral tracking error and the heading tracking error are calculated, and the coupling relationship between the lateral tracking error, the heading tracking error, and the vehicle kinematic model is established to generate coupled error state data;

[0011] Based on the multi-scale curvature feature data and the coupled error state data, a control law is designed using the backstepping control method, and the dynamic look-ahead distance is analyzed in real time by solving the pure tracking geometric model.

[0012] generate adaptive angular velocity control instruction based on the dynamic look-ahead distance to drive the actuator to make the vehicle track the reference path.

[0013] As preferred, the three-point circle curvature estimation method is optimized by fusing the local direction change, bending degree and direction consistency characteristics of the path, including:

[0014] Calculate the direction angle and direction angle change of the forward vector and the backward vector formed by the continuous path points to perceive the local turning characteristics;

[0015] Calculate the arc length and chord length of the path, and quantify the path bending degree based on the ratio of the arc length and chord length of the path;

[0016] Calculate the cosine value of the included angle between the forward vector and the backward vector as a direction consistency correction factor to suppress noise;

[0017] Based on the direction angle change, the path bending degree and the direction consistency correction factor, the optimized curvature value is calculated by adaptive weight fusion.

[0018] As preferred, the control law is designed by using backstepping control method, and the dynamic look-ahead distance is analyzed in real time by combining with the pure pursuit geometric model, including:

[0019] Design a virtual control quantity for making the lateral tracking error converge according to the lateral tracking error;

[0020] Define a synthetic error variable between the heading tracking error and the virtual control quantity;

[0021] Design an angular velocity control law to make the synthetic error variable converge;

[0022] Combine the angular velocity control law with the angular velocity formula of the pure pursuit model to solve the explicit expression of the dynamic look-ahead distance, which is a function of path curvature, vehicle speed, lateral tracking error, heading tracking error and control gain.

[0023] As preferred, the dynamic look-ahead distance can be adaptively adjusted according to the tracked state of the tracked vehicle and the path curvature, and the adjustment mechanism is:

[0024] When the lateral tracking error or the heading tracking error increases, the dynamic look-ahead distance is automatically shortened to improve the convergence speed;

[0025] When the path curvature increases, the dynamic look-ahead distance is automatically shortened to enhance the tracking accuracy on the curve.

[0026] The embodiment of another aspect of the application provides a pure tracking control system for orchard tracked vehicles, comprising:

[0027] a curvature estimation module for optimizing a three-point circular arc curvature estimation method by fusing local direction change, bending degree and direction consistency features of a path based on an orchard reference path point sequence to generate multi-scale curvature feature data resistant to noise;

[0028] an error calculation module for calculating lateral tracking error and heading tracking error based on real-time vehicle pose and reference path, and establishing a coupling relationship between lateral tracking error, heading tracking error and a vehicle kinematics model to generate coupled error state data;

[0029] a dynamic look-ahead controller for designing a control law using backstepping control method based on the multi-scale curvature feature data and the coupled error state data, and real-time analyzing a dynamic look-ahead distance by solving a pure tracking geometric model;

[0030] generating an adaptive angular velocity control instruction based on the dynamic look-ahead distance;

[0031] an actuator driving module for converting the angular velocity control instruction into a tracked vehicle steering control signal to drive an actuator to make the vehicle track the reference path.

[0032] As a preference, the three-point circular arc curvature estimation method is optimized by fusing local direction change, bending degree and direction consistency features of a path, comprising:

[0033] calculating direction angles and direction angle change amounts of forward vectors and backward vectors formed by consecutive path points to perceive local turning characteristics;

[0034] calculating arc length and chord length of the path, and quantifying path bending degree based on a ratio of the arc length to the chord length;

[0035] calculating a cosine value of an included angle between the forward vector and the backward vector as a direction consistency correction factor for suppressing noise;

[0036] calculating an optimized curvature value by adaptive weight fusion based on the direction angle change amount, the path bending degree and the direction consistency correction factor.

[0037] As a preference, the control law is designed using backstepping control method, and the dynamic look-ahead distance is real-time analyzed by solving a pure tracking geometric model, comprising:

[0038] designing a virtual control amount for making the lateral tracking error converge according to the lateral tracking error;

[0039] defining a synthetic error variable between the heading tracking error and the virtual control amount;

[0040] a designed angular velocity control law that makes the synthesis error variable converge;

[0041] a solving the explicit expression of the dynamic look-ahead distance by combining the angular velocity control law with the angular velocity formula of the pure pursuit model, which is a function of path curvature, vehicle speed, lateral tracking error, heading tracking error and control gain.

[0042] As preferred, the dynamic look-ahead distance can be adaptively adjusted according to the tracked state of the tracked vehicle and the path curvature, and the adjustment mechanism is:

[0043] When the lateral tracking error or the heading tracking error increases, the dynamic look-ahead distance is automatically shortened to improve the convergence speed;

[0044] When the path curvature increases, the dynamic look-ahead distance is automatically shortened to enhance the tracking accuracy on a curve.

[0045] Compared with the prior art, the present application has the advantages and beneficial effects that:

[0046] The present application improves the accuracy and robustness of curvature calculation under complex paths through multi-scale curvature estimation, and realizes the global asymptotic stability of lateral and heading tracking errors through dynamic look-ahead distance control based on backstepping method, significantly improving the path tracking accuracy and robustness of electric tracked agricultural machines in complex orchard environments.

[0047] Additional aspects and advantages of the present application will be in part apparent and in part pointed out hereinafter. BRIEF DESCRIPTION OF DRAWINGS

[0048] The above and / or additional aspects and advantages of the present application will become apparent and be readily appreciated from the following description, including the accompanying drawings.

[0049] Figure 1 A flowchart of the present application, a multi-scale feature curvature estimation and dynamic look-ahead distance orchard tracked vehicle pure pursuit control method.

[0050] Figure 2 A schematic diagram of the geometric parameters involved in the multi-scale curvature estimation method of the present application (showing arc length, chord length, vector, etc.).

[0051] Figure 3 A schematic diagram of the kinematic model of the tracked vehicle in the present application (showing the relationship between the two side wheel speeds and the turning radius).

[0052] Figure 3bThe schematic diagram of the geometric relationship and error definition of the pure pursuit algorithm in the application (showing the global / local coordinate system, preview point, error e d , theta e , etc.).

[0053] Figure 4 The structure schematic diagram of the pure pursuit control system of the orchard tracked vehicle of the application. DETAILED DESCRIPTION

[0054] The embodiments of the application are described in detail below, examples of which are shown in the drawings, wherein the same or similar notations represent the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by reference to the drawings are exemplary and are intended to explain the application, and cannot be understood as a limitation of the application.

[0055] As shown in Figures 1-3 , the orchard tracked vehicle pure pursuit control method of the multi-scale feature curvature estimation and dynamic look-ahead distance of the embodiment of the application can specifically include:

[0056] Step S101: multi-scale curvature feature data generation;

[0057] Specifically, based on the orchard reference path point sequence, the three-point circular arc curvature estimation method is optimized by fusing the local direction change, bending degree and direction consistency features of the path, to generate multi-scale curvature feature data resistant to noise.

[0058] The optimization of the three-point circular arc curvature estimation method by fusing the local direction change, bending degree and direction consistency features of the path includes:

[0059] The direction angle and direction angle change of the forward vector and the backward vector formed by the continuous path points are calculated to perceive the local turning characteristics;

[0060] The arc length and chord length of the path are calculated, and the path bending degree is quantified based on the ratio of the arc length and chord length of the path;

[0061] The cosine value of the included angle of the forward vector and the backward vector is calculated as a direction consistency correction factor to suppress noise;

[0062] Based on the direction angle change, the path bending degree and the direction consistency correction factor, the optimized curvature value is calculated by adaptive weight fusion.

[0063] Please refer to Figure 2 , based on the input orchard reference path point sequence, a geometric-vector dynamic coupling model is constructed. i-1 (x i-1 , y i-1 ), P i (xi y i ), P i+1 (x i+1 y i+1 Construct the forward vector. With backward vector And calculate its direction angle ψ pre and ψ nex Calculate the original angle difference Δψ = ψ nex -ψ pre The instantaneous angle difference Δψ is then constrained to the interval [-π, π] through modular arithmetic to obtain a physically meaningful instantaneous angle difference. The sum of the lengths of the two line segments formed by the three points is calculated as the arc length L. arc Calculate the first and last points P i-1 With P i+1 The Euclidean distance is taken as the chord length L chord Calculate the path curvature σ = L arc / (L chord +μ), where μ is a very small positive number to prevent division by zero errors. Calculate the forward vector. With backward vector The cosine of the included angle, cosβ. Design an adaptive weighting factor. Here, γ is the scale parameter, used to dynamically balance the contributions of different features. Finally, through the fusion formula... Calculate the optimized curvature value and generate multi-scale curvature feature data.

[0064] Taking the navigation path of a tracked sprayer in an orchard as an example, the reference path consists of a sequence of coordinates of the center points between rows of fruit trees. Three consecutive points P are taken. i-1 :(0,0),P i :(2,0.5),P i+1 (3.5, 1.8), construct the forward and backward vectors and calculate their direction angles. The original angle difference, after modulo operation constraint, yields Δψ = 0.78 rad. Calculate the arc length L. arc (P i-1 To P i To P i+1 The distance between them is approximately 4.05 meters, and the chord length L is... chord (P i-1 To P i+1 The straight-line distance is approximately 3.92 meters. Introducing μ = 1e -5 To prevent division by zero, the curvature σ is calculated to be approximately 1.033 (4.05 / 3.92). The cosine of the angle between the two vectors is calculated to be approximately 0.72 (cosβ). The scale parameter γ is set to 0.5, and the adaptive weighting factor χ is set to approximately 0.62. Finally, the optimized curvature value κ at this point is calculated using the fusion formula. p ≈0.21m -1, which accurately captures the curvature characteristics of the path at the turning point, providing a high-robustness curvature input for subsequent control.

[0065] Step S102: coupling error state data generation;

[0066] Specifically, based on the real-time pose of the vehicle and the reference path, the lateral tracking error and the heading tracking error are calculated, and the coupling relationship between the lateral tracking error, the heading tracking error and the vehicle kinematic model is established to generate the coupling error state data.

[0067] Please refer to Figure 3 a and Figure 3 b, in the global coordinate system, define the vehicle mass center coordinates (x c ,y c ), the heading angle θ, and the path tangent angle θ d ; accordingly, the tracked vehicle kinematic model is established

[0068] In the local coordinate system, the Euclidean distance from the vehicle center to the preview point is the look-ahead distance L d , the x-axis distance from the vehicle center to the preview point is e l , and the angle between the vehicle heading and the preview point is the preview angle α. The angular velocity control quantity ω of the pure pursuit algorithm is derived accordingly The control quantity determines the tracking performance of the algorithm;

[0069] Specifically, the angular velocity control quantity ω is closely related to the vehicle forward speed v, the x-axis distance e l , and the look-ahead distance L d ;

[0070] Define the lateral tracking error e d as the perpendicular distance from the vehicle mass center to the reference path, and the heading tracking error θ e as the difference between the vehicle heading angle θ and the path tangent angle θ d ;

[0071] Based on the tracked vehicle kinematic characteristics, the coupling relationship model of the lateral tracking error rate and the heading tracking error rate and the vehicle longitudinal speed v and the steering angular velocity ω is established: where κ p is the curvature of the reference path;

[0072] Based on the coordinate transformation relationship, the global origin is moved to the vehicle mass center (x c ,y c ), and the relative coordinate difference is obtained. At the same time, the look-ahead distance is translated to the preview point in the local coordinate system, and the heading tracking error θ e is further considered to obtain the x-axis distance el with lateral tracking error e d and heading tracking error θ e , the display expression e l ≈e d +L d θ e ;

[0073] Based on the coupling relationship model, a coupling angular velocity control quantity expression containing lateral tracking error e d and heading tracking error θ e is generated.

[0074] Suppose a orchard track spraying machine travels at a speed of v = 1.5 m / s. In the global coordinate system, its center of mass position is (5.2, 3.1), and the heading angle θ = 0.4 rad. At this time, the tangent angle θ d of the corresponding point on the reference path is 0.6 rad. According to the kinematic model, the heading tracking error θ e = θ - θ d = 0.2 rad. The vertical distance of the vehicle center of mass to the reference path, i.e., the lateral tracking error e d = 0.15 m. Based on the coupling relationship model, the lateral tracking error rate of change e The heading tracking error rate of change e (where κ p is provided by S101, and is assumed to be 0.1). Through coordinate transformation, the x-axis distance e l of the vehicle center to the preview point in the local coordinate system can be further calculated, which is determined by e d and θ e . This process generates coupling error data containing key states (e d , θ e ), which provides accurate feedback information for the controller.

[0075] Step S103: Dynamic angular velocity control instruction generation;

[0076] Specifically, based on the multi-scale curvature feature data and the coupling error state data, a control law is designed using backstepping control method, and a dynamic preview distance is analyzed in real time by combining with a pure pursuit geometric model. An adaptive angular velocity control instruction is generated based on the dynamic preview distance.

[0077] Wherein, the control law is designed using backstepping control method, and a dynamic preview distance is analyzed in real time by combining with a pure pursuit geometric model, including:

[0078] According to the lateral tracking error, a virtual control quantity is designed to make the lateral tracking error converge;

[0079] define a composite error variable z = e

[0080] design an angular velocity control law ω1 = vκ

[0081] solving the angular velocity control law and the angular velocity formula of the pure pursuit model, an explicit expression of the dynamic look-ahead distance L

[0082] input multi-scale curvature feature data (κ p ) and coupling error state data (e d , θ e ) into the controller based on the backstepping method. First, design a virtual control quantity v with respect to the lateral tracking error e d , where k1 is a control gain greater than zero, so that e d is asymptotically stable;

[0083] define a composite error variable z = e representing the deviation of the actual heading tracking error from the ideal heading tracking error;

[0084] design an angular velocity control law ω1 = vκ p -k2vθ e -k3z, where k2, k3 are control gains greater than zero, forcing the composite error z to asymptotically converge to zero;

[0085] based on the angular velocity formula of the pure pursuit geometric model , solve it together with the backstepping angular velocity control law;

[0086] neglecting high-order small terms or solving iteratively, analytically derive an explicit expression of the dynamic look-ahead distance L d , which is a function of the vehicle longitudinal speed v, the lateral tracking error e d , the heading tracking error θ e , the path curvature κ p , and the control gains k1, k2, k3, and substitute the real-time solved L d into the pure pursuit model to finally generate adaptive angular velocity control instructions.

[0087] In the above example, the controller receives the following inputs: curvature κ p = 0.1, lateral tracking error e d = 0.15 m, heading tracking error θ e = -0.2 rad, and speed v = 1.5 m / s. Set the control gains k1 = 0.8, k2 = 1.2, and k3 = 1.5. First, design a virtual control quantity v p = 0.8e e + 1.2θ d + 1.5z. define the synthesis error Then, the backstepping angular velocity control law is calculated: ω1=vκ p -k2vθ e -k3z=1.5*0.1-1.2*1.5*(-0.2)-1.5*(-0.12)=0.15+0.36+0.18=0.69 rad / s. This control law is combined with the pure pursuit geometric formula ω=(2*v*sin(α)) / L d together, the explicit expression of the required look-ahead distance L d at this moment is derived analytically and solved (e.g. Ld≈4.32 m) instead of using a fixed value. Finally, the dynamically solved L d is substituted into the pure pursuit model to generate the adaptive angular velocity control command ω=0.69 rad / s, which realizes dynamic compensation for the path curvature and tracking error.

[0088] Further, the dynamic look-ahead distance can be adaptively adjusted according to the tracked state of the tracked vehicle and the path curvature, and the adjustment mechanism is:

[0089] When the lateral tracking error or the heading tracking error increases, the dynamic look-ahead distance is automatically shortened to improve the convergence speed;

[0090] When the path curvature increases, the dynamic look-ahead distance is automatically shortened to enhance the tracking accuracy on the curve.

[0091] Step S104, instruction execution;

[0092] The generated adaptive angular velocity control command is output to the execution mechanism driving module of the vehicle. This module controls the speed difference of the two motors according to the command, thereby accurately controlling the steering angular velocity ω of the vehicle, driving the vehicle to track the reference path, and realizing high-precision and high-robustness autonomous driving.

[0093] The final angular velocity control command ω=0.69 rad / s generated by the above process is sent to the lower execution mechanism driving module of the tracked pesticide spraying machine. This module is usually a motor controller, and the received command means that the vehicle needs to generate a counterclockwise (positive) steering angular velocity. The controller will analyze the angular velocity command into the target speed difference (Δn) of the left and right drive motors according to the kinematic model of the vehicle. For example, if the left motor reduces the speed and the right motor increases the speed, the required steering effect can be generated. The driving module accurately adjusts the torque output of the two motors to make their actual speed difference stably track the command requirements, thereby driving the tracked vehicle to steer at the calculated optimal angular velocity, accurately reducing the lateral deviation and heading deviation, and realizing stable and smooth autonomous path tracking.

[0094] In another embodiment, the present application provides a pure tracking control system for orchard tracked vehicles, as shown, comprising: Figure 4

[0095] a curvature estimation module for optimizing the three-point circle arc curvature estimation method by fusing the local direction change, bending degree and direction consistency features of the path, based on the orchard reference path point sequence, to generate multi-scale curvature feature data resistant to noise;

[0096] wherein the optimization of the three-point circle arc curvature estimation method by fusing the local direction change, bending degree and direction consistency features of the path comprises:

[0097] calculating the direction angles and direction angle changes of the forward and backward vectors formed by consecutive path points to perceive the local turning characteristics;

[0098] calculating the arc length and chord length of the path, and quantifying the path bending degree based on the ratio of the arc length to the chord length of the path;

[0099] calculating the cosine of the included angle of the forward and backward vectors as a direction consistency correction factor to suppress noise;

[0100] based on the direction angle changes, the path bending degree and the direction consistency correction factor, calculating the optimized curvature value through adaptive weight fusion.

[0101] Please refer to Figure 2 , based on the input orchard reference path point sequence, a geometric-vector dynamic coupling model is constructed. i-1 (x i-1 , y i-1 ), P i (x i , y i ), P i+1 (x i+1 , y i+1 ), the forward vector and the backward vector are constructed and their direction angles ψ pre and ψ nex are calculated. The original angle difference Δψ = ψ nex - ψ pre is calculated and is constrained to the interval [-π, π] through modulo operation to obtain the instantaneous angle difference Δψ with clear physical meaning. The sum of the lengths of the two line segments formed by three points is calculated as the arc length L arc , and the Euclidean distance between the first and last points P i-1 and P i+1 is calculated as the chord length L chord . The path bending degree σ = L arc / (L chord ​+ μ), where μ is a small positive number to prevent division by zero. The forward vector and the backward vector are calculated. The adaptive weight factor is designed, where γ is a scale parameter to dynamically balance the contribution of different features. Finally, the optimized curvature value is calculated by the fusion formula to generate the multi-scale curvature feature data.

[0102] Take the navigation path of a orchard working track-type spraying machine as an example. The reference path is composed of a series of center point coordinate sequences between the rows of fruit trees. Take the consecutive three points P i-1 :(0, 0), P i :(2, 0.5), P i+1 :(3.5, 1.8) to construct the forward vector and the backward vector and calculate the direction angle. The original angle difference is constrained by the modulo operation to obtain Δψ = 0.78 rad. The arc length L arc (the distance from P i-1 to P i to P i+1 ) is about 4.05 meters, and the chord length L chord (the straight-line distance from P i-1 to P i+1 ) is about 3.92 meters. Introduce μ = 1e -5 to prevent division by zero, and obtain the bending degree σ = 4.05 / 3.92 ≈ 1.033. Calculate the cosine of the angle between the two vectors cosβ ≈ 0.72. Set the scale parameter γ = 0.5, and the adaptive weight factor χ ≈ 0.62. Finally, the optimized curvature value κ p at this point is calculated by the fusion formula to be κ -1 ≈ 0.21 m c , which accurately captures the bending characteristics of the path at the turning point and provides a high-robustness curvature input for subsequent control.

[0103] An error calculation module is configured to calculate lateral tracking error and heading tracking error based on the real-time pose of the vehicle and the reference path, and to establish a coupling relationship between the lateral tracking error, the heading tracking error, and a kinematic model of the vehicle, thereby generating coupled error state data.

[0104] Please refer to Figure 3 a and Figure 3 b, in the global coordinate system, define the vehicle mass center coordinates (x c , y c ), the heading angle θ, and the path tangent angle θ d ; and accordingly, establish a kinematic model of the track-type vehicle

[0105] In the local coordinate system, the Euclidean distance from the vehicle center to the preview point is the look-ahead distance L d, the x-axis distance from the vehicle center to the preview point is e l , the angle between the vehicle body heading and the preview point is the preview angle a. Thus the angle velocity control quantity of the pure pursuit algorithm is derived as The control quantity determines the tracking performance of the algorithm;

[0106] Specifically, the angle velocity control quantity ω is closely related to the vehicle forward speed v, the x-axis distance e l , and the look-ahead distance L d .

[0107] The lateral tracking error e d is defined as the vertical distance from the vehicle center to the reference path, and the heading tracking error θ e is the difference between the vehicle heading angle θ and the path tangent angle θ d .

[0108] Based on the kinematic characteristics of the tracked vehicle, the lateral tracking error rate and the heading tracking error rate and the coupling relationship model of the vehicle longitudinal speed v and the steering angle velocity ω are established as follows: where κ p is the curvature of the reference path;

[0109] Based on the coordinate transformation relationship, the global origin is moved to the vehicle center (x c , y c ), and the relative coordinate difference is obtained. At the same time, the preview point due to the look-ahead distance is translated and rotated to the local coordinate system. Further considering the heading tracking error θ e , the x-axis distance e l from the vehicle center to the preview point is obtained, and the explicit expression of the lateral tracking error e d and the heading tracking error θ e is e l ≈e d +L d θ e .

[0110] Based on the coupling relationship model, the coupling angle velocity control quantity expression containing the lateral tracking error e d and the heading tracking error θ e is generated.

[0111] Suppose a tracked orchard pesticide sprayer travels at a speed of v = 1.5 m / s. In the global coordinate system, its center position is (5.2, 3.1), and the heading angle θ = 0.4 rad. At this time, the tangent angle θ d of the corresponding point on the reference path is 0.6 rad. According to the kinematic model, the heading tracking error θ e = θ - θ d .= 0.2 rad. The lateral tracking error e d = 0.15 m. Based on the coupling relationship model, the lateral tracking error rate of change The heading tracking error rate of change (where κ p provided by S101, assuming 0.1). Through coordinate transformation, the x-axis distance e l from the vehicle center to the preview point in the local coordinate system can be further calculated d , which is determined by e e and θ d . This process generates coupling error data containing key states (e e , θ p ), providing accurate feedback information for the controller.

[0112] A dynamic preview controller is configured to design a control law using backstepping control method based on the multi-scale curvature feature data and the coupling error state data, and to calculate a dynamic preview distance in real time by solving the pure pursuit geometric model.

[0113] Generate adaptive angular velocity control instructions based on the dynamic preview distance.

[0114] The control law is designed using backstepping control method, and the dynamic preview distance is calculated by solving the pure pursuit geometric model, including:

[0115] Design a virtual control quantity for the lateral tracking error e

[0116] Define a synthetic error variable between the heading tracking error e

[0117] Design an angular velocity control law that makes the synthetic error variable converge;

[0118] Solve the explicit expression of the dynamic preview distance by combining the angular velocity control law with the angular velocity formula of the pure pursuit model, which is a function of path curvature, vehicle speed, lateral tracking error, heading tracking error, and control gain.

[0119] Input multi-scale curvature feature data (κ p ) and coupling error state data (e d , θ e ) into the controller based on backstepping method. First, design a virtual control quantity d for the lateral tracking error e where k1 is a control gain greater than zero, so that e d is asymptotically stable.

[0120] Define the synthetic error variable characterize the deviation of the actual course tracking error from the ideal course tracking error;

[0121] Design the angular velocity control law ω1 = vκ p - k2vθ e - k3z, where k2, k3 are positive control gains that force the synthetic error z to asymptotically converge to zero;

[0122] Based on the pure pursuit geometric model angular velocity formula Solve the simultaneous equations with the backstepping angular velocity control law;

[0123] Neglect the high-order small terms or solve analytically from the simultaneous equations to derive the explicit expression of the dynamic look-ahead distance L d as a function of the vehicle longitudinal speed v, lateral tracking error e d , course tracking error θ e , path curvature κ p , and control gains k1, k2, k3, and substitute the real-time solved L d into the pure pursuit model to finally generate the adaptive angular velocity control command.

[0124] Taking the above example, the controller receives the following inputs: curvature κ p = 0.1, lateral tracking error e d = 0.15 m, course tracking error θ e = -0.2 rad, and speed v = 1.5 m / s. Set the control gains k1 = 0.8, k2 = 1.2, and k3 = 1.5. First, design the virtual control quantity Define the synthetic error Next, calculate the backstepping angular velocity control law: ω1 = vκ p - k2vθ e - k3z = 1.5 * 0.1 - 1.2 * 1.5 * (-0.2) - 1.5 * (-0.12) = 0.15 + 0.36 + 0.18 = 0.69 rad / s. Substitute this control law with the pure pursuit geometric formula ω = (2 * v * sin(α)) / L d to solve the explicit expression of the required look-ahead distance L d at this time (e.g., L d ≈ 4.32 m) instead of using a fixed value. Finally, substitute the dynamically solved L d into the pure pursuit model to generate the adaptive angular velocity control command ω = 0.69 rad / s, achieving dynamic compensation for path curvature and tracking error.

[0125] Further, the dynamic look-ahead distance can be adaptively adjusted according to the tracked state of the tracked vehicle and the path curvature, and the adjustment mechanism is as follows:

[0126] When the lateral tracking error or the heading tracking error increases, the dynamic look-ahead distance is automatically shortened to improve the convergence speed.

[0127] When the path curvature increases, the dynamic look-ahead distance is automatically shortened to enhance the tracking accuracy of the curve.

[0128] An actuator driving module is configured to convert the angular velocity control instruction into a tracked vehicle steering control signal to drive the actuator to make the vehicle track the reference path.

[0129] The generated adaptive angular velocity control instruction is output to the actuator driving module of the vehicle. The module controls the speed difference between the two motors according to the instruction, thereby accurately controlling the steering angular velocity ω of the vehicle, driving the vehicle to track the reference path, and realizing high-precision and high-robustness autonomous driving.

[0130] The final angular velocity control instruction ω=0.69 rad / s generated by the above process is sent to the lower actuator driving module of the tracked pesticide spraying machine. The module is usually a motor controller, and the instruction received by the module means that the vehicle needs to generate a counterclockwise (positive) steering angular velocity. The controller will analyze the angular velocity instruction into the target speed difference (Δn) of the left and right drive motors according to the kinematic model of the vehicle. For example, if the left motor reduces the speed and the right motor increases the speed, the required steering effect can be generated. The driving module accurately adjusts the torque output of the two motors to make the actual speed difference stably track the instruction requirement, thereby driving the tracked vehicle to steer at the calculated optimal angular velocity, accurately reducing the lateral deviation and heading deviation, and realizing stable and smooth autonomous path tracking.

[0131] The present application improves the accuracy and robustness of curvature calculation under complex paths through multi-scale curvature estimation, and realizes the global asymptotic stability of lateral and heading tracking errors through dynamic look-ahead distance control based on backstepping method, significantly improving the path tracking accuracy and robustness of the electric tracked agricultural machine in complex orchard environment.

[0132] The technical scheme of the present application significantly improves the comprehensive performance of path tracking through multi-scale feature curvature estimation and dynamic look-ahead distance optimization based on backstepping control. First, by constructing a geometric-vector dynamic coupling model, multi-dimensional features such as direction change perception, path curvature and direction consistency correction are fused, and the defects of traditional three-point arc method such as noise sensitivity and large fluctuation of estimation results are completely overcome, thereby providing a high-precision and high-robustness curvature input for the control system and ensuring the reliability of the decision information from the source.

[0133] Secondly, the application creatively deduces the explicit analytic expression of the dynamic look-ahead distance based on the backstepping control method, and realizes the accurate adaptive matching of the look-ahead distance, the path curvature, the vehicle speed, the lateral tracking error and the heading tracking error. This mechanism not only solves the contradiction between the convergence speed and the tracking accuracy of the fixed look-ahead distance, but also theoretically guarantees the global asymptotic stability of the closed-loop system, so that the vehicle can quickly converge when the lateral tracking error is large, accurately track in the sharp curve section, and keep stable when driving in a straight line, thereby completely avoiding the blindness of the traditional experience parameter tuning or optimization search method.

[0134] Finally, the synergistic effect of the above-mentioned scheme of the application makes the scheme exhibit excellent comprehensive performance in actual complex agricultural environments. The system can effectively resist path noise, uneven ground and vehicle skidding and other disturbances, significantly improve the tracking accuracy, adaptability and overall robustness of the electric crawler-type agricultural machine in unstructured environments such as orchards, and provide a reliable technical guarantee for high-precision autonomous operation of agricultural machinery.

[0135] In the description of the present specification, the description of the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" and the like means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In the present specification, the illustrative description of the above terms does not necessarily refer to the same embodiment or example. Moreover, the described specific features, structures, materials or characteristics can be combined in any one or more embodiments or examples in a suitable manner.

[0136] It is not difficult for those skilled in the art to understand that the present application includes any combination of the above-mentioned description of the summary and detailed description and the parts shown in the drawings, which is limited in length and is brief in the specification. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application shall be included in the protection scope of the present application.

[0137] Although the embodiments of the present application have been shown and described above, it should be understood that the above-mentioned embodiments are exemplary and cannot be understood as limiting the present application, and those skilled in the art can make changes, modifications, replacements and variations to the above-mentioned embodiments without departing from the principles and purposes of the present application within the scope of the present application. The scope of the present application is defined by the appended claims and their equivalents.

Claims

1. An orchard track-type-vehicle pure pursuit control method, characterized by, The method comprises the following steps: Based on the sequence of orchard reference path points, the three-point circular arc curvature estimation method is optimized by fusing the local direction change, bending degree and direction consistency features of the path to generate multi-scale curvature feature data resistant to noise; Based on the real-time pose of the vehicle and the reference path, the lateral tracking error and the heading tracking error are calculated, and the coupling relationship between the lateral tracking error, the heading tracking error and the vehicle kinematics model is established to generate coupled error state data; Based on the multi-scale curvature feature data and the coupled error state data, a control law is designed using backstepping control method, and a dynamic look-ahead distance is solved in real time by combining with the pure pursuit geometric model; Based on the dynamic look-ahead distance, an adaptive angular velocity control instruction is generated to drive the actuator, so that the vehicle tracks the reference path.

2. The orchard track-type-vehicle pure pursuit control method of claim 1, wherein, The optimization of the three-point circular arc curvature estimation method by fusing the local direction change, bending degree and direction consistency features of the path comprises: The direction angle and the direction angle change of the forward vector and the backward vector formed by the continuous path points are calculated to perceive the local turning characteristics; The arc length and the chord length of the path are calculated, and the bending degree of the path is quantified based on the ratio of the arc length to the chord length; The cosine value of the included angle between the forward vector and the backward vector is calculated as a direction consistency correction factor to suppress noise; Based on the direction angle change, the path bending degree and the direction consistency correction factor, the optimized curvature value is calculated by adaptive weight fusion.

3. The orchard track-type vehicle pure pursuit control method of claim 1, wherein, The dynamic look-ahead distance is solved in real time by designing a control law using backstepping control method and combining with the pure pursuit geometric model, which comprises: According to the lateral tracking error, a virtual control quantity is designed to make the lateral tracking error converge; A synthetic error variable between the heading tracking error and the virtual control quantity is defined; An angular velocity control law is designed to make the synthetic error variable converge; The angular velocity control law is combined with the angular velocity formula of the pure pursuit model to solve the explicit expression of the dynamic look-ahead distance, which is a function of path curvature, vehicle speed, lateral tracking error, heading tracking error and control gain.

4. The orchard track-type-vehicle pure pursuit control method of claim 1, wherein, The dynamic look-ahead distance can be adaptively adjusted according to the tracked state of the tracked vehicle and the path curvature, and the adjustment mechanism is: When the lateral tracking error or the heading tracking error increases, the dynamic look-ahead distance is automatically shortened to improve the convergence speed; When the path curvature increases, the dynamic look-ahead distance is automatically shortened to enhance the tracking accuracy on the curve.

5. An orchard track-type vehicle pure pursuit control system, characterized by, The method comprises the following steps: A curvature estimation module is used to optimize the three-point circular arc curvature estimation method by fusing the local direction change, bending degree and direction consistency features of the path based on the sequence of orchard reference path points to generate multi-scale curvature feature data resistant to noise; An error calculation module is used to calculate the lateral tracking error and the heading tracking error based on the real-time pose of the vehicle and the reference path, and to establish the coupling relationship between the lateral tracking error, the heading tracking error and the vehicle kinematics model to generate coupled error state data; a dynamic look-ahead controller configured to design a control law using backstepping control method based on the multi-scale curvature feature data and the coupling error state data, and to solve a dynamic look-ahead distance in real time by solving a pure pursuit geometric model; generate an adaptive angular velocity control command based on the dynamic look-ahead distance; an actuator driving module configured to convert the angular velocity control command into a tracked vehicle steering control signal to drive an actuator to make the vehicle track a reference path.

6. The orchard track vehicle pure pursuit control system of claim 5, wherein, The three-point arc curvature estimation method is optimized by fusing the local direction change, bending degree and direction consistency features of the path, including: calculating the direction angle and direction angle change of the forward and backward vectors formed by the continuous path points to perceive the local turning characteristics; calculating the arc length and chord length of the path, and quantifying the path bending degree based on the ratio of the arc length to the chord length of the path; calculating the cosine value of the included angle between the forward and backward vectors as a direction consistency correction factor to suppress noise; based on the direction angle change, the path bending degree and the direction consistency correction factor, the optimized curvature value is calculated by adaptive weight fusion.

7. The orchard track vehicle pure pursuit control system of claim 5, wherein, The control law is designed using backstepping control method, and the dynamic look-ahead distance is solved in real time by solving the pure pursuit geometric model, including: design a virtual control quantity for making the lateral tracking error converge according to the lateral tracking error; define a synthetic error variable between the heading tracking error and the virtual control quantity; design an angular velocity control law to make the synthetic error variable converge; the angular velocity control law and the angular velocity formula of the pure pursuit model are solved to obtain the explicit expression of the dynamic look-ahead distance, which is a function of path curvature, vehicle speed, lateral tracking error, heading tracking error and control gain.

8. The orchard track vehicle pure pursuit control system of claim 5, wherein, The dynamic look-ahead distance can be adaptively adjusted according to the tracked vehicle state and the path curvature, and the adjustment mechanism is: when the lateral tracking error or the heading tracking error increases, the dynamic look-ahead distance is automatically shortened to improve the convergence speed; when the path curvature increases, the dynamic look-ahead distance is automatically shortened to enhance the tracking accuracy on the curve.

Images