CN112985401A - Headline full path planning and tracking method - Google Patents
Headline full path planning and tracking method Download PDFInfo
- Publication number
- CN112985401A CN112985401A CN202110142952.7A CN202110142952A CN112985401A CN 112985401 A CN112985401 A CN 112985401A CN 202110142952 A CN202110142952 A CN 202110142952A CN 112985401 A CN112985401 A CN 112985401A
- Authority
- CN
- China
- Prior art keywords
- turning
- area
- path
- boundary
- planning
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 238000013519 translation Methods 0.000 claims description 10
- 238000005457 optimization Methods 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 6
- 235000014443 Pyrus communis Nutrition 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 5
- 238000010586 diagram Methods 0.000 description 7
- 241000220324 Pyrus Species 0.000 description 5
- 230000007246 mechanism Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 239000000446 fuel Substances 0.000 description 3
- 239000002699 waste material Substances 0.000 description 3
- 206010063385 Intellectualisation Diseases 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 235000013339 cereals Nutrition 0.000 description 2
- 239000000295 fuel oil Substances 0.000 description 2
- 241001124569 Lycaenidae Species 0.000 description 1
- 240000007594 Oryza sativa Species 0.000 description 1
- 235000007164 Oryza sativa Nutrition 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000012271 agricultural production Methods 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004720 fertilization Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 235000021017 pears Nutrition 0.000 description 1
- 239000000575 pesticide Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 235000009566 rice Nutrition 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
- 238000005507 spraying Methods 0.000 description 1
- 238000003971 tillage Methods 0.000 description 1
- 238000009966 trimming Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
Abstract
The invention discloses a method for planning and tracking a full route of a thread, which comprises the following steps: step (1): boundary acquisition and turning mode determination; step (2): determining the width of a turning area and determining the operation direction; and (3): the planning method provided by the invention can plan a straight line operation path, a turning path and an edge folding path, and can improve the effective operation area and the land utilization rate.
Description
Technical Field
The invention relates to a method for planning and tracking a full route of a headset, which is suitable for the field of global route planning and guidance control of agricultural vehicles such as tractors, rice transplanters, harvesters and the like.
Background
The grain problem is a big problem concerning the national civilians, and is imperative for ensuring the grain safety, precision agriculture and fine agriculture. With the development and wide application of high-precision navigation and positioning technology, the automatic navigation and driving technology of agricultural machinery becomes a hotspot for research of people. Traditional agriculture usually depends on sense and experience of drivers to cultivate, and operation overlapping and omission often occur, especially in large-area farmlands. And night work is often needed in busy seasons, and the agricultural machine is more difficult to control to complete agricultural operation strictly according to a preset route only by means of sensory and empirical judgment of a driver. In fine agriculture, agricultural machines are correspondingly controlled to carry out agricultural operations such as automatic variable rate fertilization and variable rate pesticide spraying at a specific advancing speed according to demands at different spatial positions, so that the requirements of the agricultural operations are difficult to guarantee only by the personal experience of a driver, and the agricultural production is influenced. The automatic navigation of the agricultural machine provides an effective means for accurate operation.
The automatic navigation is the basis for implementing accurate operation, and meanwhile, path planning is the core of an automatic navigation system, so that an operation reference path is required to be planned in advance to enable a navigation tractor to complete an operation task in a given land block, and the quality of the reference path directly influences the operation quality. The method for planning the paths of the navigation tractor aiming at various land conditions based on the GIS/GPS can promote the intelligent equipment of the agricultural machinery, and is an effective way for realizing the intellectualization of the agricultural machinery. In order to realize the national agricultural mechanization and modernization targets, frugal and fine mode operation is carried out, and meanwhile, the automation, the intellectualization and the like of agricultural machine navigation are realized.
The existing path tracking method mainly comprises a control method based on a model and a model irrelative, wherein the control method based on a kinematic model generally needs to carry out small-angle equal linearization processing and constant speed hypothesis on the model, not only linear errors are introduced, but also the robustness is poor when the speed changes; although the dynamic model has high precision, the computational complexity is large, the real-time performance is not high, and some model parameters are difficult to obtain in real time. The model-independent methods include methods such as PID (proportion integration differentiation), but the parameter determination completely depends on the experience of technicians, and the debugging difficulty is high and the adaptability is poor when the number of parameters is large, so that the method is not beneficial to farmers. A control method which ensures high path tracking precision, good operation effect, simplicity and easiness in use and strong adaptability is also a key technology for realizing fine agriculture.
Disclosure of Invention
Aiming at the problems in the prior art, the invention mainly provides a headset line full-path planning and tracking method, which improves the operation efficiency and the land utilization rate and reduces the manpower and fuel cost.
In order to achieve the purpose, the invention provides the following technical scheme: a method for planning and tracking a full path of a headset comprises the following steps:
step (1): boundary acquisition and turning mode determination;
step (2): determining the width of a turning area and determining the operation direction;
and (3): planning the path of the operation area and planning the edge closing path of the turning area at the head of the ground.
Preferably, the step (1) includes: the method comprises the steps of acquiring boundary information by using navigation positioning equipment or other sensors, processing data, inputting vehicle parameters and farm tool parameter information through upper computer software, and sending the boundary parameters to a path planning module, wherein the path planning module determines a turning mode according to turning radius and operation width information of a vehicle.
Preferably, the step (2) includes: and calculating the width d of the minimum turning area to be reserved according to the turning radius, the operation width and the turning mode of the vehicle.
Preferably, the step (2) includes: and (3) based on the original boundary, obtaining the inner boundary by utilizing a boundary scaling algorithm or a line translation algorithm, wherein the scaling distance or the translation distance is the minimum turning area width d.
Preferably, the optimal operation direction theta is solved by determining the operation direction and taking the minimum total number of times of turning of each boundary as an optimization target.
Preferably, the step (3): obtaining a series of linear paths in the operation area through a linear translation algorithm according to the information of the inner boundary, the optimal operation direction theta and the operation width W, and obtaining key path points in the operation area according to the intersection point of the parallel straight lines and the inner boundary to realize the linear path planning of the operation area; and respectively taking two intersection points of two adjacent parallel straight lines on the same boundary as a starting point and an end point of the turning path, and then determining a key characteristic point of the turning mode according to the geometric relationship to finish the planning of the turning path.
Preferably, the turning mode has 4 modes of semicircle, arc, pear shape and fishtail shape, and the specific details are as follows: when the turning radius is smaller than 1/2 operation breadth, determining the turning mode of the operation machine in the ground turning area as an arch turning mode; when the turning radius is equal to 1/2 operation breadth, determining that the turning mode of the operation machine in the ground turning area is a semicircular turning mode; when the turning radius is larger than 1/2 operation breadth, determining that the turning mode of the operation machine in the headland turning area is a pear-shaped turning mode; and when the turning radius is smaller than 1/2 operation breadth and the operation machine is in a reverse mode, determining that the turning mode of the operation machine in the ground turning area is a fishtail turning mode.
Preferably, in order to increase the effective working area, the path planning is also carried out in the ground turning area, and the path planning in the ground turning area can be obtained once by contracting 1/2 the working width in the outer boundary.
Preferably, in order to ensure that the operation machine in the turning area of the ground can realize smooth transition between straight navigation paths, two intersecting straight lines need to be connected through a common tangent circle, and the radius of the common tangent circle is the minimum turning radius of the operation machine and can meet the maximum curvature constraint; calculating the circle center of the common tangent circle and the tangent point of the two alternating straight lines and the common tangent circle according to the information of the two alternating straight lines and the radius, and finishing the full-coverage optimal path planning of the land parcel; and storing the generated full-coverage optimal planned path, displaying the boundary and all the planned paths on an upper computer, and displaying the position of the operation machine and the current operation leading line in real time in the actual operation process.
Preferably, the generated path track points, the real-time position coordinates (x, y) of the working machine, the heading, the speed information, the vehicle parameters and the like are sent to a path tracking module to realize automatic driving; and calculating the path tracking deviation according to the real-time position coordinates of the working machine and the expected path information, and transmitting the path tracking deviation to an upper computer for real-time display.
Compared with the prior art, the invention has the following beneficial effects:
1. the planning method provided by the invention can plan a straight line operation path, a turning path and a side folding path, and can improve the effective operation area and the land utilization rate;
2. the planning method provided by the invention takes the minimum turning number as an optimization target, reduces the turning number and the driving path, reduces fuel oil and loss, and saves cost;
3. the planning method provided by the invention can complete path planning only by inputting the boundary, the operation parameters, the vehicle parameters and the like, does not need to perform complex operation, and is simple and easy to implement;
4. the planning method provided by the invention does not relate to complex arithmetic solving operation, has high operation real-time performance, low resource consumption, low requirement on hardware performance and stronger universality;
5. the planning method provided by the invention is combined with a pure tracking path tracking method, so that full-automatic unmanned driving can be realized, the labor cost is reduced, the method is not limited by weather and time, continuous all-weather high-precision operation without interruption can be realized, and the operation efficiency is improved;
6. the planning method provided by the invention is particularly suitable for an accurate agricultural unmanned control system.
Drawings
FIG. 1 is a schematic diagram of a job path planning according to the present invention;
FIG. 2 is a schematic view of the bow turn mode of the present invention;
FIG. 3 is a schematic view of a semi-circular turning pattern in the present invention;
FIG. 4 is a schematic view of the pear turn pattern of the present invention;
FIG. 5 is a schematic view of a fishtail mode of turning in accordance with the present invention;
FIG. 6 is a schematic diagram of a pure tracking algorithm model according to the present invention;
FIG. 7 is a schematic view of a semicircular turn path plan in accordance with the present invention;
FIG. 8 is a schematic diagram of the pear-shaped turn path planning of the present invention;
FIG. 9 is a flow chart of path planning in the present invention;
FIG. 10 is a schematic diagram of a path tracking algorithm of the present invention;
fig. 11 is a block diagram of an automatic driving system according to the present invention.
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.
In the description of the present invention, it should be noted that the terms "upper", "lower", "inner", "outer", "front", "rear", "both ends", "one end", "the other end", and the like indicate orientations or positional relationships based on those shown in the drawings, and are only for convenience of description and simplicity of description, but do not indicate or imply that the referred device or element must have a specific orientation, be constructed in a specific orientation, and be operated, and thus, should not be construed as limiting the present invention. Furthermore, the terms "first" and "second" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.
In the description of the present invention, it is to be noted that, unless otherwise explicitly specified or limited, the terms "mounted," "disposed," "connected," and the like are to be construed broadly, such as "connected," which may be fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; they may be connected directly or indirectly through intervening media, or they may be interconnected between two elements. The specific meanings of the above terms in the present invention can be understood in specific cases to those skilled in the art.
Referring to fig. 1 to 11, a method for full-path planning and tracking of a headset includes:
the steps of boundary acquisition and turning mode determination specifically comprise: acquiring boundary vertex information by using navigation positioning equipment or other sensors, and processing data; setting information such as vehicle parameters, farm tool parameters and the like through upper computer software; determining a turning mode according to the turning radius of the vehicle and the operation width;
the specific steps of determining the width of the turning area and determining the working direction include: determining the minimum ground distance to be reserved according to the determined turning mode, the turning radius of the vehicle, the operation width, the length of the carried farm tools and the like, and accordingly dividing an operation area and a ground turning area; translating the original boundary inwards by the width d of the minimum turning area to obtain an inner boundary, wherein the part between the original boundary and the inner boundary is a ground head turning area, and the part within the inner boundary is an operation area;
the operation area path planning and the head turning area edge closing path planning specifically comprise the following steps: determining constraint conditions and establishing an optimization model related to operation direction angles by determining optimization targets such as the shortest total driving path, the largest effective operation area ratio, the lowest fuel cost, the smallest number of turns at each boundary and the like, and obtaining the optimal operation direction theta by solving, wherein the angle is generally the direction angle corresponding to the longest side in the boundary; according to the inner boundary, the optimal operation direction theta, the operation width W and the like, the linear path planning of the operation area can be realized through a linear translation algorithm; determining key characteristic points of a turning mode according to the intersection points of the inner boundary and each straight line operation path obtained by calculation and the geometric relationship, and finishing the planning of the turning path; the straight path planning of the ground turning area can be obtained by internally contracting 1/2 operation breadth at the outer boundary, when the minimum turning area breadth is more than one time of the operation breadth, the operation breadth can be further internally contracted 1/2 on the basis until the ground turning area can be covered, and then the path common tangent circle of two alternating straight lines is obtained to realize the smooth connection of the tracks; storing the generated full-coverage optimal planning path information and displaying the information on an upper computer; taking the planned path as a target path, calculating an expected front wheel corner through a path tracking algorithm such as pure tracking according to information such as real-time position coordinates (x, y), course and speed of the operation machine, converting the control quantity into an execution mechanism control instruction and issuing the execution mechanism control instruction to a motor to realize driving path tracking;
according to the information of operation width, minimum turning radius, backing/advancing and the like, the invention mainly has 4 modes of semicircle, arc, pear shape and fishtail shape, and the specific steps are as follows:
(1) when the turning radius is smaller than 1/2 operation breadth, the turning mode of the operation machine in the ground turning area is an arch turning mode;
(2) when the turning radius is equal to 1/2 operation breadth, the turning mode of the operation machine in the ground turning area is a semicircular turning mode;
(3) when the turning radius is larger than 1/2 operation breadth, the turning mode of the operation machine in the turning area at the ground head is a pear-shaped turning mode;
(4) when the turning radius is smaller than the 1/2 operation width and the operation machine is in the reverse mode, the turning mode in the ground turning area is the fishtail turning mode.
In the invention, the semicircular turning mode specifically comprises the following steps: the circular arc is formed by 1/2 circular arcs with the radius of the minimum turning radius of the operation machinery, and key characteristic points of the circular arc comprise a starting point, an end point and a circle center, and can be obtained by a boundary and a planning straight line path;
in the invention, the bow-shaped turning mode is specifically as follows: the arc-shaped circular arc is composed of two arcs with the radius of the minimum turning radius of the operation machinery and a straight line, is similar to an arc shape, and has key characteristic points comprising a starting point, an end point and a circle center, and the intersection point of the straight line and the circle can be obtained through boundary, planning straight line path, geometric relationship and the like;
in the invention, the pear-shaped turning mode is specifically as follows: the three segments of 1/2 circular arcs with the smallest turning radius are shaped like pears, and key characteristic points of the 1/2 circular arcs comprise a starting point, an end point and a circle center, and the intersection points of the circles and the circles can be obtained through boundaries, planning straight paths, geometric relations and the like;
in the invention, the turning mode of the fishtail is as follows: the mode is mainly used for reversing and turning, and key characteristic points of the mode comprise a starting point, an end point and a circle center, and intersection points of straight lines and circles can be obtained through boundaries, planning straight line paths, geometric relations and the like;
in the invention, the minimum turning area width can ensure that the operation machine completes turning in the original boundary, the turning area can be reduced to the maximum extent, the effective operation area loss is ensured to be small as much as possible, and different turning modes have different minimum turning area widths;
in the present invention, the best working direction is specifically as follows: through analysis, a model with the minimum total number of turns, the shortest total path, the shortest driving time, the lowest fuel cost and the like as optimization targets is established, and the optimal operation direction can be obtained;
in the invention, the route planning of the turning area of the ground head specifically comprises the following steps: the route planning of the turning area at the head of the land is different from the route planning of the turning, the route planning of the turning is to plan a turning route, and the route planning of the turning area is to plan a straight line operation route, so that the land utilization rate is improved.
Referring to fig. 1 to 11, a method for planning and tracking a full route of a thread according to the present invention includes the following steps:
step (1): the boundary is collected and the turning mode is determined,
the method specifically comprises the following steps:
firstly, a global path is planned, and the traveling path track of the working machine has the characteristics that: except for the turning adjustment in the ground area, the operation is carried out back and forth along parallel straight lines in the operation area. Therefore, for any given polygonal plot, if the working direction, working width and turning mode are determined, the turning situation of the head of the plot on each side and the width of the turning area of the head of the plot can be determined according to each boundary information, so that the working area of the plot can be divided. Therefore, a general farm field can be divided into a working area and a turning area at the head of the field, as shown in fig. 1. The corresponding path planning can be divided into straight path planning of an operation area and path planning of a ground turning area.
Determining a turning mode: and acquiring plot boundary information and setting operation parameters including an operation width, a minimum turning radius of the operation machine, a reversing/advancing mode and the like through a sensor, and planning a turning path according to the operation width and the minimum turning radius. According to the size relationship between the turning radius and the operation width, 4 modes of semicircle, arc, pear shape and fishtail shape are mainly provided, and the following modes are specifically provided:
(1) when the turning radius is smaller than 1/2 operation breadth, determining the turning mode of the operation machine in the ground turning area as an arch turning mode; such as the arcuate turn pattern illustrated in fig. 2.
(2) When the turning radius is equal to 1/2 operation breadth, determining that the turning mode of the operation machine in the ground turning area is a semicircular turning mode; a semi-circular turning pattern as illustrated in figure 3.
(3) When the turning radius is larger than 1/2 operation breadth, determining that the turning mode of the operation machine in the headland turning area is a pear-shaped turning mode; the pear-shaped turning pattern is schematically shown in fig. 4.
(4) And when the turning radius is smaller than 1/2 operation breadth and the operation machine is in a reverse mode, determining that the turning mode of the operation machine in the ground turning area is a fishtail turning mode. Fig. 5 shows a fishtail-shaped turning pattern.
Step (2): determining the width of a turning area and determining the operation direction;
the method specifically comprises the following steps:
2-5, R is the minimum turning radius of the work machine, W is the work width, and P1, P2, P3, P4 are key feature points of the turning path.
Determining the minimum ground turning area width: in order to turn the work tractor completely, sufficient turning areas must be divided at the boundaries of the land area for turning the work machine around. If the reserved turning area is too large, the effective operation area is reduced, and waste of land resources is caused. Therefore, to ensure smooth turning and not waste resources, a minimum headland turning area must be determined. The width of the turning area is determined by the minimum turning radius (i.e. the selected turning style), the working width, the working direction and the length of the carried implement. For different cornering styles there is an extreme point of cornering (the point in the curve that is furthest from the boundary of the working area) from which the minimum width of the headland turning area can be determined. The minimum width d of the ground turning area is different for different turning forms.
Determining the optimal operation direction: for a given convex polygonal field area, the straight path (work area) consumption is almost constant, and the total consumption is mainly determined by the turn path (ground area) consumption. The work machine is required to undergo deceleration, steering and re-acceleration during steering in the ground area, and the cost consumption is much higher than for the straight portion. The main factors that affect the cost consumption in the ground turning area are the turning mode and the number of turns, and in a particular turning mode, the increase in the number of turns also inevitably increases the steering cost consumption, and therefore, in order to improve the work efficiency of the working machine and reduce the work cost consumption, it is the only preferable method to reduce the number of turns. By analyzing the number of turns on each boundary of the convex polygonal plot, the operation direction angle theta is enabled to search for an optimal theta value within the range of [0,180 DEG, the total number of turns N in the whole operation process can be ensured to be minimum, and the operation direction corresponding to the theta angle is the optimal operation direction of the convex polygonal plot. The longest boundary direction is generally the optimal working direction.
And (3): planning a path of an operation area and planning a margin path of a head turning area;
the method specifically comprises the following steps:
planning the path of the operation area: the operation machine generally operates along a straight line in an operation area, after the width d of the minimum ground head turning area is determined, for a regular polygonal plot, according to the boundary obtained by the sensor, the boundary is retracted by the distance d to obtain a new boundary, for distinguishing, the new boundary is called an inner boundary, and the original boundary obtained by the sensor is used as the outer boundary. According to the optimal operation direction theta and the operation width W, a series of parallel straight lines can be generated, and the intersection point of the parallel straight lines and the inner boundary is the required operation area track point. And determining key characteristic points of the turning mode according to the intersection point of two adjacent parallel straight lines on the same boundary, thereby completing the straight line path planning in the operation area and the turning path planning in the turning area.
Planning the edge closing path in the ground turning area:
the turning area of the ground does not carry out tillage in the general operation process, and the area is mainly used for switching and transition among different leading lines. However, if the turning radius of the operation machine is too large and the farm implement is wide, the reserved turning area of the ground is too large, which results in low utilization rate of the ground, and therefore, in order to avoid waste of the ground and achieve the purpose of planning the full-coverage path, the operation path planning needs to be performed on the turning area of the ground.
The straight line work path within the turn zone is obtained by translating 1/2 the work width inward from the outer boundary. And the boundary junction is too large and discontinuous, the steering limit of the actual operation vehicle can not meet the requirement, the processing is needed, the smooth connection of the operation path can be realized by searching a common tangent circle of two intersecting boundary straight lines, and the turning radius of the common tangent circle is the minimum turning radius of the operation machine. It should be noted that the working machine should start working from the second leading line in the working area during actual working, so as to avoid the repeated working caused by the repeated operation of the straight line path in the turning area of the ground.
As shown in fig. 7 and 8, a semicircular turning path planning diagram and a pear-shaped turning path planning diagram are respectively shown, in which B is a plot boundary, C is a straight line operation path planned in an operation area, and D is an operation path in a turning area, which may also be called a trimming path.
In the present invention, the path tracking method specifically includes: the application of pure tracking algorithm in the intelligent vehicle path tracking control has been studied in a great deal, the algorithm takes the rear axle of the vehicle as a tangent point, the longitudinal vehicle body of the vehicle as a tangent line, and the vehicle can run along an arc passing through a target road point (goal point) by controlling the corner of the front wheel, as shown in fig. 6,
the desired front wheel steering angle δ can be obtained from equation 1:
wherein L is the front wheel base, LdThe distance from the current position (namely the rear shaft position) of the vehicle to the target road point, namely the forward looking distance is shown, and alpha represents the included angle between the current vehicle body posture and the target road point.
After each instruction cycle is executed, the coordinates of the rear axle and the foresight distance are updated, real-time steering angle control quantity of the front wheels can be obtained, converted into instructions and sent to a steering system, and the steering system completes steering action, so that path tracking is realized. The planning method provided by the invention is combined with a pure tracking path tracking method, so that full-automatic unmanned driving can be realized, the labor cost is reduced, the method is not limited by weather and time, continuous all-weather high-precision operation can be realized without interruption, and the operation efficiency is improved.
The planning method provided by the invention can plan a straight line operation path, a turning path and a side folding path, and can improve the effective operation area and the land utilization rate; the minimum turning number is taken as an optimization target, so that the turning number and the driving path are reduced, the fuel oil and the loss are reduced, and the cost is saved; the path planning can be completed only by inputting boundaries, operation parameters, vehicle parameters and the like, and complex operation is not needed, so that the method is simple and easy to implement; the method does not involve complex arithmetic solving operation, and has the advantages of high operation real-time performance, low resource consumption, low requirement on hardware performance and strong universality.
The following provides a specific embodiment of the present invention
Referring to fig. 1 to 11, a method for full-path planning and tracking of a headset includes:
acquiring boundary information by using navigation positioning equipment or other sensors, and processing data;
inputting information such as vehicle parameters and farm tool parameters through upper computer software, and sending parameters such as boundaries to a path planning module;
the path planning module determines a turning mode according to the turning radius and the operation width information of the vehicle;
calculating the width d of the minimum turning area to be reserved according to the turning radius, the operation width and the turning mode of the vehicle;
based on the original boundary, obtaining an inner boundary by utilizing a boundary scaling algorithm or a line translation algorithm, wherein the scaling distance or the translation distance is the minimum turning area width d;
solving the optimal operation direction theta by taking the minimum total times of turning of each boundary as an optimization target;
a series of linear paths in the operation area can be obtained through a linear translation algorithm according to the information of the inner boundary, the optimal operation direction theta, the operation width W and the like, and key path points in the operation area can be obtained according to the intersection point of the parallel straight lines and the inner boundary, so that the linear path planning of the operation area is realized;
respectively taking two intersection points of two adjacent parallel straight lines on the same boundary as a starting point and an end point of the turning path, and then determining key characteristic points of a turning mode according to a geometric relationship to finish the planning of the turning path;
in order to increase the effective operation area, path planning is also carried out in the ground turning area, and one-time path planning in the ground turning area can be obtained by contracting 1/2 operation breadth inwards at the outer boundary;
in order to ensure that the operation machinery in the turning area at the ground can realize smooth transition between straight navigation paths, two alternating straight lines need to be connected through a common tangent circle, and the radius of the common tangent circle is the minimum turning radius of the operation machinery and can meet the maximum curvature constraint;
in order to ensure that the operation machinery in the turning area of the ground can realize smooth transition between straight navigation paths, two alternating straight lines need to be connected through a common tangent circle, the radius of the common tangent circle is the minimum turning radius of the operation machinery and can meet the maximum curvature constraint, and the circle center of the common tangent circle, the tangent point of the two alternating straight lines and the common tangent circle can be calculated according to the information and the radius of the two alternating straight lines, so that the full-coverage optimal path planning of the land parcel is completed;
storing the generated full-coverage optimal planned path, displaying the boundary and all the planned paths on an upper computer, and displaying the position of the operation machine and the current operation navigation line in real time in the actual operation process;
the generated path track points, information such as real-time position coordinates (x, y), course, speed and the like of the operation machine, vehicle parameters and the like are sent to a path tracking module to realize automatic driving;
calculating a path tracking deviation according to the real-time position coordinates of the working machine and the expected path information, and sending the path tracking deviation to an upper computer for real-time display;
calculating an expected front wheel angle by using a pure tracking algorithm calculation formula 1, converting the control quantity into an actuating mechanism control instruction and issuing the actuating mechanism control instruction to a motor, and realizing the tracking of a planned path;
when the operation machine runs to the ground, prompt information is given, a series of operations such as automatic deceleration, automatic lifting of farm tools, automatic turning and the like are carried out, and after normal operation is finished, edge folding is reminded, and operation in a ground turning area is finished.
It will be evident to those skilled in the art that the invention is not limited to the details of the foregoing illustrative embodiments, and that the present invention may be embodied in other specific forms without departing from the spirit or essential attributes thereof. The present embodiments are therefore to be considered in all respects as illustrative and not restrictive, the scope of the invention being indicated by the appended claims rather than by the foregoing description, and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim concerned.
Claims (10)
1. A method for full route planning and tracking of a thread is characterized in that: the method comprises the following steps:
step (1): boundary acquisition and turning mode determination;
step (2): determining the width of a turning area and determining the operation direction;
and (3): planning the path of the operation area and planning the edge closing path of the turning area at the head of the ground.
2. The method for full headset line path planning and tracking according to claim 1, wherein the step (1) comprises: the method comprises the steps of acquiring boundary information by using navigation positioning equipment or other sensors, processing data, inputting vehicle parameters and farm tool parameter information through upper computer software, and sending the boundary parameters to a path planning module, wherein the path planning module determines a turning mode according to turning radius and operation width information of a vehicle.
3. The method for full headset line path planning and tracking according to claim 2, wherein the step (2) comprises: and calculating the width d of the minimum turning area to be reserved according to the turning radius, the operation width and the turning mode of the vehicle.
4. The method for full headset line path planning and tracking according to claim 2, wherein the step (2) comprises: and (3) based on the original boundary, obtaining the inner boundary by utilizing a boundary scaling algorithm or a line translation algorithm, wherein the scaling distance or the translation distance is the minimum turning area width d.
5. The method for full headset line path planning and tracking according to claim 3 or 4, wherein the optimal working direction θ is solved by determining the working direction with the minimum total number of turns of each boundary as an optimization target.
6. The method for full headset line path planning and tracking according to claim 5, wherein the step (3): obtaining a series of linear paths in the operation area through a linear translation algorithm according to the information of the inner boundary, the optimal operation direction theta and the operation width W, and obtaining key path points in the operation area according to the intersection point of the parallel straight lines and the inner boundary to realize the linear path planning of the operation area; and respectively taking two intersection points of two adjacent parallel straight lines on the same boundary as a starting point and an end point of the turning path, and then determining a key characteristic point of the turning mode according to the geometric relationship to finish the planning of the turning path.
7. The method for full headset line path planning and tracking according to claim 6, wherein the turning mode includes 4 modes of semicircle, bow, pear and fishtail, and the following are specific: when the turning radius is smaller than 1/2 operation breadth, determining the turning mode of the operation machine in the ground turning area as an arch turning mode; when the turning radius is equal to 1/2 operation breadth, determining that the turning mode of the operation machine in the ground turning area is a semicircular turning mode; when the turning radius is larger than 1/2 operation breadth, determining that the turning mode of the operation machine in the headland turning area is a pear-shaped turning mode; and when the turning radius is smaller than 1/2 operation breadth and the operation machine is in a reverse mode, determining that the turning mode of the operation machine in the ground turning area is a fishtail turning mode.
8. The method of claim 7, wherein to increase the effective working area, the route is planned in the turning area of the ground, and the route is planned once in the turning area of the ground by contracting 1/2 the outer boundary.
9. The method as claimed in claim 8, wherein in order to ensure the operation machine in the turning area of the ground can realize smooth transition between the straight navigation paths, the two intersecting straight lines need to be connected by a common tangent circle, the radius of the common tangent circle is the minimum turning radius of the operation machine to meet the maximum curvature constraint, and the center of the common tangent circle, the tangent point of the two intersecting straight lines and the common tangent circle can be calculated according to the information and the radius of the two intersecting straight lines, so as to complete the optimal path planning of the ground block with full coverage; and storing the generated full-coverage optimal planned path, displaying the boundary and all the planned paths on an upper computer, and displaying the position of the operation machine and the current operation leading line in real time in the actual operation process.
10. The method for full headwine path planning and tracking according to claim 8, wherein the tracking method uses the rear axle as a tangent point and the longitudinal body of the vehicle as a tangent line, and controls the front wheel turning angle to enable the vehicle to travel along an arc passing through a target road point (goalpoint), and the desired front wheel turning angle δ is obtained by formula 1:
wherein L is the front wheel base, LdThe distance between the current position of the vehicle and the target waypoint, namely the forward looking distance, is represented, and alpha represents the included angle between the current vehicle body posture and the target waypoint.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110142952.7A CN112985401A (en) | 2021-02-02 | 2021-02-02 | Headline full path planning and tracking method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110142952.7A CN112985401A (en) | 2021-02-02 | 2021-02-02 | Headline full path planning and tracking method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112985401A true CN112985401A (en) | 2021-06-18 |
Family
ID=76346113
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110142952.7A Pending CN112985401A (en) | 2021-02-02 | 2021-02-02 | Headline full path planning and tracking method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112985401A (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113587930A (en) * | 2021-10-08 | 2021-11-02 | 广东省科学院智能制造研究所 | Indoor and outdoor navigation method and device of autonomous mobile robot based on multi-sensor fusion |
| CN115200589A (en) * | 2022-09-16 | 2022-10-18 | 北京博创联动科技有限公司 | Agricultural machinery edge folding track planning method and device, computer terminal and storage medium |
| CN116481547A (en) * | 2022-09-09 | 2023-07-25 | 丰疆智能(深圳)有限公司 | Route planning method, electronic device and storage medium |
| WO2023207022A1 (en) * | 2022-04-24 | 2023-11-02 | 丰疆智能软件科技(南京)有限公司 | Path planning method and system for automatic operation of agricultural machinery, and device and storage medium |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080249692A1 (en) * | 2007-04-03 | 2008-10-09 | Dix Peter J | Method for creating end of row turns for agricultural vehicles |
| CN106909150A (en) * | 2017-01-22 | 2017-06-30 | 无锡卡尔曼导航技术有限公司 | For the unpiloted avoidance of agricultural machinery, turn around path planning and its control method |
| CN107368094A (en) * | 2017-08-25 | 2017-11-21 | 上海拓攻机器人有限公司 | A kind of unmanned plane plant protection operation flight course planning method and device |
| WO2018116772A1 (en) * | 2016-12-19 | 2018-06-28 | 株式会社クボタ | Work vehicle automatic traveling system |
| CN108415435A (en) * | 2018-04-04 | 2018-08-17 | 上海华测导航技术股份有限公司 | A kind of agricultural machinery circular curve automatic Pilot control method |
| CN111959505A (en) * | 2020-06-29 | 2020-11-20 | 北京百度网讯科技有限公司 | Vehicle cruise control method, device, electronic equipment and storage medium |
| CN112229415A (en) * | 2020-11-03 | 2021-01-15 | 上海华测导航技术股份有限公司 | Path planning method and device, electronic equipment and storage medium |
-
2021
- 2021-02-02 CN CN202110142952.7A patent/CN112985401A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080249692A1 (en) * | 2007-04-03 | 2008-10-09 | Dix Peter J | Method for creating end of row turns for agricultural vehicles |
| WO2018116772A1 (en) * | 2016-12-19 | 2018-06-28 | 株式会社クボタ | Work vehicle automatic traveling system |
| CN106909150A (en) * | 2017-01-22 | 2017-06-30 | 无锡卡尔曼导航技术有限公司 | For the unpiloted avoidance of agricultural machinery, turn around path planning and its control method |
| CN107368094A (en) * | 2017-08-25 | 2017-11-21 | 上海拓攻机器人有限公司 | A kind of unmanned plane plant protection operation flight course planning method and device |
| CN108415435A (en) * | 2018-04-04 | 2018-08-17 | 上海华测导航技术股份有限公司 | A kind of agricultural machinery circular curve automatic Pilot control method |
| CN111959505A (en) * | 2020-06-29 | 2020-11-20 | 北京百度网讯科技有限公司 | Vehicle cruise control method, device, electronic equipment and storage medium |
| CN112229415A (en) * | 2020-11-03 | 2021-01-15 | 上海华测导航技术股份有限公司 | Path planning method and device, electronic equipment and storage medium |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113587930A (en) * | 2021-10-08 | 2021-11-02 | 广东省科学院智能制造研究所 | Indoor and outdoor navigation method and device of autonomous mobile robot based on multi-sensor fusion |
| CN113587930B (en) * | 2021-10-08 | 2022-04-05 | 广东省科学院智能制造研究所 | Indoor and outdoor navigation method and device of autonomous mobile robot based on multi-sensor fusion |
| WO2023207022A1 (en) * | 2022-04-24 | 2023-11-02 | 丰疆智能软件科技(南京)有限公司 | Path planning method and system for automatic operation of agricultural machinery, and device and storage medium |
| CN116481547A (en) * | 2022-09-09 | 2023-07-25 | 丰疆智能(深圳)有限公司 | Route planning method, electronic device and storage medium |
| CN116481547B (en) * | 2022-09-09 | 2024-02-13 | 丰疆智能(深圳)有限公司 | Route planning method, electronic device and storage medium |
| WO2024051039A1 (en) * | 2022-09-09 | 2024-03-14 | 丰疆智能(深圳)有限公司 | Route planning method, electronic device and storage medium |
| CN115200589A (en) * | 2022-09-16 | 2022-10-18 | 北京博创联动科技有限公司 | Agricultural machinery edge folding track planning method and device, computer terminal and storage medium |
| CN115200589B (en) * | 2022-09-16 | 2022-11-29 | 北京博创联动科技有限公司 | Agricultural machinery edge folding track planning method and device, computer terminal and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112985401A (en) | Headline full path planning and tracking method | |
| CN111189444A (en) | Automatic driving agricultural machinery field operation path planning system and planning method | |
| US10203694B2 (en) | System and method for controlling a vehicle | |
| US10459447B2 (en) | System and method for generating partitioned swaths | |
| CN106643719A (en) | Route planning algorithm of intelligent lawn mower | |
| CN112015176B (en) | Unmanned tractor field operation path planning method and device | |
| US11044842B2 (en) | Path planning for area coverage | |
| CN111256700B (en) | Edge narrowing planning method for planning operation path of automatic driving agricultural machine | |
| US7742860B2 (en) | Route planning system and method for agricultural working machines | |
| Sabelhaus et al. | Using continuous-curvature paths to generate feasible headland turn manoeuvres | |
| CN108919792B (en) | Path planning control method for automatic navigation system | |
| CN109074081A (en) | Autonomous driving path generates system | |
| US20190033874A1 (en) | Apparatus and method for controlling a vehicle | |
| US7706948B2 (en) | Method for creating spiral swaths for irregular field boundaries | |
| US20180359905A1 (en) | System and method for determining swath connections | |
| CN109634276B (en) | Agricultural vehicle unmanned control method and system and agricultural vehicle | |
| US10197407B2 (en) | Method and robot system for autonomous control of a vehicle | |
| CN107831771A (en) | A kind of dilatory machine automatic drive system, control method and caterpillar tractor | |
| CN110837252B (en) | Full-path planning and breakpoint endurance method for unmanned rice transplanter | |
| JP2018073050A (en) | Running route creating device | |
| CA2593486A1 (en) | Path planner and method for planning a contour path of a vehicle | |
| CN107817794A (en) | Automatic navigation method, control device and navigation system | |
| CN105182976A (en) | Visual navigation strategy of agricultural robot | |
| CN106909150A (en) | For the unpiloted avoidance of agricultural machinery, turn around path planning and its control method | |
| CN106647770A (en) | Field turning path planning and control method used for farm machinery driverless driving |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210618 |