CN113361764B - Path planning method for automatic operation of agricultural machinery - Google Patents
Path planning method for automatic operation of agricultural machinery Download PDFInfo
- Publication number
- CN113361764B CN113361764B CN202110614366.8A CN202110614366A CN113361764B CN 113361764 B CN113361764 B CN 113361764B CN 202110614366 A CN202110614366 A CN 202110614366A CN 113361764 B CN113361764 B CN 113361764B
- Authority
- CN
- China
- Prior art keywords
- path
- line
- point
- global
- global path
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 239000013598 vector Substances 0.000 claims abstract description 49
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 34
- 230000008569 process Effects 0.000 claims description 13
- 230000007704 transition Effects 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 abstract description 7
- 238000010586 diagram Methods 0.000 description 9
- 230000008901 benefit Effects 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000005192 partition Methods 0.000 description 3
- 238000005452 bending Methods 0.000 description 2
- 230000003247 decreasing effect Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q10/00—Administration; Management
- G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
- G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q10/00—Administration; Management
- G06Q10/06—Resources, workflows, human or project management; Enterprise or organisation planning; Enterprise or organisation modelling
- G06Q10/063—Operations research, analysis or management
- G06Q10/0633—Workflow analysis
-
- G06Q50/40—
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 traveler algorithm (TSP) is adopted to carry out the switching sequence of N operation lines in the unit, thereby realizing the updating of the paths and completing 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
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 one of the 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 final point of a first operation Line (wherein the starting point of the operation Line is marked as a point A and the final point of the operation Line is marked as a point B in the embodiment) in a local coordinate system, and inputting 5 parameters including an operation Line distance Line _ Width, an operation Line quantity Line _ Num and a minimum turning 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 machine, the operating line or the turning path is sequentially generated one by one, so that the path generation has the advantage of reducing the data volume; 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 an end point P1 of a current operating line and an end point P2 of a next operating line, inserting two circle centers with the radius being the minimum steering radius between the connecting lines, wherein a vector pointing to the P1 from the circle center 1 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 circle center of the radius vector 1 from the circle center 1 to the circle center 2, rotating the circle center by 90 degrees to form a radius vector 2, and generating a 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 determination method of the rotation direction is not limited thereto.
When the Line quantity Line _ Num in the step (1) is uncertain and continues to operate; 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 = 1) may be increased as appropriate, but cannot be decreased, and the decrease may result in a path that cannot be generated (the maximum steering angle of the front wheels exceeds 30 degrees, and steering cannot be performed according to the planned path).
The number of the units connected is = 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 the 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 running 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 specific working conditions). All points are updated in real time during vehicle motion.
In step (4):
the traveler algorithm (TSP) gives the switching sequence 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 less than 2 × R _minis taken as an infinite distance, so that the generated route conforms to the Ackerman steering principle, the change rate of the front wheel corner is smooth, and the maximum front wheel corner can be automatically limited within a reasonable range under the condition of not additionally adding a constraint condition; 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 the unit or the operating line does not need to be updated under the current vehicle state, if so, updating the unit index _ Block = index _ Block +1, or updating the operating line index _ Rank = 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 end 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 the trajectory tracking controller, and judging in the step 2) again;
2.2 When a global path point closest to the vehicle position is close to the end of the global path, judging the type of the current path;
2.2.1 When the current path type is judged to be a steering path, a working line is generated to be used as a global path, and the global path type is switched to be the working line; inquiring a global path point closest to the current vehicle position;
2.2.2 When the current path type is judged to be the operation line, judging whether the current unit is the last operation line of the last unit;
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 line is the last line of the current unit; updating the repeating unit;
2.2.2.2.2 Judging that the current operation line is not the last operation line of the current unit, generating a steering path as a global path, and switching the type of the global path into the steering 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 Service Provider (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 schematic diagram of 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 a global waypoint of 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, and the like in a specific posture (as shown in the drawing), and if the specific posture is changed, the directional indication is 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 below with reference to the following embodiments, 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.
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 traveler algorithm (TSP) is adopted to carry out the switching sequence of N operation lines in the unit, so as to realize the updating of the path and complete 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 operation line, the operation lines are 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 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 defect of large data volume of all global paths generated at the same time is avoided.
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.
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.
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 an end point P1 of a current operation line and an end point P2 of a next operation line, inserting two circle centers with the radius being the minimum steering radius between the connection lines, wherein a 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 a 1/4 circular arc going out of the current operation line; changing the center of the radius vector 1 (R1) from the center 1 (O1) to the center 2 (O2), rotating the center 90 degrees to become a radius vector 2 (R2), and generating a 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.
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.
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 quantity Line _ Num in the step (1) is uncertain and continues to operate; line _ Num can be set to a sufficiently large number (or to infinity) at which time the vehicle will continue to track the path until the user manually switches off 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.
As a practical matter, the lower limit of the number N of lines per unit (when N = 1) can 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 cannot be performed according to the planned path).
The number of the units connected is = 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 Service Provider (TSP) algorithm is large, and the algorithm calculation time is long; the storage 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, 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, the next line is 12 to 8, 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 the unit lines is 11, the first 8 units have the same structure, but the number of the unit lines is 12, so that all the line lines (100 =11 + 8+12 + 1) can be traversed. 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):
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 knots 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 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.
Setting State _ Pos _ E to be an abscissa of a point A (a starting point mark of a working line), setting State _ Pos _ N to be an ordinate of the point A, setting State _ Heading to be atan2 (B (2) -A (2), B (1) -A (1)), and setting State _ Speed to be a longitudinal control feedback rate; 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 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 or subtracting tangent vectors by using the current node coordinates.
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 created in this way are smooth and do not kink or jagge as shown in fig. 5 and 6.
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 _minis taken as an infinite distance, so that the generated route conforms to the Ackerman steering principle, the change rate of the front wheel corner is smooth, and the maximum front wheel corner 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 the updating unit or the working line unit is not needed in the current vehicle state, if so, updating the unit index _ Block = index _ Block +1 or updating the working line index _ Rank = 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, adopting a 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 route point index _ transition closest to the current vehicle state, if the current Global route point has a change index _ Global! If = index _ transition, the local path is updated; 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 schematic 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:
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 end 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 the trajectory 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 end of the global path, judging the current path type;
2.2.1 When the current path type is judged to be a steering path, a working line is generated to be used as a global path, and the global path type is switched to be the working line; inquiring a global path point closest to the current vehicle position;
2.2.2 When the current path type is judged to be the operation line, judging whether the current unit is the last operation line of the last unit;
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 Determine that the current line is the last line of the current cell; updating the repeating unit;
2.2.2.2.2 Judging that the current operation line is not the last operation line of the current unit, generating a steering path as a global path, and switching the type of the global path into the steering path.
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 (7)
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) 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 switching sequence of N operation lines in the unit, so as to realize path updating and complete automatic operation;
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;
the number of the units connected is = 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 the operation lines of a single unit;
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; after the Local path is initialized, initializing an index _ Local which represents the position of the Local path point and is used for identifying 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.
2. The path planning method for agricultural machinery automatic operation according to claim 1, characterized in that:
in the step (1), generating the global path point 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 make the agricultural machine turn for 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 operation line, updating the global path into a steering path, and setting the type of the global path to be 1; taking an end point P1 of a current operating line and an end point P2 of a next operating line, inserting two circle centers with the radius being the minimum steering radius between the connecting lines, wherein a vector pointing to the P1 from the circle center 1 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 circle center of the radius vector 1 from the circle center 1 to the circle center 2, rotating the circle center by 90 degrees to form a radius vector 2, and generating a 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 (4): the path updating specifically includes: when the main cycle starts, judging that the unit or the operating line does not need to be updated under the current vehicle state, if so, updating the unit index _ Block = index _ Block +1, or updating the operating line index _ Rank = 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 turning path, updating the next global path into the operation line.
7. 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:
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 end 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 the trajectory tracking controller, and judging in the step 2) again;
2.2 When a global path point closest to the vehicle position is close to the end of the global path, judging the type of the current path;
2.2.1 When the current path type is judged to be a steering path, a working line is generated to be used as a global path, and the global path type is switched to be the working line; inquiring a global path point closest to the current vehicle position;
2.2.2 When the current path type is judged to be the operation line, judging whether the current unit is the last operation line of the last unit;
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 line is the last line of the current unit; updating the repeating unit;
2.2.2.2.2 Judging that the current operation line is not the last operation line of the current unit, generating a steering path as a global path, and switching the type of the global path into the steering path.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110614366.8A CN113361764B (en) | 2021-06-02 | 2021-06-02 | Path planning method for automatic operation of agricultural machinery |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110614366.8A CN113361764B (en) | 2021-06-02 | 2021-06-02 | Path planning method for automatic operation of agricultural machinery |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113361764A CN113361764A (en) | 2021-09-07 |
| CN113361764B true CN113361764B (en) | 2022-12-06 |
Family
ID=77531313
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110614366.8A Active CN113361764B (en) | 2021-06-02 | 2021-06-02 | Path planning method for automatic operation of agricultural machinery |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113361764B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114089758B (en) * | 2021-11-18 | 2022-11-04 | 上海联适导航技术股份有限公司 | WebGIS-based unmanned rice transplanting operation path planning method |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5155679A (en) * | 1989-12-18 | 1992-10-13 | Hewlett-Packard Company | Set-up optimization for flexible manufacturing systems |
| DE19629618A1 (en) * | 1996-07-23 | 1998-01-29 | Claas Ohg | Route planning system for agricultural work vehicles |
| US6782306B2 (en) * | 1999-12-16 | 2004-08-24 | Siemens Energy & Automation | Motion control system and method utilizing spline interpolation |
| DE102004027242A1 (en) * | 2004-06-03 | 2005-12-22 | Claas Selbstfahrende Erntemaschinen Gmbh | Route planning system for agricultural machines |
| US9513125B2 (en) * | 2008-01-14 | 2016-12-06 | The Boeing Company | Computing route plans for routing around obstacles having spatial and temporal dimensions |
| CN104063746B (en) * | 2014-06-18 | 2017-03-15 | 浙江大学 | A kind of curved surface finishing cutter track generation method based on traveling salesman problem solver |
-
2021
- 2021-06-02 CN CN202110614366.8A patent/CN113361764B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113361764A (en) | 2021-09-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109215370B (en) | Path planning method for calculating an optimal parking maneuver adapted to a road vehicle and corresponding system | |
| CN110954122B (en) | Automatic driving track generation method under high-speed scene | |
| CN107179078A (en) | A kind of AGV paths planning methods optimized based on time window | |
| CN107664503A (en) | Vehicle path planning method and device | |
| CN111238521A (en) | Path planning method and system for unmanned vehicle | |
| CN111026133A (en) | Path planning method, vehicle and computer readable medium | |
| CN113361764B (en) | Path planning method for automatic operation of agricultural machinery | |
| CN105528955A (en) | Method and device for generating road network | |
| CN108205322A (en) | A kind of agricultural machinery working method and system that working path is not automatically generated by map | |
| CN108804801B (en) | Method for realizing R language of urban discrete traffic network design based on target distribution flow | |
| CN107608344A (en) | Vehicle motion control method, apparatus and relevant device based on trajectory planning | |
| CN108180921A (en) | Utilize the AR-HUD navigation system and its air navigation aid of GPS data | |
| CN107289938A (en) | A kind of unmanned ground vehicle local paths planning method | |
| CN110347154A (en) | High-precision map paths planning method based on analog control system | |
| CN115525047B (en) | Vehicle local track planning method and system with multi-type obstacle avoidance mode | |
| CN108647475B (en) | Urban discrete traffic network design R language implementation method based on load balancing | |
| CN116797002B (en) | Electric vehicle charging station planning method, device and storage medium | |
| CN115079702B (en) | Intelligent vehicle planning method and system under mixed road scene | |
| CN114399125B (en) | Motorcade optimal trajectory control method and device, electronic equipment and storage medium | |
| CN115713856A (en) | Vehicle path planning method based on traffic flow prediction and actual road conditions | |
| CN115116220A (en) | Unmanned multi-vehicle cooperative control method for loading and unloading scene of mining area | |
| CN105910611A (en) | Road matching method based on matching degree feedback | |
| CN115071686A (en) | Parking method for unmanned mining vehicle in long and narrow area | |
| CN113593238A (en) | Intersection virtual lane modeling method for automatic driving navigation | |
| CN112923940A (en) | Path planning method, device, processing equipment, mobile equipment and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |