Unmanned agricultural machine dynamic obstacle avoidance and path tracking method based on optimal control theory

Abstract

The invention discloses an unmanned agricultural machine dynamic obstacle avoidance and path tracking method based on an optimal control theory, and belongs to the field of agricultural machine intelligent control. The method mainly comprises the following steps: 1, acquiring unmanned agricultural machinery and obstacle information by using a vehicle-mounted sensor, and establishing a system model; 2, Euler discretization is carried out on the system model, and the system model is used as a prediction model of a model prediction control algorithm; 3, designing a collision-free constraint condition, and establishing an obstacle avoidance constraint objective function; 4, establishing a control constraint objective function by using the path tracking error and the control deviation; and 5, converting the target function into a multi-constraint optimal control problem and solving the multi-constraint optimal control problem. The method has the advantages that firstly, the influence of the transverse speed and the corner rate on the unmanned agricultural machine is considered, and model establishment is more accurate; secondly, collision avoidance of static and moving obstacles is considered, limitation of road boundaries is also considered, and the method is suitable for more real and wide agricultural scenes; and thirdly, hyperelliptical obstacle avoidance constraint is adopted, so that the contour of the obstacle can be described more accurately.

Description

Technical Field

[0001] The invention discloses a dynamic obstacle avoidance and path tracking method for unmanned agricultural machinery based on optimal control theory, and belongs to the field of intelligent control of agricultural machinery. Background Art

[0002] In the field of modern agricultural production, unmanned agricultural machinery is becoming increasingly important. They play a key role in improving operational efficiency, reducing labor costs, and promoting the implementation of precision agriculture. These advances not only optimize agricultural production processes, but also help achieve more efficient use of resources and refined crop management. Although unmanned agricultural machinery has shown great potential in agricultural automation, their performance in unknown or complex environments is still limited by their perception and decision-making capabilities. In order to achieve autonomous driving from the garage to the field and then perform operational activities, unmanned agricultural machinery must be able to continuously and accurately obtain information about the surrounding environment and respond in time to avoid collisions with stationary or moving obstacles. This requires the vehicle to have a high degree of environmental perception and dynamic decision-making capabilities.

[0003] Liu proposed a nonlinear model predictive control (NLMPC) algorithm with a single-level structure. The algorithm aims to optimize the steering angle of a large autonomous vehicle navigating in an unstructured environment. The algorithm goal is to minimize the time required to reach the target destination while bypassing stationary obstacles, but the experimental environment is an off-road environment and lacks clear lanes. Considering the particularity of agricultural operations, the boundaries of the roads where unmanned agricultural machinery travel are often composed of vegetation or farmland. This road environment increases the complexity of driving, requiring the vehicle to not only be able to respond to emergencies and avoid sudden obstacles such as other vehicles, pedestrians or animals, but also to comply with the constraints of the road boundaries.

[0004] In addition, the dynamic changes of roads in agricultural environments also place higher demands on the obstacle avoidance capabilities of unmanned agricultural machinery. Seasonal crop growth, field operations, and weather changes may all have an impact on roads. Therefore, incorporating dynamic obstacle avoidance strategies into model predictive control algorithms is crucial to ensure that unmanned agricultural machinery operates safely and efficiently in real agricultural scenarios.

[0005] Therefore, the research focuses on developing a control strategy that can adapt to the dynamic nature of the agricultural environment, so that unmanned agricultural machinery can maintain efficient operation while ensuring driving safety, improve the reliability and adaptability of unmanned agricultural machinery in practical applications, and thus promote the development of modern agriculture to a higher level of automation and intelligence. This control strategy that comprehensively considers static, dynamic obstacles and road boundaries can not only improve the operating efficiency of unmanned agricultural machinery, but also enhance its robustness in complex agricultural environments. Summary of the invention

[0006] In order to solve the problems of dynamic obstacle avoidance and path tracking of existing unmanned agricultural machinery in complex real agricultural scenes, the present invention studies the dynamic collision avoidance and path tracking control of unmanned agricultural machinery from three aspects: vehicle modeling, dynamic obstacle avoidance method, and path tracking control, and proposes a dynamic obstacle avoidance and path tracking method for unmanned agricultural machinery based on optimal control theory.

[0007] A method for dynamic obstacle avoidance and path tracking of unmanned agricultural machinery based on optimal control theory includes the following steps:

[0008] S1: Use Beidou RTK positioning module, laser radar and satellite map to obtain the status information of unmanned agricultural machinery, obstacle information and road information;

[0009] S2: Based on the current state information of the unmanned agricultural machinery and obstacles, the kinematic models of the unmanned agricultural machinery and obstacles are established, and the two are combined into a nonlinear system model. The above equations are integrated using the Euler discretization method to obtain a discretized nonlinear system model as a prediction model of the nonlinear model predictive control algorithm;

[0010] S3: Selecting reference points for unmanned agricultural machinery path tracking from the required tracking path;

[0011] S4: Based on the acquired obstacle information and road boundary information, the obstacle avoidance constraint conditions are designed by comprehensively considering the status of the unmanned agricultural machinery and obstacles;

[0012] S5: Establish obstacle avoidance constraint objective function;

[0013] S6: Combined with the constraints of the unmanned agricultural machinery power system, the path tracking error, control deviation and obstacle avoidance constraint objective functions are combined to design the overall objective function, which is transformed into a multi-constraint optimal control problem and solved.

[0014] 2. The method according to claim 1, characterized in that said S2 comprises the following steps:

[0015] S2.1. Establish the kinematic model of the unmanned agricultural machinery. The specific form is as follows:

[0016]

[0017] Where: v x represents the longitudinal speed of the unmanned agricultural machine, v y Indicates the lateral speed of the unmanned agricultural machine; L f represents the distance from the front axle of the unmanned agricultural machine to the center of gravity, L represents the wheelbase of the unmanned agricultural machine, ψ represents the heading angle of the unmanned agricultural machine, and δ f Indicates the steering angle of the front wheels of the unmanned agricultural machinery;

[0018] S2.2. Establish the kinematic model of the obstacle. The specific form is as follows:

[0019]

[0020] Where: (x m ,y m ) represents the position of the obstacle, v m represents the speed of the obstacle, ψ m Indicates the heading angle of the obstacle;

[0021] S2.3. Combine the kinematic models of the unmanned agricultural machinery and the obstacles to obtain a system model, which is shown in the following form:

[0022]

[0023] Where: z represents the state information of the unmanned agricultural machinery and obstacles, and u represents the control quantity, including the steering angle δ of the unmanned agricultural machinery f And the time step ΔT of each iteration of the algorithm:

[0024] S2.4. By integrating the above equations through the Euler discretization method, we can obtain the discretized nonlinear system model, which is as follows:

[0025] z(k+1)=z(k)+ΔTf(z, u), k∈[1, N]

[0026] Where: ΔT represents the time step of each iteration of the algorithm, and N represents the total number of prediction steps of the algorithm.

[0027] 3. The method according to claim 1, characterized in that said S3 comprises the following steps:

[0028] S3.1 In the nonlinear model predictive control algorithm, the future N-step state of the unmanned agricultural machinery is predicted based on the current state of the unmanned agricultural machinery; based on the acquired state information and road information of the unmanned agricultural machinery, N+1 points are determined on the required tracking path, and these points will be used as reference points for the unmanned agricultural machinery path tracking;

[0029] S3.2 When the unmanned agricultural machine does not deviate from the required tracking path, directly select the reference point closest to the current position of the unmanned agricultural machine, that is, directly in front of the unmanned agricultural machine;

[0030] S3.3 When the unmanned agricultural machine deviates from the required tracking path for a short time due to obstacle avoidance, the center of gravity of the unmanned agricultural machine is projected onto the reference path along the X and Y axes of the global coordinate system. Of the two projection points, the projection point closer to the target will be selected as the reference point.

[0031] 4. The method according to claim 1, characterized in that said S4 comprises the following steps:

[0032] S4.1. For stationary obstacles, most obstacle avoidance constraints previously used standard circles or standard ellipses. The obstacle avoidance effects vary greatly due to the different shapes of the obstacles. Therefore, a super ellipse obstacle avoidance constraint is proposed, which can more accurately depict the outline of the obstacle and obtain a more accurate safety area. The obstacle avoidance constraint function is thus established. The specific form is as follows:

[0033]

[0034] Where: (x(t), y(t)) represents the position of the unmanned agricultural machine when the prediction step is t, Indicates the position of the jth stationary obstacle when the prediction step number is t; n1 is used to determine the specific shape of the superellipse; L represents the wheelbase of the unmanned agricultural machinery, and W represents the track width of the unmanned agricultural machinery; r1 and r2 are taken according to the geometric shape of the stationary obstacle to obtain a more accurate obstacle avoidance area; N represents the total prediction step number, and S represents the total number of stationary obstacles;

[0035] S4.2. For moving obstacles, a similar approach is used as for stationary obstacles. Considering the speed of the moving obstacles, relatively conservative constraints are adopted to ensure absolute safety. The specific form is as follows:

[0036]

[0037] Where: (x(t), y(t)) represents the position of the unmanned agricultural machine when the prediction step is t, Indicates the position of the kth moving obstacle when the prediction step number is t; n2 is used to determine the specific shape of the super ellipse; L represents the wheelbase of the unmanned agricultural machinery, and W represents the track width of the unmanned agricultural machinery; r3 and r4 are taken according to the geometric shape of the moving obstacle to obtain a more accurate obstacle avoidance area; N represents the total number of prediction steps, and M represents the total number of moving obstacles;

[0038] S4.3. For the road boundary, discretize it into boundary points, treat it as a stationary obstacle and use a similar method, which is as follows:

[0039]

[0040] Where: (x(t), y(t)) represents the position of the unmanned agricultural machine when the prediction step is t, It represents the position of the i-th boundary point when the prediction step number is t; n3 is used to determine the specific shape of the super ellipse; L represents the wheelbase of the unmanned agricultural machinery, W represents the track width of the unmanned agricultural machinery; N represents the total prediction steps, and R represents the total boundary points.

[0041] 5. The method according to claim 1 is characterized in that, in said S5, the objective function J of the unmanned agricultural machinery obstacle avoidance is set by combining the unmanned agricultural machinery obstacle avoidance constraint and comprehensively considering the obstacle speed and collision avoidance weight. obs The specific form is as follows:

[0042]

[0043] Where: The first sum represents the objective function of road boundary collision avoidance, i represents the i-th road boundary point, R represents the total number of road boundary points, ω or represents the weight of the road boundary collision avoidance constraint; x and y represent the position of the unmanned agricultural machinery, and represents the position of the i-th road boundary point; N represents the total number of prediction steps of the algorithm, ξ represents a small positive constant to avoid 0 in the denominator; the second sum represents the objective function of stationary obstacle avoidance, j represents the j-th stationary obstacle, S represents the total number of stationary obstacles, ω os represents the weight of the static obstacle avoidance constraint; x and y represent the position of the unmanned agricultural machinery, and represents the position of the jth stationary obstacle; N represents the total number of prediction steps of the algorithm, ξ represents a small positive constant to avoid 0 in the denominator; the third sum represents the objective function of mobile obstacle avoidance, k represents the kth mobile obstacle, M represents the total number of mobile obstacles, ω om represents the weight of the mobile obstacle avoidance constraint; x and y represent the position of the unmanned agricultural machinery, and represents the position of the kth moving obstacle; N represents the total number of prediction steps of the algorithm, and ξ represents a small positive constant to avoid 0 in the denominator.

[0044] 6. The method according to claim 1, characterized in that said S6 comprises the following steps:

[0045] S6.1. Considering the constraints of the unmanned agricultural machinery linkage on the steering system, the steering angle is limited to a constant range, which can be expressed as:

[0046]

[0047] Where: represents the minimum front wheel steering angle, Indicates the maximum front wheel steering angle;

[0048] S6.2. Considering the characteristics of the power system of the unmanned agricultural machinery and the density of obstacles in the working environment, specific upper and lower limits are set for the longitudinal speed and yaw angle of the unmanned agricultural machinery, which are specifically expressed as follows:

[0049]

[0050] ψ min ≤ψ(t)≤ψ max

[0051] Where: Indicates the minimum longitudinal speed of the unmanned agricultural machinery. represents the maximum longitudinal speed of the unmanned agricultural machinery; ψ min represents the minimum value of the heading angle of the unmanned agricultural machinery, ψ max Indicates the maximum value of the heading angle of the unmanned agricultural machinery;

[0052] S6.3. Combining the tracking error, control deviation and obstacle avoidance constraint objective functions, the objective function of the model predictive control algorithm is designed as follows:

[0053]

[0054] Where: t represents the tth prediction step, x(t) represents the current position of the unmanned agricultural machine, x ref is the reference position of the unmanned agricultural machinery; u(t) represents the current control value of the unmanned agricultural machinery, u ref represents the reference control quantity of unmanned agricultural machinery; The two variables x and y represent the position of the unmanned agricultural machinery. Represents the front wheel turning angle δ of the unmanned agricultural machinery f and the time step ΔT of each iteration of the algorithm; J obs represents the obstacle avoidance constraint objective function;

[0055] S6.4. Taking the total objective function,minimum as the goal and satisfying all the constraints mentioned above, the IPOPT solver is used for numerical solution.

[0056] Through the above technical scheme, compared with the prior art, the present invention has the following beneficial effects:

[0057] In order to solve the problem of dynamic obstacle avoidance and path tracking of unmanned agricultural machinery in complex agricultural scenes, the present invention discloses a method for dynamic obstacle avoidance and path tracking of unmanned agricultural machinery based on optimal control theory; first, according to the state information of the unmanned agricultural machinery and obstacles, a nonlinear system model is constructed, and its Euler discretization is used as the prediction model of the model predictive control algorithm; secondly, an obstacle avoidance constraint is established, which not only considers the collision avoidance of moving obstacles and stationary obstacles, but also considers the limitation of road boundaries, so that it is applicable to a wider and more complex agricultural scene. In addition, in order to enable the unmanned agricultural machinery to obtain a more accurate safety area and obstacle avoidance path, a super ellipse obstacle avoidance constraint is proposed, which can better depict the outline of the obstacle, so that the unmanned agricultural machinery can safely pass through areas with dense or irregularly distributed obstacles. Finally, the obstacle avoidance constraint and the control of the unmanned agricultural machinery are transformed into a multi-constrained optimal control problem, so as to realize the dynamic obstacle avoidance and path tracking of the unmanned agricultural machinery, which has high robustness and good real-time performance, and is applicable to complex agricultural scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1This is the structure diagram of the unmanned agricultural machinery dynamic obstacle avoidance and path tracking system based on optimal control theory;

[0059] Figure 2 This is the kinematic model diagram of the unmanned agricultural machinery;

[0060] Figure 3 This is a schematic diagram of the obstacle avoidance process of unmanned agricultural machinery;

[0061] Figure 4 This is a schematic diagram of the path tracking of unmanned agricultural machinery; DETAILED DESCRIPTION

[0062] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0063] A method for dynamic obstacle avoidance and path tracking control of unmanned agricultural machinery based on optimal control theory, the specific process of the implementation of the method is as follows:

[0064] S1: Use Beidou RTK positioning module, laser radar and satellite map to obtain the status information of unmanned agricultural machinery, obstacle information and road information;

[0065] S2: Figure 2 As shown, according to the current state information of the unmanned agricultural machinery and obstacles, the kinematic models of the unmanned agricultural machinery and obstacles are established, and the two are combined into a nonlinear system model. The above equations are integrated using the Euler discretization method to obtain a discretized nonlinear system model as a prediction model of the nonlinear model predictive control algorithm;

[0066] S2.1. Establish the kinematic model of the unmanned agricultural machinery. The specific form is as follows:

[0067]

[0068] Where: v x represents the longitudinal speed of the unmanned agricultural machine, v y Indicates the lateral speed of the unmanned agricultural machine; L f represents the distance from the front axle of the unmanned agricultural machine to the center of gravity, L represents the wheelbase of the unmanned agricultural machine, ψ represents the heading angle of the unmanned agricultural machine, and δ f Indicates the steering angle of the front wheels of the unmanned agricultural machinery;

[0069] S2.2. Establish the kinematic model of the obstacle. The specific form is as follows:

[0070]

[0071] Where: (x m ,y m ) represents the position of the obstacle, v m represents the speed of the obstacle, ψ mIndicates the heading angle of the obstacle;

[0072] S2.3. Combine the kinematic models of the unmanned agricultural machinery and the obstacles to obtain a system model, which is shown in the following form:

[0073]

[0074] Where: z represents the state information of the unmanned agricultural machinery and obstacles, and u represents the control quantity, including the steering angle δ of the unmanned agricultural machinery f and the time step ΔT of each iteration of the algorithm;

[0075] S2.4. By integrating the above equations through the Euler discretization method, we can obtain the discretized nonlinear system model, which is as follows:

[0076] z(k+1)=z(k)+ΔTf(z, u), k∈[1, N]

[0077] Where: ΔT represents the time step of each iteration of the algorithm, and N represents the total number of prediction steps of the algorithm.

[0078] S3: Selecting reference points for unmanned agricultural machinery path tracking from the required tracking path;

[0079] S3.1 In the nonlinear model predictive control algorithm, the future N-step state of the unmanned agricultural machinery is predicted based on the current state of the unmanned agricultural machinery; based on the acquired state information and road information of the unmanned agricultural machinery, N+1 points are determined on the required tracking path, and these points will be used as reference points for the unmanned agricultural machinery path tracking;

[0080] S3.2 When the unmanned agricultural machine does not deviate from the required tracking path, directly select the reference point closest to the current position of the unmanned agricultural machine, that is, directly in front of the unmanned agricultural machine;

[0081] S3.3 When the unmanned agricultural machine deviates from the required tracking path for a short time due to obstacle avoidance, the center of gravity of the unmanned agricultural machine is projected onto the reference path along the X and Y axes of the global coordinate system. Of the two projection points, the projection point closer to the target will be selected as the reference point.

[0082] S4: Figure 3 The figure shows a schematic diagram of the obstacle avoidance process of the unmanned agricultural machinery. According to the acquired obstacle information and road boundary information, the states of the unmanned agricultural machinery and obstacles are comprehensively considered to design obstacle avoidance constraints;

[0083] S4.1. For stationary obstacles, most obstacle avoidance constraints previously used standard circles or standard ellipses. The obstacle avoidance effects vary greatly due to the different shapes of the obstacles. Therefore, a super ellipse obstacle avoidance constraint is proposed, which can more accurately depict the outline of the obstacle and obtain a more accurate safety area. The obstacle avoidance constraint function is thus established. The specific form is as follows:

[0084]

[0085] Where: (x(t), y(t)) represents the position of the unmanned agricultural machine when the prediction step is t, Indicates the position of the jth stationary obstacle when the prediction step number is t; n1 is used to determine the specific shape of the superellipse; L represents the wheelbase of the unmanned agricultural machinery, and W represents the track width of the unmanned agricultural machinery; r1 and r2 are taken according to the geometric shape of the stationary obstacle to obtain a more accurate obstacle avoidance area; N represents the total prediction step number, and S represents the total number of stationary obstacles;

[0086] S4.2. For moving obstacles, a similar approach is used as for stationary obstacles. Considering the speed of the moving obstacles, relatively conservative constraints are adopted to ensure absolute safety. The specific form is as follows:

[0087]

[0088] Where: (x(t), y(t)) represents the position of the unmanned agricultural machine when the prediction step is t, Indicates the position of the kth moving obstacle when the prediction step number is t; n2 is used to determine the specific shape of the super ellipse; L represents the wheelbase of the unmanned agricultural machinery, and W represents the track width of the unmanned agricultural machinery; r3 and r4 are taken according to the geometric shape of the moving obstacle to obtain a more accurate obstacle avoidance area; N represents the total number of prediction steps, and M represents the total number of moving obstacles;

[0089] S4.3. For the road boundary, discretize it into boundary points, treat it as a stationary obstacle and use a similar method, which is as follows:

[0090]

[0091] Where: (x(t), y(t)) represents the position of the unmanned agricultural machine when the prediction step is t, It represents the position of the i-th boundary point when the prediction step number is t; n3 is used to determine the specific shape of the super ellipse; L represents the wheelbase of the unmanned agricultural machinery, W represents the track width of the unmanned agricultural machinery; N represents the total prediction steps, and R represents the total boundary points.

[0092] S5: Establish obstacle avoidance constraint objective function; Combined with the obstacle avoidance constraint of unmanned agricultural machinery, comprehensively consider the obstacle speed and collision avoidance weight, and set the objective function J of unmanned agricultural machinery obstacle avoidance obs The specific form is as follows:

[0093]

[0094] Where: The first sum represents the objective function of road boundary collision avoidance, i represents the i-th road boundary point, R represents the total number of road boundary points, ω orrepresents the weight of the road boundary collision avoidance constraint; x and y represent the position of the unmanned agricultural machinery, and represents the position of the i-th road boundary point; N represents the total number of prediction steps of the algorithm, ξ represents a small positive constant to avoid 0 in the denominator; the second sum represents the objective function of stationary obstacle avoidance, j represents the j-th stationary obstacle, S represents the total number of stationary obstacles, ω os represents the weight of the static obstacle avoidance constraint; x and y represent the position of the unmanned agricultural machinery, and represents the position of the jth stationary obstacle; N represents the total number of prediction steps of the algorithm, ξ represents a small positive constant to avoid 0 in the denominator; the third sum represents the objective function of mobile obstacle avoidance, k represents the kth mobile obstacle, M represents the total number of mobile obstacles, ω om represents the weight of the mobile obstacle avoidance constraint; x and y represent the position of the unmanned agricultural machinery, and represents the position of the kth moving obstacle; N represents the total number of prediction steps of the algorithm, and ξ represents a small positive constant to avoid 0 in the denominator.

[0095] S6: Figure 4 The figure shows a schematic diagram of the path tracking of unmanned agricultural machinery. Combined with the constraints of the unmanned agricultural machinery power system, the path tracking error, control deviation and obstacle avoidance constraint objective functions are combined to design the overall objective function, which is converted into a multi-constraint optimal control problem and solved.

[0096] S6.1. Considering the constraints of the unmanned agricultural machinery linkage on the steering system, the steering angle is limited to a constant range, which can be expressed as:

[0097]

[0098] Where: represents the minimum front wheel steering angle, Indicates the maximum front wheel steering angle;

[0099] S6.2. Considering the characteristics of the power system of the unmanned agricultural machinery and the density of obstacles in the working environment, specific upper and lower limits are set for the longitudinal speed and yaw angle of the unmanned agricultural machinery, which are specifically expressed as follows:

[0100]

[0101] ψ min ≤ψ(t)≤ψ max

[0102] Where: Indicates the minimum longitudinal speed of the unmanned agricultural machinery. represents the maximum longitudinal speed of the unmanned agricultural machinery; ψmin represents the minimum value of the heading angle of the unmanned agricultural machinery, ψ max Indicates the maximum value of the heading angle of the unmanned agricultural machinery;

[0103] S6.3. Combining the tracking error, control deviation and obstacle avoidance constraint objective functions, the objective function of the model predictive control algorithm is designed as follows:

[0104]

[0105] Where: t represents the tth prediction step, x(t) represents the current position of the unmanned agricultural machine, x ref is the reference position of the unmanned agricultural machinery; u(t) represents the current control value of the unmanned agricultural machinery, u ref represents the reference control quantity of unmanned agricultural machinery; The two variables x and y represent the position of the unmanned agricultural machinery. Represents the front wheel turning angle δ of the unmanned agricultural machinery f and the time step ΔT of each iteration of the algorithm; J obs represents the obstacle avoidance constraint objective function;

[0106] S6.4. With the goal of minimizing the total objective function J and satisfying all the constraints mentioned above, the IPOPT solver is used for numerical solution.

[0107] In addition, the present invention also provides a reference method for research in the same field, and can be further extended to other related unmanned agricultural machinery obstacle avoidance and path tracking control fields as a basis, and has high practicality and promotion value.

[0108] The series of detailed descriptions listed above are only specific descriptions of feasible implementation methods of the present invention. They are not intended to limit the scope of protection of the present invention. All equivalent methods or changes that do not deviate from the technical creation of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for dynamic obstacle avoidance and path tracking of unmanned agricultural machinery based on optimal control theory, characterized in that: The following steps are involved: S1: Use Beidou RTK positioning module, laser radar and satellite map to obtain the status information of unmanned agricultural machinery, obstacle information and road information; S2: Based on the current state information of the unmanned agricultural machinery and obstacles, the kinematic models of the unmanned agricultural machinery and obstacles are established, and the two are combined into a nonlinear system model. The above equations are integrated using the Euler discretization method to obtain a discretized nonlinear system model as a prediction model of the nonlinear model predictive control algorithm; S3: Selecting reference points for unmanned agricultural machinery path tracking from the required tracking path; S4: Based on the acquired obstacle information and road boundary information, the obstacle avoidance constraint conditions are designed by comprehensively considering the status of the unmanned agricultural machinery and obstacles; S5: Establish obstacle avoidance constraint objective function; S6: Combined with the constraints of the unmanned agricultural machinery power system, the path tracking error, control deviation and obstacle avoidance constraint objective functions are combined to design the overall objective function, which is transformed into a multi-constraint optimal control problem and solved.

2. The method according to claim 1, characterized in that: The S2 comprises the following steps: S2.

1. Establish the kinematic model of the unmanned agricultural machinery. The specific form is as follows: Where: v x represents the longitudinal speed of the unmanned agricultural machine, v y Indicates the lateral speed of the unmanned agricultural machine; L f represents the distance from the front axle of the unmanned agricultural machine to the center of gravity, L represents the wheelbase of the unmanned agricultural machine, ψ represents the heading angle of the unmanned agricultural machine, and δ f Indicates the steering angle of the front wheels of the unmanned agricultural machinery; S2.

2. Establish the kinematic model of the obstacle. The specific form is as follows: Where: (x m ,y m ) represents the position of the obstacle, v m represents the speed of the obstacle, ψ m Indicates the heading angle of the obstacle; S2.

3. Combine the kinematic models of the unmanned agricultural machinery and the obstacles to obtain a system model, which is shown in the following form: Where: z represents the state information of the unmanned agricultural machinery and obstacles; u represents the control quantity, including the steering angle δ of the unmanned agricultural machinery f and the time step ΔT of each iteration of the algorithm; S2.

4. By integrating the above equations through the Euler discretization method, we can obtain the discretized nonlinear system model, which is as follows: z(k+1)=z(k)+ΔTf(z,u),k∈[1,N] Where: ΔT represents the time step of each iteration of the algorithm, and N represents the total number of prediction steps of the algorithm.

3. The method according to claim 1, characterized in that The S3 comprises the following steps: S3.1 In the nonlinear model predictive control algorithm, the future N-step state of the unmanned agricultural machinery is predicted based on the current state of the unmanned agricultural machinery; based on the acquired state information and road information of the unmanned agricultural machinery, N+1 points are determined on the required tracking path, and these points will be used as reference points for the unmanned agricultural machinery path tracking; S3.2 When the unmanned agricultural machine does not deviate from the required tracking path, directly select the reference point closest to the current position of the unmanned agricultural machine, that is, directly in front of the unmanned agricultural machine; S3.3 When the unmanned agricultural machine deviates from the required tracking path for a short time due to obstacle avoidance, the center of gravity of the unmanned agricultural machine is projected onto the reference path along the X and Y axes of the global coordinate system. Of the two projection points, the projection point closer to the target will be selected as the reference point.

4. The method according to claim 1, characterized in that The S4 comprises the following steps: S4.

1. For stationary obstacles, most obstacle avoidance constraints previously used standard circles or standard ellipses. The obstacle avoidance effects vary greatly due to the different shapes of the obstacles. Therefore, a super ellipse obstacle avoidance constraint is proposed, which can more accurately depict the outline of the obstacle and obtain a more accurate safety area. The obstacle avoidance constraint function is thus established. The specific form is as follows: Where: (x(t), y(t)) represents the position of the unmanned agricultural machine when the prediction step is t, Indicates the position of the jth stationary obstacle when the prediction step number is t; n1 is used to determine the specific shape of the superellipse; L represents the wheelbase of the unmanned agricultural machinery, and W represents the track width of the unmanned agricultural machinery; r1 and r2 are taken according to the geometric shape of the stationary obstacle to obtain a more accurate obstacle avoidance area; N represents the total prediction step number of the algorithm, and S represents the total number of stationary obstacles; S4.

2. For moving obstacles, a similar approach is used as for stationary obstacles. Considering the speed of the moving obstacles, relatively conservative constraints are adopted to ensure absolute safety. The specific form is as follows: Where: (x(t), y(t)) represents the position of the unmanned agricultural machine when the prediction step is t, Indicates the position of the kth moving obstacle when the prediction step number is t; n2 is used to determine the specific shape of the superellipse; L represents the wheelbase of the unmanned agricultural machinery, and W represents the track width of the unmanned agricultural machinery; r3 and r4 are taken according to the geometric shape of the moving obstacle to obtain a more accurate obstacle avoidance area; N represents the total prediction step number of the algorithm, and M represents the total number of moving obstacles; S4.

3. For the road boundary, discretize it into boundary points, treat it as a stationary obstacle and use a similar method, which is as follows: Where: (x(t), y(t)) represents the position of the unmanned agricultural machine when the prediction step is t, It represents the position of the i-th boundary point when the prediction step number is t; n3 is used to determine the specific shape of the super ellipse; L represents the wheelbase of the unmanned agricultural machinery, and W represents the track width of the unmanned agricultural machinery; N represents the total prediction steps of the algorithm, and R represents the total boundary points.

5. The method according to claim 1, characterized in that In S5, the objective function J of the unmanned agricultural machinery obstacle avoidance is set by combining the unmanned agricultural machinery obstacle avoidance constraints and comprehensively considering the obstacle speed and collision avoidance weight. obs The specific form is as follows: Where: The first sum represents the objective function of road boundary collision avoidance, i represents the i-th road boundary point, R represents the total number of road boundary points, ω or represents the weight of the road boundary collision avoidance constraint; x and y represent the position of the unmanned agricultural machinery, and represents the position of the i-th road boundary point; N represents the total number of prediction steps of the algorithm, ξ represents a small positive constant to avoid 0 in the denominator; the second sum represents the objective function of stationary obstacle avoidance, j represents the j-th stationary obstacle, S represents the total number of stationary obstacles, ω os represents the weight of the static obstacle avoidance constraint; x and y represent the position of the unmanned agricultural machinery, and represents the position of the jth stationary obstacle; N represents the total number of prediction steps of the algorithm, ξ represents a small positive constant to avoid 0 in the denominator; the third sum represents the objective function of mobile obstacle avoidance, k represents the kth mobile obstacle, M represents the total number of mobile obstacles, ω om represents the weight of the mobile obstacle avoidance constraint; x and y represent the position of the unmanned agricultural machinery, and represents the position of the kth moving obstacle; N represents the total number of prediction steps of the algorithm, and ξ represents a small positive constant to avoid 0 in the denominator.

6. The method according to claim 1, characterized in that The S6 comprises the following steps: S6.

1. Considering the constraints of the unmanned agricultural machinery linkage on the steering system, the steering angle is limited to a constant range, which can be expressed as: Where: represents the minimum front wheel steering angle, Indicates the maximum front wheel steering angle; S6.

2. Considering the characteristics of the power system of the unmanned agricultural machinery and the density of obstacles in the working environment, specific upper and lower limits are set for the longitudinal speed and yaw angle of the unmanned agricultural machinery, which are specifically expressed as follows: ψ min ≤ψ(t)≤ψ max Where: Indicates the minimum longitudinal speed of the unmanned agricultural machinery. represents the maximum longitudinal speed of the unmanned agricultural machinery; ψ min represents the minimum value of the heading angle of the unmanned agricultural machinery, ψ max Indicates the maximum value of the heading angle of the unmanned agricultural machinery; S6.

3. Combining the tracking error, control deviation and obstacle avoidance constraint objective functions, the objective function of the model predictive control algorithm is designed as follows: Where: t represents the tth prediction step, x(t) represents the current position of the unmanned agricultural machine, x ref is the reference position of the unmanned agricultural machinery; u(t) represents the current control value of the unmanned agricultural machinery, u ref represents the reference control quantity of unmanned agricultural machinery; The two variables x and y represent the position of the unmanned agricultural machinery. Represents the front wheel turning angle δ of the unmanned agricultural machinery f and the time step ΔT of each iteration of the algorithm; J obs represents the obstacle avoidance constraint objective function; S6.

4. With the goal of minimizing the total objective function J and satisfying all the constraints mentioned above, the IPOPT solver is used for numerical solution.

Images