Path tracking control method for crawler-type unmanned plant protection machine in orchard environment

Abstract

The invention discloses a path tracking control method for a crawler-type unmanned plant protection machine in an orchard environment. The method comprises the following steps: acquiring a to-be-tracked path; continuously moving the unmanned plant protection machine, traversing path points in the path to obtain a path point closest to the current point, and recording the path point as a closest point; traversing remaining path points except the nearest point in the path, presetting a fixed distance, obtaining a path point with the distance being the fixed distance from the nearest point, and recording the path point as a target point; fitting the nearest point and the target point to obtain a navigation line; calculating transverse deviation and course deviation of the current point and the navigation line; and taking the transverse deviation and the course deviation as two input quantities of a controller, fuzzifying the input quantities, and outputting an angular velocity by the controller according to a fuzzy control rule table. According to the method, the multiple path points are collected, the navigation line is dynamically fitted in the path tracking process of the unmanned plant protection machine, and the problem that the curve tracking effect is not ideal is solved.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle path tracking, and in particular to a path tracking control method for a crawler-type unmanned plant protection machine in an orchard environment. Background Art

[0002] Path tracking for an autonomous vehicle can be defined as selecting a control point on the vehicle to track a time-independent geometric curve. This means that the control point is required to track a target path point on the desired path at a given speed. The lateral motion controller is typically designed assuming a constant longitudinal velocity, such that the lateral displacement and heading angle errors between the control point and the desired path converge asymptotically to zero.

[0003] In orchards, crawler-type unmanned plant protection systems inevitably encounter cornering situations. Conventional path tracking methods often produce suboptimal tracking results when dealing with cornering situations. Some methods can only track straight lines, making it difficult to track corners, resulting in suboptimal tracking results. Summary of the Invention

[0004] The present invention provides a path tracking control method for a crawler-type unmanned plant protection machine in an orchard environment, which has at least solved the above-mentioned technical problems.

[0005] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0006] A path tracking control method for a crawler-type unmanned plant protection machine in an orchard environment comprises the following steps:

[0007] S1. Obtain the path to be tracked;

[0008] S2. The UAV continues to move, and based on the path to be tracked and each current point in the UAV's movement process, the angular velocity of each current point is dynamically output;

[0009] S21, traverse the path points in the path, obtain the path point closest to the current point, and record it as the closest point;

[0010] S22, traverse the remaining path points except the closest point in the path, preset a fixed distance, and obtain a path point that is a fixed distance away from the closest point, and record it as the target point;

[0011] S23, obtaining a navigation line by fitting the nearest point and the target point;

[0012] S24, calculating the lateral deviation and heading deviation between the current point and the navigation line;

[0013] S25. The lateral deviation and the heading deviation are used as two inputs of the controller. After fuzzy processing of the inputs, the controller outputs the angular velocity according to the fuzzy control rule table.

[0014] Furthermore, in S1, the path to be tracked is obtained by RTK positioning, and any path point in the path is represented by (x o ,y o ,yaw o ), where x o Represents the coordinates of the path point in two-dimensional space, yaw o Indicates the expected heading angle of the UAV at this path point. The expected heading angle is obtained from the path points during mapping. The specific method of obtaining it is to fit the two-dimensional coordinates of each path point to get an expected heading angle.

[0015] Furthermore, in S21, a path point closest to the current point is obtained according to the distance formula between two points in two-dimensional space, and is recorded as the closest point (x1, y1, yaw1).

[0016] Furthermore, in S22, a path point having a fixed distance from the nearest point is obtained according to the distance formula between two points in two-dimensional space, and is recorded as the target point (x2, y2, yaw2).

[0017] Furthermore, in S23, the following navigation line is obtained by fitting the nearest point (x1, y1, yaw1) and the target point (x2, y2, yaw2):

[0018] Ax+By+C=0;

[0019] A=y2-y1;

[0020] B = x1-x2;

[0021] C=x2y1-x1y2.

[0022] Furthermore, in the step S24, the lateral deviation between the current point (x0, y0, yaw0) and the navigation line Ax+By+C=0 is calculated.

[0023] Furthermore, in S24, the heading deviation Erro_yaw=yaw1-yaw0 between the current point (x0, y0, yaw0) and the navigation line Ax+By+C=0 is calculated.

[0024] Furthermore, in S25, the fuzzy variables in the fuzzification processing are defined as follows: the range of the lateral deviation d is [-1, 1], the quantization level is {-3, -2, -1, 0, 1, 2, 3,} = {negative large, negative medium, negative small, 0, positive small, positive medium, positive large}, and the quantization factor is 6; the range of the heading deviation Erro_yaw is [-60, 60], the quantization level is {-3, -2, -1, 0, 1, 2, 3} = {negative large, negative medium, negative small, 0, positive small, positive medium, positive large}, and the quantization factor is 0.05.

[0025] Compared with the prior art, the present invention has the following beneficial effects:

[0026] The prior art usually fits a fixed straight line to achieve path tracking, and it is difficult to achieve curve tracking. The present invention solves the problem of unsatisfactory curve tracking effect by collecting multiple path points and dynamically fitting the navigation line during the path tracking process of the unmanned plant protection aircraft, improves the curve tracking effect of the automatic navigation of the unmanned plant protection aircraft, and realizes the path tracking of any path. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 is a flowchart of the method of the present invention DETAILED DESCRIPTION OF THE EMBODIMENTS

[0028] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the scope of protection of the present invention.

[0029] Motion control is one of the core links of the entire system of an automatic navigation vehicle. It outputs corresponding control signals according to the predetermined path and the driving state of the vehicle, so that the vehicle can stably, accurately and smoothly achieve path tracking. The vehicle system model can be divided into geometric model, kinematic model, kinematic and dynamic model. Specific path tracking control methods include pure pursuit algorithm, Stanley algorithm, fuzzy control, PID control, full state feedback control, sliding mode control, model predictive control, etc.

[0030] Among them, fuzzy control is a computer digital control technology based on fuzzy set theory, fuzzy language variables and fuzzy logic reasoning. It is essentially a non-linear control. It uses fuzzy mathematics to deal with the control problems of complex systems. It does not strictly depend on the mathematical model of the controlled object, has strong robustness and adaptability, and can show good control effects when dealing with complex systems such as non-linearity, time-variation and time-delay. With the continuous improvement of the theory, it has been widely used in various engineering controls and has become an important intelligent control method.

[0031] Therefore, the present invention uses a controller based on fuzzy control to achieve the path tracking of an unmanned plant protection aircraft in an orchard environment.

[0032] As Figure 1 shown, a path tracking control method for a tracked unmanned plant protection aircraft in an orchard environment provided by the present invention includes the following steps:

[0033] S1. Obtain the path to be tracked;

[0034] S2. The unmanned plant protection aircraft continuously moves, and based on the path to be tracked and each current point during the movement of the unmanned plant protection aircraft, the angular velocity of each current point is dynamically output;

[0035] S21. Traverse the path points in the path to obtain the path point closest to the current point, denoted as the closest point;

[0036] S22. Traverse the remaining path points in the path except the closest point, preset a fixed distance, and obtain a path point at a fixed distance from the closest point, denoted as the target point;

[0037] S23. Fit a navigation line through the closest point and the target point;

[0038] S24. Calculate the lateral deviation and course deviation between the current point and the navigation line;

[0039] S25. Take the lateral deviation and course deviation as the two input quantities of the controller. After fuzzy processing the input quantities, according to the fuzzy control rule table, the controller outputs the angular velocity.

[0040] In the present invention, by collecting multiple path points and dynamically fitting a navigation line during the path tracking of the unmanned plant protection aircraft, the problem of unsatisfactory curve tracking effect can be effectively solved, the curve tracking effect of the automatic navigation of the unmanned plant protection aircraft can be improved, and the path tracking of any path can be realized.

[0041] In some embodiments of the present invention, in S1, the path to be tracked is obtained through RTK positioning (Real-Time Kinematic, a high-precision differential global navigation satellite system positioning technology), and any path point in the path is represented as (x o , y o , yaw o ), where x o represents the coordinates of the path point in the two-dimensional space, and yaw o represents the expected course angle of the unmanned plant protection aircraft at this path point.

[0042] In some embodiments of the present invention, in S21, according to the distance formula between two points in the two-dimensional space the path point closest to the current point is obtained, denoted as the closest point (x1, y1, yaw1). Take the minimum value of all calculated d, and the corresponding path point is the closest point to the current point.

[0043] In some embodiments of the present invention, in S22, according to the distance formula between two points in the two-dimensional space Obtain a path point closest to the current point and a path point at a fixed distance from the closest point, denoted as the target point (x2, y2, yaw2). When d is the preset fixed distance, the corresponding path point is the target point of the closest point. The fixed distance is determined according to the sparsity of the path points in the path to be tracked, but it must be ensured that there is at least one target point within the fixed distance that can be fitted with the closest point into a navigation line.

[0044] In some embodiments of the present invention, in S23, the following navigation line is obtained by fitting according to the closest point (x1, y1, yaw1) and the target point (x2, y2, yaw2):

[0045] Ax + By + C = 0;

[0046] A = y2 - y1;

[0047] B = x1 - x2;

[0048] C = x2y1 - x1y2.

[0049] In some embodiments of the present invention, in S24, calculate the lateral deviation of the current point (x0, y0, yaw0) from the navigation line Ax + By + C = 0 In S24, calculate the heading deviation of the current point (x0, y0, yaw0) from the navigation line Ax + By + C = 0, Erro_yaw = yaw1 - yaw0.

[0050] In S25 of the present invention, the lateral deviation and the heading deviation are used as two input quantities of the controller. After fuzzy processing of the input quantities, according to the fuzzy control rule table, the controller outputs the angular velocity. The definitions of the fuzzy variables in the fuzzy processing are as follows: the range of the lateral deviation d is [-1, 1], the quantization levels are {-3, -2, -1, 0, 1, 2, 3} = {negative large, negative medium, negative small, 0, positive small, positive medium, positive large}, and the quantization factor is 6; the range of the heading deviation Erro_yaw is [-60, 60], the quantization levels are {-3, -2, -1, 0, 1, 2, 3} = {negative large, negative medium, negative small, 0, positive small, positive medium, positive large}, and the quantization factor is 0.05.

[0051] In the actual program, a custom definition of the fuzzy control rule table is as follows,

[0052] The encoded angular velocity is an integer, specifically as follows:

[0053] Negative large (PB): 0;

[0054] Negative medium (PM): 1;

[0055] Negative small (PS): 2;

[0056] Zero (ZO): 3;

[0057] Positive Small (NS): 4;

[0058] Positive Middle (NM): 5;

[0059] Positive Big (NB): 6;

[0060] These codes are used to represent the angular velocity that needs to be output under specific lateral error and heading error conditions. The lateral error and heading error are stored in two integer variables z and re, and the values of these variables will be used to index the ruleMatrix matrix. The ruleMatrix[7][7] matrix is used to store the angular velocity that needs to be output under different combinations of lateral error and heading error. Each element ruleMatrix[z][re] of the matrix represents the angular velocity that needs to be output when the lateral error is z and the heading error is re.

[0061] The ruleMatrix[7][7] matrix is as follows: ruleMatrix[7][7] = {{NB, NB, NM, NM, NS, ZO, ZO}, {NB, NB, NM, NS, NS, ZO, PS}, {NM, NM, NM, NS, ZO, PS, PS}, {NM, NM, NS, ZO, PS, PM, PM}, {NS, NS, ZO, PS, PS, PM, PM}, {NS, ZO, PS, PM, PM, PM, PB}, {ZO, PM, PM, PM, PM, PB, PB}};

[0062] Define the following constants to represent different angular velocities (unit: degree, positive value represents right turn, negative value represents left turn):

[0063] NB represents a large positive right turn, with a value of 400;

[0064] NM represents a medium positive right turn, with a value of 200;

[0065] NS represents a small positive right turn, with a value of 150;

[0066] ZO represents zero angle, that is, no turn, with a value of 0;

[0067] PS represents a small negative left turn, with a value of -150;

[0068] PM represents a medium negative left turn, with a value of -200;

[0069] PB represents a large negative left turn, with a value of -400;

[0070] Finally, the angular velocity output by the controller is:

[0071] ruleMatrix[z][re] * 0.001 rad / s.

[0072] Finally, it should be noted that the above embodiments are only preferred embodiments of the present invention to illustrate the technical solutions of the present invention, rather than limiting it, and certainly not limiting the patent scope of the present invention; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the various embodiments of the present invention; that is to say, any meaningless modifications or polishing made on the main design concept and spirit of the present invention, as long as the technical problems solved are still the same as those of the present invention, should be included in the protection scope of the present invention; in addition, directly or indirectly applying the technical solutions of the present invention to other related technical fields shall also be included in the patent protection scope of the present invention by the same token.

Claims

1. A path tracking control method for a tracked unmanned plant protection machine in an orchard environment, characterized in that, It includes the following steps: S1. Obtain the path to be tracked; S2. The unmanned plant protection aircraft continuously moves, and based on the path to be tracked and each current point during the movement of the unmanned plant protection aircraft, dynamically outputs the angular velocity of each current point; S21. Traverse the path points in the path to obtain the path point closest to the current point, denoted as the closest point; S22. Traverse the remaining path points in the path except the closest point, preset a fixed distance, and obtain a path point at a fixed distance from the closest point, denoted as the target point; S23. Fit a navigation line through the closest point and the target point; S24. Calculate the lateral deviation and heading deviation between the current point and the navigation line; S25. Use the lateral deviation and heading deviation as the two input quantities of the controller. After fuzzy processing of the input quantities, according to the fuzzy control rule table, the controller outputs the angular velocity.

2. A path tracking control method for a crawler-type unmanned plant protection machine in an orchard environment according to claim 1, characterized in that, In S1, the path to be tracked is obtained through RTK positioning, and any path point in the path is represented as (x o , y o , yaw o ), where x o represents the coordinates of the path point in the two-dimensional space, and yaw o represents the expected heading angle of the unmanned plant protection aircraft at this path point.

3. A path tracking control method for a tracked unmanned plant protection machine in an orchard environment according to claim 1, characterized in that, In S21, according to the distance formula between two points in the two-dimensional space, obtain the path point closest to the current point, denoted as the closest point (x1, y1, yaw1).

4. A path tracking control method for a crawler-type unmanned plant protection machine in an orchard environment according to claim 3, characterized in that, In S22, according to the distance formula between two points in the two-dimensional space, obtain a path point at a fixed distance from the closest point, denoted as the target point (x2, y2, yaw2).

5. A path tracking control method for a crawler-type unmanned plant protection machine in an orchard environment according to claim 4, characterized in that In S23, according to the closest point (x1, y1, yaw1) and the target point (x2, y2, yaw2), fit to obtain the following navigation line: Ax + By + C = 0; A = y2 - y1; B = x1 - x2; C = x2y1 - x1y2.

6. A path tracking control method for a crawler-type unmanned plant protection machine in an orchard environment according to claim 5, characterized in that In the step S24, calculate the lateral deviation between the current point (x0, y0, yaw0) and the navigation line Ax + By + C = 0 7. A path tracking control method for a tracked unmanned plant protection machine in an orchard environment according to claim 5, characterized in that, In S24, calculate the heading deviation Erro_yaw = yaw1 - yaw0 between the current point (x0, y0, yaw0) and the navigation line Ax + By + C = 0.

8. A path tracking control method for a crawler-type unmanned plant protection machine in an orchard environment according to claim 1, characterized in that, In S25, the definitions of fuzzy variables in the fuzzy processing are as follows: the range of the lateral deviation d is [-1, 1], the quantization levels are {-3, -2, -1, 0, 1, 2, 3} = {negative large, negative medium, negative small, 0, positive small, positive medium, positive large}, and the quantization factor is 6; the range of the heading deviation Erro_yaw is [-60, 60], the quantization levels are {-3, -2, -1, 0, 1, 2, 3} = {negative large, negative medium, negative small, 0, positive small, positive medium, positive large}, and the quantization factor is 0.05.

Images