A method and device for planning a dynamic speed profile of an unmanned agricultural machine based on timing optimization

Abstract

The application discloses a kind of unmanned agricultural machine dynamic speed profile planning method and equipment based on timing optimization, it is related to the technical field of agricultural machinery automation control and intelligent path planning.The method comprises the following specific steps: step 1 receives path point sequence, agricultural machine state, reference speed, dynamics parameters and dynamic obstacle safety time window;Step 2 constructs the multi-objective function of efficiency, smoothness and agronomic requirements with adjacent path point time interval as decision variable, and safety and dynamics constraints are included;Step 3 uses relaxation convex feasible set algorithm, converts non-convex problem into quadratic programming sub-problem and iteratively solves;Step 4 generates path point timestamp and smooth speed, acceleration, jerk profile, and outputs to underlying motion controller.The method enables unmanned agricultural machine to generate safety, efficiency, smooth and agronomic requirements speed instruction in real time in dynamic farmland environment, and has important theoretical and application value.

Description

Technical Field

[0001] This invention relates to the field of agricultural machinery automation control technology, and in particular to a speed planning method for unmanned agricultural machinery to operate autonomously in dynamic and uncertain environments. Specifically, it is a dynamic speed profile planning method and device based on time-series optimization. Background Technology

[0002] With the development of precision agriculture, the application of unmanned agricultural machinery (such as unmanned tractors, unmanned harvesters, and unmanned sprayers) is becoming increasingly widespread. One of its core technologies is trajectory planning, which aims to generate a safe, efficient, and agronomically compliant trajectory. Trajectory planning typically employs a hierarchical architecture, first performing geometric path planning and then velocity planning.

[0003] Existing speed planning methods for unmanned agricultural machinery mostly focus on static environments or employ simple constant speed, start-stop strategies, which are insufficient to cope with complex dynamic scenarios in the field, such as avoiding suddenly appearing obstacles (personnel, animals, other agricultural machinery), achieving spatiotemporal obstacle avoidance in multi-machine collaboration, and adaptively adjusting speed to ensure stability on slopes or rugged terrain. Some methods use search-based or sampling-based strategies, but the discreteness may lead to non-optimal and non-smooth solutions; others use parametric curves (such as Bézier curves), but these methods struggle to handle complex spatiotemporal coupling constraints.

[0004] Therefore, there is an urgent need for a velocity profile planning method that can directly handle dynamic constraints, ensure smooth motion, and is computationally efficient, in order to meet the high requirements of modern smart agriculture for the autonomous and intelligent operation of unmanned agricultural machinery. Summary of the Invention

[0005] This invention discloses a time-series optimization-based dynamic speed profile planning method and device for unmanned agricultural machinery, addressing the technical problem that existing unmanned agricultural machinery struggles to simultaneously guarantee operational safety, efficiency, and agronomical requirements in dynamic farmland environments. The method first receives a discrete pathpoint sequence, agricultural machinery state parameters, and dynamic obstacle safety time window constraints, constructing a multi-objective optimization problem with pathpoint time intervals as decision variables. Second, it employs a relaxed convex feasible set algorithm, introducing relaxed variables to transform the non-convex optimization problem into an iteratively solved sequential quadratic programming problem. Then, based on the optimal solution, it calculates the absolute timestamps of each pathpoint, generating smooth speed, acceleration, and judder profiles. Finally, it outputs the optimized speed, acceleration, and judder profiles to the underlying controller for execution, achieving safe, efficient, and stable operation of the unmanned agricultural machinery in dynamic environments.

[0006] The technical solution adopted in this invention is as follows:

[0007] A time-series optimization-based dynamic velocity profile planning method for unmanned agricultural machinery includes the following steps:

[0008] Step 1: Obtain the path point sequence, system state parameters, and dynamic constraints of the unmanned agricultural machinery, and complete the preprocessing of the path geometric features to lay the data foundation for velocity profile optimization;

[0009] Step 2: Based on the path geometric feature parameters and system constraints obtained in Step 1, calculate the key kinematic variables using the discretization difference method, establish the mathematical relationship between velocity, acceleration, jerk and time interval, and lay the kinematic foundation for constructing the time-series optimization problem;

[0010] Step 3: Based on the path geometric parameters in Step 1 and the kinematic relationship established in Step 2, the velocity profile planning problem is constructed into a nonlinear optimization model with time interval as the decision variable (this nonlinear optimization model is a nonconvex optimization model). This model realizes the mathematical description and optimization solution of the agricultural machinery motion process by comprehensively considering the multi-objective function and constraint system of safety, dynamic feasibility and operation efficiency.

[0011] Step 4: Based on the non-convex optimization model established in Step 3, the original problem is transformed into a sequential quadratic programming problem using the Relaxed Convex Feasible Set (SCFS) algorithm. Efficient optimization calculation is achieved through iterative solution strategy, and finally the optimal time allocation scheme that satisfies all constraints is obtained.

[0012] Step 5: Based on the optimal time allocation scheme obtained in Step 4, the absolute timestamps of each path point are calculated and a complete kinematic profile is generated. The optimization results are then converted into speed commands that can be executed by the underlying controller, ultimately realizing high-precision path tracking and automated operation control of agricultural machinery.

[0013] Furthermore, the specific implementation method of step 1 is as follows:

[0014] Step 1.1: Obtain the geometric path for unmanned agricultural machinery operation provided by the path planning module. This path consists of a series of ordered path points. Description, where the i-th path point , (x i ,y i () represents the two-dimensional plane coordinates of the i-th path point. Represents the coordinates x of the point i and y i It consists of two real values ​​located in a two-dimensional plane; simultaneously, the input system's initial state parameters, including initial velocity... and initial acceleration Agronomic reference operation speed Agricultural machinery dynamic limit parameters, including maximum longitudinal acceleration. Maximum lateral acceleration and the minimum and maximum operating speeds ; and the dynamic obstacle safety time window constraint provided by the environmental perception module. This constraint represents the earliest and latest times when the unmanned agricultural machinery passes through the i-th path point;

[0015] Step 1.2: Perform geometric preprocessing on the path: Calculate the vectors between adjacent path points. Euclidean distance ; Calculate the heading angle at each path point based on the tangential direction of the path:

[0016]

[0017] Where x i y i The x and y coordinates of the current i-th path point are represented by x. i+1 y i+1 This represents the x and y coordinates of the next target point i+1; thus, the vertical and horizontal unit vectors of the current path point i are obtained:

[0018]

[0019] This preprocessing provides the necessary geometric basis for subsequent kinematic calculations.

[0020] The specific implementation method of step 2 is as follows:

[0021] Physical quantities such as velocity, acceleration, and sag are numerically approximated using discrete path points and timestamps. To ensure accuracy and stability, a central difference scheme is used for discretization, as detailed below:

[0022] (1) Velocity v i (at point) Because of its simple calculation and direct correlation with decision variables, the mathematical relationship is established using first-order forward difference:

[0023] (1)

[0024] Its longitudinal and lateral components are:

[0025] (Assuming no lateral slippage)

[0026] (2) Acceleration (at point) Place, Using the first-order central difference method, with point i as the center:

[0027] (2)

[0028] Its longitudinal and lateral components are:

[0029]

[0030] (3) Jump (at point) Place, The second-order central difference is used to measure the rate of change of acceleration.

[0031] (3)

[0032] Its longitudinal and lateral components are:

[0033]

[0034] The specific implementation methods for step 3 include the following:

[0035] Step 3.1: Based on the path geometry parameters and system constraints obtained above, the velocity profile planning problem is formulated as a problem using time interval vectors. For a nonlinear optimization problem with decision variables, where Indicates from the path point To the waypoint The time interval, and the absolute timestamps of all path points can be obtained by summing them:

[0036]

[0037] Step 3.2: Define the optimization objective function J(t) as:

[0038]

[0039] in These represent longitudinal acceleration and lateral acceleration, respectively. These represent the vertical jump and the lateral jump, respectively. Indicates longitudinal velocity. Indicates reference speed. to is a weighting coefficient used to adjust the trade-off between operational efficiency, motion stability, and agronomic requirements; N represents the number of path points.

[0040] Specifically, motion stability is determined by This indicates that this section improves motion smoothness by minimizing acceleration and jerk; operational efficiency is determined by... This section indicates that operational efficiency is improved by minimizing the deviation between the actual speed and the reference speed; agronomic requirements, on the other hand, are determined by weighting coefficients. to Adjustments were made to ensure that the optimization results met the specific requirements of agronomic operations;

[0041] Step 3.3: The constraints include the following:

[0042] (1) Safety constraints (temporal obstacle avoidance):

[0043] (4)

[0044] This constraint can be written as a linear inequality:

[0045]

[0046] Where the matrix sum vector By all and It is constructed, and its specific structure is as follows:

[0047] definition:

[0048]

[0049] in Corresponding lower bound constraint :

[0050]

[0051] Corresponding upper bound constraint :

[0052]

[0053] (2) Dynamic feasibility constraints:

[0054] (5)

[0055] (3) Work efficiency constraints:

[0056] (6)

[0057] Among them, constraint (4) can be reorganized as about system of linear inequalities Constraints (5) and (6) are about Nonlinear constraints.

[0058] Step 4 is implemented as follows:

[0059] Since the objective function in step 3.2 is non-convex due to the nonlinear relationship between the kinematic quantities and t, direct solution would be extremely difficult. This invention employs the relaxed convex feasible set algorithm to transform it into a sequential convex optimization problem, thereby enabling efficient solution, including the following steps:

[0060] Step 4.1: Problem Refactoring and Relaxation

[0061] Introduce an auxiliary matrix variable u, where Represents the kinematic quantity of type j at point i ( The objective function can be rewritten as a quadratic form. ,in The diagonal weight matrix is: ;

[0062] The key point is to combine all kinematic quantities with All relationships can be expressed as nonlinear equality constraints:

[0063] (7)

[0064] Where i is the path point index; Identify kinematic quantity categories; This is only related to the path geometry features and time interval vector. The relevant function characterizes the motion residuals when the kinematic quantities are zero; For only the time interval vector The relevant function characterizes the scaling factor of the kinematic quantities in the equation.

[0065] For example, for longitudinal acceleration Its constraint equation is derived from the acceleration difference formula (2), specifically:

[0066]

[0067] in Indicates from the path point arrive The time interval; Represented as the i-th path segment in the vertical direction The projected length on, i.e. ; It is represented as the projected length of the (i-1)th path segment in the longitudinal direction.

[0068] Corresponding constraint (7):

[0069]

[0070] in ; ; Let represent the longitudinal acceleration at point i.

[0071] To further address nonconvexity, slack variables are introduced. and make its elements ; The specific meaning is It is the first Path point number kinematic quantities The upper bound of the absolute value;

[0072] Based on this relaxation relation, the original equality constraint (7) can be relaxed into the following pair of linear inequality constraints:

[0073] (8)

[0074] This leads to the following problem after relaxation:

[0075]

[0076] ,in Indicates the first Physical upper limit value (positive value constraint) for kinematic quantities.

[0077] Step 4.2: Solving the sequential quadratic programming problem

[0078] First, construct the initial time interval vector. Each component is derived from the formula Confirmed, among which Let represent the Euclidean distance of the i-th path segment; then calculate the initial values ​​of the slack variables. This step makes the initial point approximately satisfy the original nonlinear equality constraint; then in the second step... In the next iteration, at the current point Perform a first-order Taylor expansion (linearization) on the nonlinear constraint (8):

[0079] (9)

[0080] in and These are the decision variables for the current planning problem; and The known expansion point for the k-th iteration; For function The gradient vector at that point; For function exist The gradient vector at that point;

[0081] This yields the quadratic programming subproblem for the k-th iteration:

[0082]

[0083]

[0084] By solving this QP problem, a new iteration point is obtained. ;

[0085] Step 4.3: Variable Update and Convergence Judgment

[0086] (1) Decision variables:

[0087] (2) Relaxed variables:

[0088] (3) Check if convergence: If (in If the preset tolerance is reached, the iteration stops and the optimal solution is output. Otherwise, let The cycle continues.

[0089] Step 5 is implemented as follows:

[0090] Based on the optimal time interval vector obtained by the solver in step 4 ,in Indicates starting from path point p i to p i+1 The optimal travel time is determined; based on the optimal time interval vector, the absolute timestamp of each waypoint is calculated.

[0091]

[0092] Then, the optimal kinematic profile is calculated, as follows:

[0093] (1) Optimal longitudinal velocity profile: ;

[0094] (2) Optimal acceleration profile: Substituting into the acceleration calculation formula (2), the optimal acceleration is calculated. and its longitudinal components and horizontal components ;

[0095] (3) Optimal jump profile: Substituting into the quirk calculation formula (3), the optimal quirk is calculated. and its longitudinal components and horizontal components ;

[0096] Finally, the generated optimal motion profile will be... The output is sent to the underlying motion controller of the unmanned agricultural machinery, which uses a path tracking control algorithm to achieve precise motion control of the machinery and complete automated operation tasks.

[0097] The beneficial effects of this invention are:

[0098] 1. Traditional agricultural machinery speed planning methods are mostly based on static environment assumptions, making it difficult to effectively cope with suddenly appearing moving obstacles in the field (such as personnel, animals, and other agricultural machinery). This invention introduces a core mechanism of safe time window constraints, transforming the spatial location prediction information of dynamic obstacles into time interval restrictions for agricultural machinery to pass through specific path points. This method enables agricultural machinery to proactively perform spatiotemporal joint planning, rather than simply avoiding obstacles in space;

[0099] 2. This invention abandons the traditional approach of optimizing a single indicator and constructs a composite objective function that comprehensively considers time efficiency, motion stability, and agronomic requirements. Through flexible configuration of weighting coefficients, optimal balance can be achieved in different operating scenarios: in long straight sections, constant speed is prioritized to meet the uniformity requirements of sowing and pesticide application; in turning areas at the edge of the field, speed is automatically and smoothly adjusted to ensure mechanical structural safety and passenger comfort; and acceleration is actively limited on slopes to prevent slippage and tipping. The speed profile generated by this multi-objective optimization mechanism not only improves operating efficiency by approximately 15%-20%, but also effectively reduces wear and tear on the agricultural machinery transmission system by approximately 30% by minimizing acceleration and jolts, extending equipment lifespan, while providing a more comfortable operating experience for potential agricultural operators.

[0100] 3. Facing the industry challenge of directly solving nonlinear and nonconvex optimization problems with high computational complexity, this invention innovatively applies the relaxed convex feasible set algorithm. Through ingenious variable relaxation and sequential quadratic programming strategies, the original problem is decomposed into a series of structurally regular and efficiently solvable quadratic programming subproblems. This transformation not only ensures the convergence of the algorithm but also controls the computation time of a single programming problem to the order of hundreds of milliseconds, fully meeting the real-time requirements of unmanned agricultural machinery at typical operating speeds (usually below 5 m / s). Experimental results show that this method can replan a typical operating path containing 50-100 path points on conventional computing hardware in less than 1 second, providing a solid technical foundation for the rapid response and online replanning of agricultural machinery in dynamically changing environments.

[0101] 4. The input of this invention only requires a sequence of discrete path points and does not rely on complex path parameterization forms. This allows it to seamlessly integrate with various higher-level path planning algorithms (such as A*, RRT, etc.), exhibiting excellent system integration compatibility. Furthermore, since the core algorithm does not impose special restrictions on path geometry, this invention can be widely applied to typical farmland scenarios such as straight-line operations between rows, curved boundary operations, field turning, slope adaptive operations, and multi-machine collaborative operations. Whether it's sowing and cultivation management in dryland fields or transplanting and plant protection operations in paddy fields, this method can generate high-quality velocity profiles that meet specific agronomic requirements and machine dynamics constraints, demonstrating strong versatility and application value. Attached Figure Description

[0102] Figure 1 This is a flowchart of the speed planning method of the present invention;

[0103] Figure 2 This is a schematic diagram of the specific path of the present invention;

[0104] Figure 3 This is a schematic diagram illustrating the timing optimization method of this invention.

[0105] Figure 4 This is a schematic diagram of the specific velocity curve, acceleration curve, and jump curve of the present invention;

[0106] Figure 5 This is a schematic diagram of the specific timestamp of the present invention. Detailed Implementation

[0107] The invention will now be further described with reference to the accompanying drawings.

[0108] like Figure 1 As shown, the present invention relates to a speed profile planning method for unmanned agricultural machinery based on time-series optimization, which specifically includes the following steps:

[0109] Step 1: Establish the system input parameter set and complete path geometry preprocessing:

[0110] Step 1.1: Obtain the geometric path for unmanned agricultural machinery operation provided by the path planning module. This path consists of a series of ordered path points. Description, where the i-th path point , (x i ,y i () represents the two-dimensional plane coordinates of the i-th path point. Represents the coordinates x of the point i and y i It consists of two real values ​​located in a two-dimensional plane; simultaneously, the input system's initial state parameters, including initial velocity... and initial acceleration Agronomic reference operation speed Agricultural machinery dynamic limit parameters, including maximum longitudinal acceleration. Maximum lateral acceleration and the minimum and maximum operating speeds ; and the dynamic obstacle safety time window constraint provided by the environmental perception module. This constraint represents the earliest and latest times when the unmanned agricultural machinery passes through the i-th path point;

[0111] Step 1.2: Perform geometric preprocessing on the path: Calculate the vectors between adjacent path points. Euclidean distance ; Calculate the heading angle at each path point based on the tangential direction of the path:

[0112]

[0113] Where x i y i The x and y coordinates of the current i-th path point are represented by x. i+1 y i+1 This represents the x and y coordinates of the next target point i+1; thus, the vertical and horizontal unit vectors of the current path point i are obtained:

[0114]

[0115] This preprocessing provides the necessary geometric basis for subsequent kinematic calculations.

[0116] The specific implementation method of step 2 is as follows:

[0117] Physical quantities such as velocity, acceleration, and sag are numerically approximated using discrete path points and timestamps. To ensure accuracy and stability, a central difference scheme is used for discretization, as detailed below:

[0118] (1) Velocity v i (at point) Because of its simple calculation and direct correlation with decision variables, the mathematical relationship is established using first-order forward difference:

[0119] (1)

[0120] Its longitudinal and lateral components are:

[0121] (Assuming no lateral slippage)

[0122] (2) Acceleration (at point) Place, Using the first-order central difference method, with point i as the center:

[0123] (2)

[0124] Its longitudinal and lateral components are:

[0125]

[0126] (3) Jump (at point) Place, The second-order central difference is used to measure the rate of change of acceleration.

[0127] (3)

[0128] Its longitudinal and lateral components are:

[0129]

[0130] The specific implementation methods for step 3 include the following:

[0131] Step 3.1: Based on the path geometry parameters and system constraints obtained above, the velocity profile planning problem is formulated as a problem using time interval vectors. For a nonlinear optimization problem with decision variables, where Indicates from the path point To the waypoint The time interval, and the absolute timestamps of all path points can be obtained by summing them:

[0132]

[0133] Step 3.2: Define the optimization objective function J(t) as:

[0134]

[0135] in These represent longitudinal acceleration and lateral acceleration, respectively. These represent the vertical jump and the lateral jump, respectively. Indicates longitudinal velocity. Indicates reference speed. to is a weighting coefficient used to adjust the trade-off between operational efficiency, motion stability, and agronomic requirements; N represents the number of path points.

[0136] Specifically, motion stability is determined by This indicates that this section improves motion smoothness by minimizing acceleration and jerk; operational efficiency is determined by... This section indicates that operational efficiency is improved by minimizing the deviation between the actual speed and the reference speed; agronomic requirements, on the other hand, are determined by weighting coefficients. to Adjustments were made to ensure that the optimization results met the specific requirements of agronomic operations;

[0137] Step 3.3: The constraints include the following:

[0138] (1) Safety constraints (temporal obstacle avoidance):

[0139] (4)

[0140] This constraint can be written as a linear inequality:

[0141]

[0142] Where the matrix sum vector By all and It is constructed, and its specific structure is as follows:

[0143] definition:

[0144]

[0145] in Corresponding lower bound constraint :

[0146]

[0147] Corresponding upper bound constraint :

[0148]

[0149] (2) Dynamic feasibility constraints:

[0150] (5)

[0151] (3) Work efficiency constraints:

[0152] (6)

[0153] Among them, constraint (4) can be reorganized as about system of linear inequalities Constraints (5) and (6) are about Nonlinear constraints.

[0154] Step 4 is implemented as follows:

[0155] Since the objective function in step 3.2 is non-convex due to the nonlinear relationship between the kinematic quantities and t, direct solution would be extremely difficult. This invention employs the relaxed convex feasible set algorithm to transform it into a sequential convex optimization problem, thereby enabling efficient solution, including the following steps:

[0156] Step 4.1: Problem Refactoring and Relaxation

[0157] Introduce an auxiliary matrix variable u, where Represents the kinematic quantity of type j at point i ( The objective function can be rewritten as a quadratic form. ,in The diagonal weight matrix is: ;

[0158] The key point is to combine all kinematic quantities with All relationships can be expressed as nonlinear equality constraints:

[0159] (7)

[0160] Where i is the path point index; Identify kinematic quantity categories; This is only related to the path geometry features and time interval vector. The relevant function characterizes the motion residuals when the kinematic quantities are zero; For only the time interval vector The relevant function characterizes the scaling factor of the kinematic quantities in the equation.

[0161] For example, for longitudinal acceleration Its constraint equation is derived from the acceleration difference formula (2), specifically:

[0162]

[0163] in Indicates from the path point arrive The time interval; Represented as the i-th path segment in the vertical direction The projected length on, i.e. ; It is represented as the projected length of the (i-1)th path segment in the longitudinal direction.

[0164] Corresponding constraint (7):

[0165]

[0166] in ; ; This represents the longitudinal acceleration at point i; .

[0167] To further address nonconvexity, slack variables are introduced. and order ; The specific meaning is It is the first Point 1 kinematic quantities The upper bound of the absolute value;

[0168] Based on this relaxation relation, the original equality constraint (7) can be relaxed into the following pair of linear inequality constraints:

[0169] (8)

[0170] This leads to the following problem after relaxation:

[0171]

[0172] ,in Indicates the first Physical upper limit value (positive value constraint) for kinematic quantities.

[0173] Step 4.2: Solving the sequential quadratic programming problem

[0174] First, construct the initial time interval vector. Each component is derived from the formula Confirmed, among which Let represent the Euclidean distance of the i-th path segment; then calculate the initial values ​​of the slack variables. This step makes the initial point approximately satisfy the original nonlinear equality constraint; then in the second step... In the next iteration, at the current point Perform a first-order Taylor expansion (linearization) on the nonlinear constraint (8):

[0175] (9)

[0176] in and These are the decision variables for the current planning problem; and The known expansion point for the k-th iteration; For function The gradient vector at that point; For function exist The gradient vector at that point;

[0177] This yields the quadratic programming subproblem for the k-th iteration:

[0178]

[0179]

[0180] By solving this QP problem, a new iteration point is obtained. ;

[0181] Step 4.3: Variable Update and Convergence Judgment

[0182] (1) Decision variables:

[0183] (2) Relaxed variables:

[0184] (3) Check if convergence: If (in If the preset tolerance is reached, the iteration stops and the optimal solution is output. Otherwise, let The cycle continues.

[0185] Step 5 is implemented as follows:

[0186] Based on the optimal time interval vector obtained by the solver in step 4 ,in Indicates starting from path point p i to p i+1 The optimal travel time is determined; based on the optimal time interval vector, the absolute timestamp of each waypoint is calculated.

[0187]

[0188] Then, the optimal kinematic profile is calculated, as follows:

[0189] (1) Optimal longitudinal velocity profile: ;

[0190] (2) Optimal acceleration profile: Substitute into the acceleration calculation formula (2);

[0191] (3) Optimal jump profile: Substitute into the jump calculation formula (3);

[0192] Finally, the generated optimal motion profile will be... The output is sent to the underlying motion controller of the unmanned agricultural machinery, which uses a path tracking control algorithm to achieve precise motion control of the machinery and complete automated operation tasks.

[0193] To better verify the applicability of the proposed time-series optimization-based dynamic velocity profile planning method for unmanned agricultural machinery on arbitrary paths, this study conducted detailed simulations using Matlab to demonstrate a scenario involving dynamic obstacle avoidance. The path diagram is shown below. Figure 2 As shown, the simulation uses the Relaxed Convex Feasible Set (CFS) algorithm as the solver. It optimizes the time allocation scheme by iteratively solving the quadratic programming (QP) subproblem. The path sampling interval (delta) is set to 1 meter, the initial vehicle speed (V0) is set to 4 m / s, the reference speed (vref) is set to 5 m / s, and the weight coefficients are set to 1, 10, 10, 10, 10 according to the proportional relationship between work efficiency, motion stability and agronomic requirements.

[0194] Figure 2 The operation path of unmanned agricultural machinery is shown, including discrete path points and geometry, providing a basis for velocity profile planning; Figure 3A schematic diagram of a time-series optimization method is described, which serializes path points using timestamps and performs optimized allocation in the time domain. Figure 4 The optimized velocity, acceleration, and jerk curves are shown, indicating that the optimization method generates a smooth velocity profile that meets agronomic requirements and dynamic constraints. Figure 5 The absolute timestamps of each path point are displayed, reflecting the dynamic characteristics of the velocity profile and ensuring safety and operational quality. The simulation results show that this method can effectively generate safe, efficient and stable velocity profiles in dynamic environments, significantly improving the operational performance and adaptability of unmanned agricultural machinery.

[0195] Based on the above method, this embodiment of the invention also proposes a control device for unmanned agricultural machinery, which can execute the above method.

[0196] Based on the above method, this embodiment of the invention also proposes a storage device for unmanned agricultural machinery, which has the program of the above method built into it.

[0197] The detailed descriptions listed above are merely specific descriptions of feasible embodiments of the present invention, and are not intended to limit the scope of protection of the present invention. All equivalent methods or modifications that do not depart from the technology of the present invention should be included within the scope of protection of the present invention.

Claims

1. A dynamic velocity profile planning method for unmanned agricultural machinery based on time-series optimization, characterized in that, Includes the following steps: Step 1: Obtain the path point sequence, system state parameters, and dynamic constraints of the unmanned agricultural machinery, and complete the preprocessing of the path geometric feature parameters; Step 2: Based on the path geometric feature parameters and system constraints preprocessed in Step 1, establish the mathematical relationship between key kinematic variables (including velocity, acceleration, and jump) and time intervals using the discretization difference method. Step 3: Based on the path geometric parameters in Step 1 and the mathematical relationships of the kinematic variables established in Step 2, the velocity profile planning problem is constructed into a nonlinear optimization model with time interval as the decision variable. This nonlinear optimization model is a nonconvex optimization model. This model achieves a mathematical description and optimization solution of the agricultural machinery motion process by comprehensively considering the multi-objective function and constraint system of safety, dynamic feasibility and operation efficiency. Step 4: Based on the non-convex optimization model established in Step 3, the relaxed convex feasible set algorithm is used to transform the original non-convex optimization model into a sequential quadratic programming problem. The optimal time allocation scheme that satisfies the constraints is obtained through iterative solution. Step 5: Based on the optimal time allocation scheme obtained in Step 4, the absolute timestamps of each path point are calculated and a complete kinematic profile is generated. The optimization results are then converted into instructions that can be executed by the unmanned agricultural machinery's underlying controller, thereby realizing high-precision path tracking and automated operation control of the unmanned agricultural machinery.

2. The dynamic velocity profile planning method for unmanned agricultural machinery based on time-series optimization according to claim 1, characterized in that, The implementation of step 1 includes the following: Step 1.1: Obtain the geometric path for unmanned agricultural machinery operation provided by the path planning module. This path consists of a series of ordered path points. Description, where the i-th path point , (x i ,y i () represents the two-dimensional plane coordinates of the i-th path point. Represents the coordinates x of the point i and y i It consists of two real values ​​located in a two-dimensional plane; simultaneously, the input system's initial state parameters, including initial velocity... and initial acceleration Agronomic reference operation speed ; Agricultural machinery dynamic limit parameters, including maximum longitudinal acceleration Maximum lateral acceleration and the minimum and maximum operating speeds ; and the dynamic obstacle safety time window constraint provided by the environmental perception module. This constraint represents the earliest and latest times when the unmanned agricultural machinery passes through the i-th path point; Step 1.2: Perform geometric preprocessing on the path: Calculate the vectors between adjacent path points. Euclidean distance ; Calculate the heading angle at each path point based on the tangential direction of the path: Where x i y i The x and y coordinates of the current i-th path point are represented by x. i+1 y i+1 This represents the x and y coordinates of the next target point i+1; thus, the vertical and horizontal unit vectors of the current path point i are obtained: 。 3. The dynamic velocity profile planning method for unmanned agricultural machinery based on time-series optimization according to claim 2, characterized in that, The implementation of step 2 includes the following: Physical quantities such as velocity, acceleration, and sag are numerically approximated using discrete path points and timestamps. To ensure accuracy and stability, a central difference scheme is used for discretization, as detailed below: (1) Velocity v i (at point) Because of its simple calculation and direct correlation with decision variables, the mathematical relationship is established using first-order forward difference: (1) Its longitudinal and lateral components are: (Assuming no lateral slippage) (2) Acceleration (at point) Place, Using the first-order central difference method, with point i as the center: (2) Its longitudinal and lateral components are: (3) Jump (at point) Place, The second-order central difference is used to measure the rate of change of acceleration. (3) Its longitudinal and lateral components are: 。 4. The dynamic velocity profile planning method for unmanned agricultural machinery based on time-series optimization according to claim 3, characterized in that, The implementation of step 3 includes the following: Step 3.1: Based on the obtained path geometry parameters and system constraints, the velocity profile planning problem is expressed as a problem with time interval vectors. For a nonlinear optimization problem with decision variables, where , indicating from point Time The travel time can be obtained by summing the absolute timestamps of all waypoints: Step 3.2: Define the optimization objective function J(t) as: in These represent longitudinal acceleration and lateral acceleration, respectively. These represent the vertical jump and the lateral jump, respectively. Indicates longitudinal velocity. Indicates reference speed. to is a weighting coefficient used to adjust the trade-off between operational efficiency, motion stability, and agronomic requirements; N represents the number of path points. Step 3.3: Define the constraints as follows: (1) Security constraints: (4) (2) Dynamic feasibility constraints: (5) (3) Work efficiency constraints: (6) Among them, constraint (4) can be reorganized as about system of linear inequalities Constraints (5) and (6) are about Nonlinear constraints.

5. The dynamic velocity profile planning method for unmanned agricultural machinery based on time-series optimization according to claim 4, characterized in that, In step 3.2: motion stability is determined by... This indicates that this section improves motion smoothness by minimizing acceleration and jerk; operational efficiency is determined by... This section indicates that operational efficiency is improved by minimizing the deviation between the actual speed and the reference speed; agronomic requirements, on the other hand, are determined by weighting coefficients. to Adjustments are made to ensure that the optimization results meet the specific requirements of agronomic operations.

6. The dynamic velocity profile planning method for unmanned agricultural machinery based on time-series optimization according to claim 5, characterized in that, The implementation of step 4 includes the following: Step 4.1: Problem Refactoring and Relaxation By introducing an auxiliary matrix variable u, the objective function can be rewritten as a quadratic form. ,in The diagonal weight matrix is: ; The relationship between all kinematic quantities and t is expressed as a nonlinear equality constraint: (7) Where i is the path point index; Identify kinematic quantity categories; This is only related to the path geometry features and time interval vector. The relevant function characterizes the motion residuals when the kinematic quantities are zero; For only the time interval vector The relevant function characterizes the scaling factor of the kinematic quantities in the equation; To further address nonconvexity, a slack variable y is introduced, and its elements are set to... ; The specific meaning is It is the first Point 1 kinematic quantities The upper bound of the absolute value; Based on this relaxation relation, the original equality constraint (7) can be relaxed into the following pair of linear inequality constraints: (8) This leads to the following problem after relaxation: ,in Indicates the first Physical upper limit value (positive value constraint) for kinematic quantities; Step 4.2: Solving the sequential quadratic programming problem From the initial solution and Start iteration; at the first iteration In the next iteration, at the current point The first-order Taylor expansion is used to linearize the nonlinear constraint (8); For constraint (8), its linearized form is: This yields the quadratic programming subproblem for the k-th iteration: By solving this QP problem, a new iteration point is obtained. .

7. The dynamic velocity profile planning method for unmanned agricultural machinery based on time-series optimization according to claim 6, characterized in that, The implementation of step 4 also includes: Step 4.3: Variable Update and Convergence Judgment Determine if convergence: If ,in If a preset tolerance is set, the iteration stops and the optimal solution is output. , ,in Indicates starting from path point p i to p i+1 The optimal travel time is, where; otherwise, let The cycle continues.

8. The dynamic velocity profile planning method for unmanned agricultural machinery based on time-series optimization according to claim 7, characterized in that, The implementation of step 5 includes the following: Based on the optimal time interval vector obtained from the solution Calculate the absolute timestamps of each waypoint: The optimal kinematic profile is calculated as follows: (1) Optimal longitudinal velocity profile: ; (2) Optimal acceleration profile: Substituting into the acceleration calculation formula (2), the optimal acceleration is calculated. and its longitudinal components and horizontal components ; (3) Optimal jump profile: Substituting into the jump calculation formula (3), the optimal jump is calculated. and its longitudinal components and horizontal components ; Finally, the generated optimal motion profile will be... Output to the underlying motion controller of the unmanned agricultural machinery.

9. A control device for unmanned agricultural machinery, characterized in that, The control device is capable of performing the method described in any one of claims 1-8.

10. A storage device for unmanned agricultural machinery, characterized in that, The storage device contains a program for the method described in any one of claims 1-8.

Images