CN113361764A

CN113361764A - Path planning method for automatic operation of agricultural machinery

Info

Publication number: CN113361764A
Application number: CN202110614366.8A
Authority: CN
Inventors: 崔红杰; 毛保松; 代顺治
Original assignee: Shenzhen Binghe Navigation Technology Co ltd
Current assignee: Shenzhen Binghe Navigation Technology Co ltd
Priority date: 2021-06-02
Filing date: 2021-06-02
Publication date: 2021-09-07
Anticipated expiration: 2041-06-02
Also published as: CN113361764B

Abstract

The invention discloses a path planning method for automatic operation of an agricultural machine, and belongs to the technical field of automatic driving of agricultural machines. The path planning method for automatic operation of the agricultural machinery comprises the following steps: (1) generating global path points and generating a global path of the whole operation land block; (2) dividing the whole operation land into a plurality of connected units; (3) when the agricultural machinery moves at the adjacent global path point, the actually tracked path is a local path point, the local path point is generated by interpolation of a B spline, the B spline takes the global path point as a node, and a tangent vector near the global path point as a control point; (4) in each unit, a Traveling Salesman (TSP) algorithm is adopted to carry out the switching sequence of N operation lines in the unit, thereby realizing the updating of the paths and finishing the automatic operation. The planning method of the invention increases the interpolation precision, reduces the calculation time of the TSP operation sequence and reduces the storage cost of the operation sequence.

Description

Path planning method for automatic operation of agricultural machinery

Technical Field

The invention relates to the technical field of automatic driving of agricultural machinery, in particular to a path planning method for automatic operation of agricultural machinery.

Background

In the automatic driving engineering of agricultural machinery, trajectory planning plays a key role in lifting weight. However, if the distance between the tracking points generated by the algorithm is too large, the tracking error of the agricultural machinery is large; the generated tracking points have small distance, which causes a large memory space for algorithm application.

Trajectory planning is essentially a path optimization problem, i.e. it is desirable to find a shortest path through all the lines. The prior art for solving the path optimization problem has various methods, such as Dijkstra algorithm, a-x algorithm, Floyd algorithm, etc., but these methods are not well suited for automatic operation of agricultural machines. The method uses a prior art: the travelling quotient algorithm (TSP algorithm for short) can be used for track planning of automatic operation of agricultural machinery, but if the number of operation lines under actual working conditions is large, the solving efficiency of the large-scale travelling quotient algorithm is reduced. And even if the total path can be shortest by solving the operation sequences of all the operation lines at one time, the operation sequences do not accord with the actual working conditions: firstly, the operation sequence may have the condition of extremely large span, secondly, under the actual working condition, the agricultural machinery cannot finish all the operation lines at one time, and all the operation lines under the actual working condition can be finished in a segmented manner within several days.

Disclosure of Invention

Based on this, the embodiment of the invention provides a path planning method for automatic operation of an agricultural machine, aiming at reducing the occupied space of planning path points during large-area operation.

In order to achieve the purpose, the invention provides the following technical scheme:

a path planning method for automatic operation of agricultural machinery specifically comprises the following steps: dividing the operation land block into three layers of structures, namely a unit, a global path point and a local path point; the whole operation land block is divided into a plurality of connected units, a plurality of operation lines are arranged in the units, the operation lines are determined by a group of global path points, the local path points are generated by B-spline (B-spline) interpolation, the B-spline takes the global path points as nodes, and tangent vectors near the global path points as control points. The tangent vector is generated by neighboring global path points.

The distance between the global path points is in the meter level, so the error of directly tracking the global path points is large. The actual tracked path of vehicle is local path point, and the interval of local path point is at decimetre level, also can increase interpolation point according to actual conditions and reduce the interval to centimetre level, can increase like this and track the precision, but also can increase the cost of calculation and storage data simultaneously.

A path planning method for automatic operation of agricultural machinery comprises the following steps:

(1) generating global path points of a global path of the operation land block;

(2) dividing the whole operation land into a plurality of connected units, wherein N operation lines are arranged in each unit; the number N of the operating lines of a single unit is confirmed according to the minimum steering radius R _ min and the operating Line distance Line _ Width; the line is determined by a set of global path points;

(3) when the agricultural machinery moves at the adjacent global path point, the actually tracked path is a local path point, the local path point is generated by interpolation of a B spline, the B spline takes the global path point as a node, and a tangent vector near the global path point as a control point;

(4) in each unit, a Traveling Salesman (TSP) algorithm is adopted to carry out the switching sequence of N operation lines in the unit, thereby realizing the updating of the paths and finishing the automatic operation.

Generating the global path point in the step (1) specifically includes: inputting a starting point and a tail point of a first operating Line (wherein the starting point of the operating Line is marked as a point A and the tail point of the operating Line is marked as a point B in the embodiment) in a local coordinate system, and inputting 5 parameters including an operating Line distance Line _ Width, an operating Line quantity Line _ Num and a minimum steering radius R _ min to plan a global path point of the whole land block; the line is determined by a set of global path points.

Through the input of the position information of the initial point and the end point of the first operating Line, the information of the initial point, the included angle theta between the operating Line and the geographical north and the Length Line _ Length of the operating Line can be obtained. The global path point on the first line is generated by linear interpolation between AB points; and initializing an index _ Global representing the position of the Global path point after generation, wherein the index _ Global is used for identifying the Global path point closest to the current agricultural machinery.

The global path in the step (1) is formed by a working line or a steering path; when the agricultural machine drives into the tail end of the operation line, inquiring the serial number of the next operation line to generate a steering path, and generating the steering path by using the position relation of the two operation lines to enable the agricultural machine to perform steering transition; when the global path type is a steering path, generating a next line by using the serial number of the next line; according to the operating position of the agricultural machinery, the operating line or the steering path is sequentially generated one by one, and the advantage of generating the path is that the data volume can be reduced; the disadvantage of generating large data volumes for all global paths simultaneously is avoided.

The method for generating the operating line specifically comprises the following steps: when the current position of the vehicle is close to the end of a steering path, a global path is updated to be the operating Line, and the type of the global path is set to be 0; each Line has a number index _ Line in the vehicle operation process, the Line can be generated through the number, the Line distance, the Line length and the angle, and the running direction of the operation can be judged through judging the odd-even number of the number; when the path is updated, the new path will cover the old path.

The generation method of the steering path specifically comprises the following steps: if the current position of the vehicle is close to the tail of the operating line, updating the global path into a steering path, and setting the type of the global path to be 1; taking the terminal point P1 of the current operating line and the terminal point P2 of the next operating line, inserting two circle centers with the radius being the minimum steering radius between the connecting lines, wherein the vector of the circle center 1 pointing to the P1 is a radius vector 1, and rotating the vector by 90 degrees to generate a 1/4 circular arc going out of the current operating line; changing the center of the radius vector 1 from the center 1 to the center 2, rotating the center by 90 degrees to form a radius vector 2, and generating an 1/4 circular arc entering the next line by the same method; then judging whether the point at the joint of the two circular arcs is too close, if so, generating kinks or sawteeth in the generated interpolation, and needing to be combined; if the distance is too far, taking the tail point of the first section of circular arc and the initial point of the second section of circular arc, and performing linear interpolation between the two points until the distance between the points is smaller than a desired value; the new path will overwrite the old path.

The rotation of the radius vector in the steering path has directionality, the direction of the embodiment is determined by cross multiplication, the cross multiplication of the direction vector of the current path and the radius vector 1 is calculated, if the cross multiplication result is positive, clockwise rotation is carried out, and if not, anticlockwise rotation is carried out; the method of determining the rotation direction is not limited to this.

When the Line number Line _ Num in the step (1) is uncertain and continuously operated; line _ Num can be set to a sufficiently large number (or to infinity) at which point the vehicle will continue to track the path until the user manually switches out of the automatic state. When the direction indicated by the AB vector is the direction of the first line, all the lines are generated on one side of the first line.

In step (2):

the number N of the operation lines of the single unit meets the following conditions: n ═ 2 × ceil (2 × R _ min/Line _ Width) + a; wherein the ceil function represents rounding up, R _ min represents the minimum steering radius, Line _ Width represents the working Line spacing, and a is an integer greater than or equal to 1.

In actual circumstances, the lower limit of the number N of lines per unit (when N is 1) may be increased as appropriate, but cannot be decreased, and the decrease will result in a path that cannot be generated (the maximum steering angle of the front wheels will exceed 30 degrees, and steering according to the planned path will not be performed).

The number of the units connected is equal to floor (Line _ Num/N), wherein the floor function is rounded downwards; line _ Num is the total number of the operating lines; n is the number of lines of a single unit. After the number of units is determined, an index _ Block for identifying the unit is initialized to identify the unit where the current vehicle is located.

In the step (3):

the local path points are generated near the nearest global path point of the agricultural machine through 3 times of B-spline generation, a plurality of global path points near the agricultural machine participate in the generation of the local path points, the global path points are used as nodes of the local path points, and an algorithm inserts two control points beside each node by calculating the slope of a connecting line of the nodes; the B-spline thus generated will pass through the nodes, but not the control points. In the algorithm of the prior art, the path points are not set as spline nodes, so that the generated splines do not pass through the path points; the problem is solved by changing the path points into the nodes by inserting the control points, and the interpolation precision is increased; after the Local path is initialized, an index _ Local indicating the position of the Local path point is initialized to identify the Local path point closest to the current vehicle.

The algorithm generation method of the control points comprises the following steps:

1) checking whether the current node is the first global path point or not, if so, using the difference value of the next global path point minus the current global path point as a tangent vector;

2) checking whether the current node is the last global path point or not, and if so, using the difference value of the current global path point minus the previous global path point as a tangent vector;

3) if the current node is between the starting path point and the ending path point, the half-difference value of the previous path point subtracted by the previous path point is used as a tangent vector;

4) and generating two control points by adding and subtracting tangent vectors by using the current node coordinates.

The tracking algorithm adopts pure tracking control, 3 parameters are required to be input in the pure tracking control, namely a control period dt, a vehicle wheel base L and a longitudinal control feedback rate, and a State vector is [ State _ Pos _ E, State _ Pos _ N, State _ loading and State _ Speed ]; the State _ Pos _ E is an east coordinate of the vehicle position in the local coordinate system, the State _ Pos _ N is a north coordinate of the vehicle position in the local coordinate system, the State _ Heading is a vehicle course angle, and the State _ Speed is a vehicle Speed.

During initialization, State _ Pos _ E is set as the abscissa of the A point of the starting point of the working line, State _ Pos _ N is set as the ordinate of the A point, State _ header is set as atan2(B (2) -A (2), B (1) -A (1)), and State _ Speed is set as the longitudinal control feedback rate; wherein point B is the end point of the line.

The generation process of the local path point specifically comprises the following steps; the method comprises the steps of dynamically judging a global path point n closest to the actual position of a vehicle in the driving process of the vehicle, then generating a local path between global path points n-1 to n +2 (the embodiment is not limited to four points, the number of the local path points is increased by increasing the number of the global path points, and the local path points closest to the actual position of the vehicle are judged according to the configuration of specific working conditions) near the global path points n-1 to n + 2. All points are updated in real time during vehicle motion.

In step (4):

the traveler algorithm (TSP) gives the switching order of the N lines in the cell, so that the travel distance of the vehicle is locally optimized. In the TSP algorithm, a working line with the spacing smaller than 2 × R _ min is taken as an infinite distance, so that the generated route conforms to the Ackerman steering principle, the change rate of the front wheel rotation angle is smooth, and the maximum front wheel rotation angle can be automatically limited within a reasonable range under the condition of not adding additional constraint conditions; after the switching sequence of the working lines is determined, an index _ Rank for identifying the position of the working line is initialized to identify the working line of the current vehicle.

All the indexes are updated in real time in the process of tracking the path by the vehicle.

The path updating specifically comprises: when the main cycle starts, judging that an updating unit or a working line is not needed under the current vehicle state, if so, updating the unit index _ Block to be index _ Block +1 or updating the working line index _ Rank to be index _ Rank + 1; the Global path point index _ Global is reset when the job line is updated.

When the vehicle is on the operating line, giving a label to the global path point (for example, using label 0); when the vehicle is on the turning path, giving another label to the global path point (for example, using the label 1); when the global path is updated, judging the type of the next global path by judging the current label; if the current global path is the operation line, the next global path is updated to be the steering path; and if the current global path is the steering path, updating the next global path into the operation line.

Inquiring the global path point closest to the current vehicle state, and if the current global path point changes, updating the local path; and after updating, inquiring a local path point closest to the current vehicle state, setting a pre-aiming distance by taking the point as a path starting point, integrating the path on the local path to obtain a pre-aiming point, tracking the pre-aiming point through pure tracking control, and transversely controlling the vehicle through a front wheel corner output by the pure tracking control to finish vehicle state updating.

The path planning method for automatic operation of the agricultural machinery comprises the following specific operation logic flow steps:

1) initializing a vehicle position, initializing a global path, and selecting the type of the initialized global path as a working line;

2) by judging that the global path point closest to the vehicle position is close to the end of the global path,

2.1) when the global path point closest to the vehicle position is not close to the tail of the global path, inquiring the global path point closest to the current vehicle position, generating a local path point by using a B-spline near the inquired global path point, inquiring the local path point closest to the current vehicle position, updating the vehicle state by a track tracking controller, and judging in the step 2) again;

2.2) when the global path point closest to the vehicle position is close to the tail of the global path, judging the type of the current path;

2.2.1) when the current path type is judged to be the steering path, generating a working line as a global path, and switching the global path type to be the working line; inquiring a global path point closest to the current vehicle position;

2.2.2) judging whether the current unit is the last line of the last unit when the current path type is judged to be the line;

2.2.2.1) judging that the current unit is the last line of the last unit, and ending the tracking;

2.2.2.2) judging whether the current unit is the last line of the last unit or not, and judging whether the current line is the last line of the current unit or not;

2.2.2.2.1) judging that the current operation line is the last operation line of the current unit; updating the repeating unit;

2.2.2.2.2) determining that the current line is not the last line of the current cell, generating a diversion path as a global path, and switching the global path type to the diversion path.

Compared with the prior art, the technical scheme of the invention has the following beneficial effects:

1. the invention divides the operation land into a plurality of connected units, and the vehicle enters the next unit after walking one unit; the unit is arranged because a large number of (for example, 100) operation lines cannot be operated at one time, the number of the operation lines is excessive, the span of the operation lines generated by a Traveling Salesman (TSP) algorithm is large, and the algorithm calculation time is long; the storage job sequence also requires a large space; therefore, a large number of operation lines are split, a plurality of small units are generated, and the operation is carried out one by one; thus, the method conforms to the actual working condition, reduces the calculation time of the TSP operation sequence, and reduces the storage cost of the operation sequence.

2. In the algorithm of the prior art, the path points are not set as spline nodes, so that the generated splines do not pass through the path points; the invention solves the problem by changing the path points into the nodes by inserting the control points, and increases the interpolation precision.

3. The global path is composed of a plurality of operation lines and a steering path; in the process of generating the operating line, the operating line is generated one by one; according to the operating position of the agricultural machine, a next line is generated when the previous line turns around; the benefit of this generation is that the amount of data can be reduced.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the structures shown in the drawings without creative efforts.

FIG. 1 is a diagram illustrating a global path according to an embodiment of the present invention;

FIG. 2 is a schematic illustration of a divert path as described in an embodiment of the present invention;

FIG. 3 is a schematic diagram of a process for generating a divert path in an embodiment of the present invention;

FIG. 4 is a diagram illustrating a process of generating a local path according to an embodiment of the present invention;

FIG. 5 is a first diagram illustrating local waypoint tracking according to an embodiment of the present invention;

FIG. 6 is a second exemplary diagram illustrating tracking of local waypoints in accordance with the present invention;

FIG. 7 is a partial schematic diagram of a unit and a global waypoint of a work block partition in an embodiment of the invention;

FIG. 8 is a schematic diagram of a unit and global waypoints for a work block partition in an embodiment of the invention;

fig. 9 is a schematic flow chart of the path planning method according to the embodiment of the present invention.

Wherein in fig. 1-8: e of the transverse coordinate axis refers to east coordinates of the vehicle position in the local coordinate system; and N of the longitudinal coordinate axis refers to the north coordinate of the vehicle position in the local coordinate system.

The implementation, functional features and advantages of the objects of the present invention will be further explained with reference to the accompanying drawings.

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

It should be noted that, if directional indications (such as up, down, left, right, front, back, top and bottom … …) are involved in the embodiment of the present invention, the directional indications are only used to explain the relative position relationship between the components, the motion situation, etc. in a specific posture (as shown in the drawing), and if the specific posture is changed, the directional indications are changed accordingly.

In addition, if there is a description of "first", "second", etc. in an embodiment of the present invention, the description of "first", "second", etc. is for descriptive purposes only and is not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In addition, technical solutions between various embodiments may be combined with each other, but must be realized by a person skilled in the art, and when the technical solutions are contradictory or cannot be realized, such a combination should not be considered to exist, and is not within the protection scope of the present invention.

The invention provides a path planning method for automatic operation of agricultural machinery, aiming at solving the technical problem of reducing the occupied space of planning path points during large-area operation.

The path planning method is further described in detail with reference to the following examples, but the implementation of the algorithm is not limited thereto.

During the operation of the agricultural machinery, under the working condition of 100 operating lines of 300 meters, it is not practical to finish 100 operating lines at a time, and the operating line given by the TSP algorithm may have a very large span, such as spanning from the 10 th operating line to the 80 th operating line and then spanning to the 5 th operating line. And when the number of the lines is too large, the resolving rate of the optimal path also becomes slow.

The unit structure design adopted by the path planning method accords with the actual operation condition, and the problem that the large-scale TSP is difficult to solve quickly is solved. The present algorithm can split 100 lines into several cells such that each cell satisfies the solution of the TSP problem. Although the result is that the global planned path is not the shortest, it is still locally optimal. The planned path is more consistent with the actual working condition, and the calculated amount and the storage amount of data are reduced.

Based on this, the embodiment of the invention provides a path planning method for automatic operation of an agricultural machine, which specifically comprises the following steps: dividing the operation land block into three layers of structures, namely a unit, a global path point and a local path point; the whole operation land block is divided into a plurality of connected units, a plurality of operation lines are arranged in the units, the operation lines are determined by a group of global path points, the local path points are generated by B-spline (B-spline) interpolation, the B-spline takes the global path points as nodes, and tangent vectors near the global path points as control points; the tangent vector is generated by neighboring global path points.

(1) generating global path points of a global path of the operation land block;

(4) in each unit, a Traveling Salesman (TSP) algorithm is adopted to carry out the switching sequence of the N operation lines in the unit, thereby realizing the updating of the path and finishing the planning operation of the path.

In step (1):

the generating of the global path point specifically includes: inputting a starting point and a tail point of a first operating Line (wherein the starting point of the operating Line is marked as a point A and the tail point of the operating Line is marked as a point B in the embodiment) in a local coordinate system, and inputting 5 parameters including an operating Line distance Line _ Width, an operating Line quantity Line _ Num and a minimum steering radius R _ min to plan a global path point of the whole land block; the line is determined by a set of global path points.

Through the input of the position information of the initial point and the final point of the first operating line, the included angle theta between the initial point, the operating line and the geographical north can be obtained, the Global path point on the operating line is generated through linear interpolation between the points AB, and an index _ Global which represents the position of the Global path point is initialized after the generation, and is used for identifying the Global path point which is closest to the current agricultural machinery.

The global path is composed of a plurality of operation lines and a steering path; in the process of generating the operating line, the operating line is generated one by one; and according to the information of the agricultural machinery operation and the Line Length Line _ Length of the operating Line.

The line of the global path is mainly composed as shown in fig. 1. In the example of fig. 1, the direction of the first line is from (0,0) to the upper right; turning around and driving into the next line after the operation of each line is finished until all lines are finished; in actual tracking, all the working lines shown in fig. 1 are not generated at once, but generated one by one, and a working line is generated when the agricultural machinery moves to the starting point of one working line, so that the data amount is reduced.

The method for generating the operating line specifically comprises the following steps: the user is required to input an AB point (wherein A is a starting point of a working Line, B is a tail point of the working Line, an included angle Theta between the working Line and geographical north and a Line Length Line _ Length of the working Line can be obtained through the AB point, the Line distance Line _ Width of the working Line and the number Line _ Num of the working Line, when the current position of the vehicle is close to the tail of a steering path, the global path is updated to the working Line, and the type of the global path is set to be 0. Each Line has a number index _ Line in the vehicle operation process, the Line can be generated through the number, the Line distance, the Line length and the angle, and the running direction of the operation can be judged through judging the odd-even number of the number; when the path is updated, the new path will cover the old path.

The method for generating the turning path (as shown in fig. 2 and 3) specifically includes: if the current position of the vehicle is close to the tail of the operating line, updating the global path into a steering path, and setting the type of the global path to be 1; taking the end point P1 of the current operating line and the end point P2 of the next operating line, inserting two circle centers with the radius being the minimum steering radius between the connecting lines, wherein the vector of the circle center 1(O1) pointing to the P1 is a radius vector 1(R1), and rotating the vector by 10, 20, …, 80 and 90 degrees to generate an 1/4 arc going out of the current operating line; changing the center of the radius vector 1(R1) from the center 1(O1) to the center 2(O2), rotating 90 degrees to become a radius vector 2(R2), and generating 1/4 circular arc entering the next line by the same method; then judging whether the point at the joint of the two circular arcs is too close, if so, generating kinks or sawteeth in the generated interpolation, and needing to be combined; if the distance is too far, taking the tail point of the first section of circular arc and the initial point of the second section of circular arc, and performing linear interpolation between the two points until the distance between the points is smaller than a desired value; setting an upper limit during interpolation, and jumping out of interpolation if the number of interpolation points is excessive; when the path is updated, the new path can cover the old path; the vehicle tracking accuracy is not considered when steering.

In the simulation, only the path generation of the rectangular field is considered, the path generation mode of the parallelogram land is similar, and the path generation of the parallelogram land is not considered in general.

When the Line number Line _ Num in the step (1) is uncertain and continuously operated; line _ Num can be set to a sufficiently large number (or to infinity) at which point the vehicle will continue to track the path until the user manually switches out of the automatic state; when the direction indicated by the AB vector is the direction of the first line, all the lines are generated on one side of the first line.

In step (2):

The number of the units connected is equal to floor (Line _ Num/N), wherein the floor function is rounded downwards; line _ Num is the total number of the operating lines; n is the number of lines of a single unit.

After the number of units is determined, an index _ Block for identifying the unit is initialized to identify the unit where the current vehicle is located.

When the vehicle runs out of one unit, the vehicle enters the next unit; the unit is arranged because a large number of (for example, 100) operation lines cannot be operated at one time, the number of the operation lines is excessive, the span of the operation lines generated by a Traveling Salesman (TSP) algorithm is large, and the algorithm calculation time is long; the storage job sequence also requires a large space; therefore, a large number of operation lines are split, a plurality of small units are generated, and operation is carried out one by one. Thus, the method conforms to the actual working condition, reduces the calculation time of the TSP operation sequence, and reduces the storage cost of the operation sequence. The working sequence used in the embodiment is the number of the working line, for example, the current working line is the 5 th working line, the next working line is the 12 th working line, the next working line is the 4 th working line, and the stored sequence is (5,12, 4). However, a differential sequence may be used, and in the same example as above, if a differential sequence is used, the current line is the 5 th line, the next line is the 5+7 th line, the next line is the 12-8 lines, and the stored differential sequence is (5,7, -8). The two schemes can produce the same technical effect and are within the protection scope of the invention.

The last unit in step (2) for the work parcel; since the total number of lines is not necessarily an integer multiple of the number of lines in a cell, the last cell should be merged with the remaining lines. For example, in the case of "100 lines 300 meters", if the number of lines of a cell is 11, the first 8 cells have the same structure, but the number of lines of the 9 th cell is 12, so that all lines can be traversed (100 ═ 11 × 8+12 × 1). When the unit is generated in the step (2), the operation sequence of the first unit is actually generated, in the above example of 100 lines, the 2 nd to 8 th units are the same as the 1 st unit, and the 9 th unit independently generates the operation sequence.

In the step (3):

Setting State _ Pos _ E to the abscissa of the point A (the starting point mark of the working line), State _ Pos _ N to the ordinate of the point A, State _ Heading to atan2(B (2) -A (2), B (1) -A (1)), State _ Speed to the longitudinal control feedback rate at initialization; wherein point B is the end point of the line.

The generation process of the local path point is schematically shown in fig. 4; the method comprises the steps of dynamically determining a global route point n closest to the actual position of a vehicle in the driving process of the vehicle, then generating a local route between global route points n-1 to n +2 (the embodiment is not limited to four points, the increase of the global route point number increases the local route point number, and the local route point is configured according to specific working conditions) near the n, and determining a local route point + closest to the actual position of the vehicle. All points are updated in real time during vehicle motion.

The local path point in this embodiment is a set of 3-order b-spline interpolation points, and the embodiment of the present invention is not limited to the spline order and the spline generation method. Generating a spline, wherein two points are provided, one point is a node, and the spline passes through the node; one is a control point, the spline does not pass through the control point, and the control point can control the bending direction and the bending degree of the spline. The global path point is directly used as a spline control point, so that the error is large; in order to make the b-spline pass through the global waypoints, that is, to make the global waypoints become nodes, it is necessary to additionally extrapolate 2 control points in the vicinity of each global waypoint, the 2 control points have the same distance to the local waypoints, and the three points are on the same straight line.

The length of the tangent vector in the embodiment is 0.25 meter, but is not limited to 0.25 meter, and is determined according to specific working conditions. The local waypoints generated in this way are smooth and do not create kinks or jaggies, as shown in fig. 5 and 6.

In step (4):

The path updating specifically comprises: when the main cycle starts, judging that an updating unit or a working line unit is not needed in the current vehicle state, if so, updating the unit index _ Block to be index _ Block +1 or updating the working line index _ Rank to be index _ Rank + 1; the Global path point index _ Global is reset when the job line is updated.

Inquiring the Global route point index _ transition closest to the current vehicle state, if the current Global route point has a change index _ Global! If index _ transition, updating the local path; and inquiring a Local path point index _ Local closest to the current vehicle state after updating, setting a proper pre-aiming distance by taking the point as a path starting point, integrating the path on the Local path to obtain a pre-aiming point, tracking the pre-aiming point through pure tracking control, and performing transverse control on the vehicle through a front wheel corner output by the pure tracking control to finish vehicle state updating.

Fig. 7 and 8 show schematic diagrams of the units and the global path points of the work block partition in the embodiment of the present invention.

The flow diagram of the path planning method for automatic operation of agricultural machinery in the embodiment of the invention is shown in fig. 9, and the specific operation logic flow steps are as follows:

The above description is only a preferred embodiment of the present invention, and is not intended to limit the scope of the present invention, and all modifications and equivalents of the present invention, which are made by the contents of the present specification and the accompanying drawings, or directly/indirectly applied to other related technical fields, are included in the scope of the present invention.

Claims

1. A path planning method for automatic operation of agricultural machinery is characterized in that: the method comprises the following steps:

(1) generating global path points of a global path of the operation land block;

2. The path planning method for agricultural machinery automatic operation according to claim 1, characterized in that:

generating the global path point in the step (1) specifically includes: inputting a starting point and a tail point of a first operating Line in a local coordinate system, and inputting 5 parameters including an operating Line distance Line _ Width, an operating Line quantity Line _ Num and a minimum steering radius R _ min to plan a global path point of the whole land block; the line is determined by a set of global path points.

3. The path planning method for agricultural machinery automatic operation according to claim 1, characterized in that:

the global path in the step (1) is formed by a working line or a steering path; when the agricultural machine drives into the tail end of the operation line, inquiring the serial number of the next operation line to generate a steering path, and generating the steering path by using the position relation of the two operation lines to enable the agricultural machine to perform steering transition; and when the global path type is the steering path, generating a next operation line by using the serial number of the next operation line.

4. The path planning method for agricultural machinery automatic operation according to claim 3, characterized in that: the method for generating the operating line specifically comprises the following steps: the user is required to input an AB point, the Line distance Line _ Width of the operating Line and the number Line _ Num of the operating lines; when the current position of the vehicle is close to the tail of the steering path, updating the global path into a working line, and setting the type of the global path to be 0; each Line has a number index _ Line in the vehicle operation process, the Line can be generated through the number, the Line distance, the Line length and the angle, and the running direction of the operation can be judged through judging the odd-even number of the number; when the path is updated, the new path can cover the old path; where A is the beginning of the line and B is the end of the line.

5. The path planning method for agricultural machinery automatic operation according to claim 3, characterized in that:

the generation method of the steering path specifically comprises the following steps: if the current position of the vehicle is close to the tail of the operating line, updating the global path into a steering path, and setting the type of the global path to be 1; taking the terminal point P1 of the current operating line and the terminal point P2 of the next operating line, inserting two circle centers with the radius being the minimum steering radius between the connecting lines, wherein the vector of the circle center 1 pointing to the P1 is a radius vector 1, and rotating the vector by 90 degrees to generate a 1/4 circular arc going out of the current operating line; changing the center of the radius vector 1 from the center 1 to the center 2, rotating the center by 90 degrees to form a radius vector 2, and generating an 1/4 circular arc entering the next line by the same method; then judging whether the point at the joint of the two circular arcs is too close, if so, generating kinks or sawteeth in the generated interpolation, and needing to be combined; if the distance is too far, taking the tail point of the first section of circular arc and the initial point of the second section of circular arc, and performing linear interpolation between the two points until the distance between the points is smaller than a desired value; the new path will cover the old path;

the rotation of the radius vector in the steering path has directivity, the directivity is judged through cross multiplication, the cross multiplication of the direction vector of the current path and the radius vector 1 is calculated, if the cross multiplication result is positive, clockwise rotation is carried out, and if not, anticlockwise rotation is carried out.

6. The path planning method for agricultural machinery automatic operation according to claim 1, characterized in that: in step (2): the number N of the operation lines of the single unit meets the following conditions: n ═ 2 × ceil (2 × R _ min/Line _ Width) + a; wherein the ceil function represents rounding up, R _ min represents the minimum steering radius, Line _ Width represents the working Line spacing, and a is an integer greater than or equal to 1;

7. The path planning method for agricultural machinery automatic operation according to claim 1, characterized in that:

in the step (3): the local path points are generated near the nearest global path point of the agricultural machine through 3 times of B-spline generation, a plurality of global path points near the agricultural machine participate in the generation of the local path points, the global path points are used as nodes of the local path points, and an algorithm inserts two control points beside each node by calculating the slope of a connecting line of the nodes; the B-spline thus generated will pass through the nodes, but not the control points. In the algorithm of the prior art, the path points are not set as spline nodes, so that the generated splines do not pass through the path points; the problem is solved by changing the path points into the nodes by inserting the control points, and the interpolation precision is increased; after the Local path is initialized, an index _ Local indicating the position of the Local path point is initialized to identify the Local path point closest to the current vehicle.

8. The path planning method for agricultural machinery automatic operation according to claim 7, characterized in that: the algorithm generation method of the control points comprises the following steps:

9. The path planning method for agricultural machinery automatic operation according to claim 1, characterized in that:

in step (4): the path updating specifically comprises: when the main cycle starts, judging that an updating unit or a working line is not needed under the current vehicle state, if so, updating the unit index _ Block to be index _ Block +1 or updating the working line index _ Rank to be index _ Rank + 1; resetting the Global path point index _ Global when the operation line is updated;

when the vehicle is on the operating line, giving a label to the global path point; when the vehicle is on the steering path, giving another label to the global path point; when the global path is updated, judging the type of the next global path by judging the current label; if the current global path is the operation line, the next global path is updated to be the steering path; and if the current global path is the steering path, updating the next global path into the operation line.

10. The path planning method for agricultural machinery automatic operation according to claim 1, characterized in that: the specific operation logic flow steps are as follows: