Cotton field agricultural machine path planning method based on prior navigation information

Abstract

The invention relates to the technical field of path planning, in particular to a cotton field agricultural machine path planning method based on prior navigation information, which comprises the following steps: acquiring sowing navigation data of a cotton field, marking breakpoint coordinates of missing parts in the sowing navigation data, and estimating an obstacle range according to the breakpoint coordinates; removing an artificial auxiliary driving part in the seeding navigation data, marking transverse operation and longitudinal operation endpoint coordinates, and performing coordinate translation according to the planting agronomic data; generating a U-turn path, a transverse and longitudinal operation connection path and an obstacle avoidance path; and integrating the U-turn path, the transverse and longitudinal operation connection path and the obstacle avoidance path into the seeding navigation data without the artificial auxiliary driving part to generate a full-coverage operation path, marking the specified coordinate point of the full-coverage operation path as a function point, and embedding semantics. The method can achieve the precise row control of the land parcels covered by the crops, obtains a feasible target path, improves the precision of path planning, and reduces the planning cost.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and more particularly to a cotton field agricultural machinery path planning method based on prior navigation information. Background Art

[0002] Path planning is the prerequisite for agricultural machinery to perform autonomous driving operations. At present, for path planning of large-scale agricultural machinery, the mainstream satellite navigation assisted driving system mainly adopts the AB line method, which is simple to operate, supplemented by manual U-turns, obstacle avoidance and process supervision, and can meet most basic operations. It is also the mainstream path planning method for cotton sowing in Xinjiang. In order to achieve fully automatic driving operations, researchers have conducted many studies on U-turn methods (U-shaped, bow-shaped, Ω-shaped and fishtail-shaped, etc.), route planning (opening ridges, closing ridges, shuttles, loops and detours, etc.), obstacle handling, etc., and on this basis, they have carried out research on full coverage path planning based on intelligent algorithms.

[0003] However, current research is more suitable for operating scenarios without crop cover (such as tillage and sowing) or without row alignment requirements (such as lawn mowing). For operating scenarios with existing crop cover and requiring precise row alignment, such as cotton harvesting, straw and residual film recycling, there is little research on how to better utilize the navigation data during sowing for subsequent more practical path planning.

[0004] In recent years, precision seeders equipped with Beidou satellite navigation have been widely used, accumulating a large amount of rich path data. Compared with idealized path planning, using navigation data during seeding operations as the target path and benchmark for cotton harvesting and residual film recovery is more in line with actual operations, more conducive to improving the efficiency and accuracy of path planning, reducing costs, and more importantly, improving the accuracy of corresponding operations.

[0005] According to the operation characteristics of cotton mulching seeding (mulching, laying drip irrigation tape, sowing, and covering soil in one go), precision seeders equipped with Beidou satellite navigation can realize automatic driving when working in a straight line, but manual driving is required for turning and avoiding obstacles. Therefore, the navigation path data during seeding has the following characteristics: (1) The U-turn path cannot be used directly. Manual U-turns do not consider the recycling of subsequent path data. The path is not standardized enough, there are redundant and messy phenomena, and it may contain multiple back-and-forth and repeated paths. It is not suitable for automatic driving operations, and the tolerance for vehicle turning radius is poor. (2) The operation route is not standardized and has a great influence on human factors. Due to manual U-turns, the operation cost is less considered and is more arbitrary. In addition, due to the different turning radii, the original operation sequence may not be applicable to other agricultural machinery. (3) The obstacle avoidance path cannot be used directly. When bypassing obstacles, the agricultural machinery is usually manually taken over. The bypass path has strong personal habits and is not suitable for direct automatic driving operations. (4) There is a lack of reasonable connection between the horizontal and vertical operation areas, and it is usually completed by manual driving. The data stored at the same time are generally in the geodetic coordinate system and need to be converted into a plane rectangular coordinate system to facilitate subsequent path tracking control.

[0006] Therefore, how to intelligently plan the operation path of agricultural machinery based on cotton field sowing navigation data is a technical problem that technical personnel in this field urgently need to solve. Summary of the invention

[0007] In view of this, the present invention provides a cotton field agricultural machinery path planning method based on prior navigation information, which can realize accurate alignment of plots covered with crops, obtain a feasible target path, improve the accuracy of path planning, and reduce planning costs.

[0008] In order to achieve the above object, the present invention adopts the following technical solution:

[0009] A cotton field agricultural machinery path planning method based on prior navigation information comprises the following steps:

[0010] Obtain the sowing navigation data of the cotton field, mark the breakpoint coordinates of the missing parts in the sowing navigation data, and estimate the obstacle range based on the breakpoint coordinates;

[0011] Eliminate the manual assisted driving part in the sowing navigation data, mark the coordinates of the lateral and longitudinal operation endpoints, and perform coordinate translation according to the planting agronomic data;

[0012] Generate U-turn paths, horizontal and vertical operation connection paths, and obstacle avoidance paths;

[0013] The U-turn path, horizontal and vertical operation connection path and obstacle avoidance path are integrated into the seeding navigation data without the manual assisted driving part to generate a full coverage operation path. The designated coordinate points of the full coverage operation path are marked as functional points and embedded with semantics.

[0014] Furthermore, the generation process of the U-turn path includes:

[0015] For a complete plot of land, when working vertically, the whole working row is divided into several planning units, and in each planning unit, the row operation is carried out by using the bow-shaped U-turn method;

[0016] When (n total -1) / (n unit -1) cannot be divided evenly, the remaining operation rows are turned in a fishtail manner, taking into account the U-turn area constraints; where n total Indicates the total number of job rows, n unit -Indicates the number of rows traversed by a planning unit.

[0017] Furthermore, considering the turning radius and U-turn area constraints of agricultural machinery, the distance generated by the U-turn of agricultural machinery is used as the path cost function of the U-turn of agricultural machinery, which is expressed as:

[0018]

[0019] Among them, D total Indicates the total distance of the U-turn; n total Indicates the total number of job rows; n unit Indicates the number of rows traversed by a planning unit; W indicates the working width; R indicates the minimum turning radius of the agricultural machinery; L L Indicates the length of a single job row; round indicates the round-down function; even number indicates an even number.

[0020] Furthermore, combined with the standard equation of a circle, a piecewise function is used to represent the path of the bow-shaped U-turn. Under clockwise operation, the function of a single bow-shaped U-turn path is expressed as:

[0021]

[0022] For the odd-numbered planning unit, when there are an odd number of U-turns, the piecewise function in the above formula takes a positive sign; when there are an even number of U-turns, the piecewise function in the above formula takes a negative sign, and:

[0023]

[0024] For an even-numbered planning unit, when there are an odd number of U-turns, the piecewise function in the above formula takes a negative sign; when there are an even number of U-turns, the piecewise function in the above formula takes a positive sign, and:

[0025]

[0026] Where i = 1, 2, ..., n, n represents the number of planning units, and n = round ((n total -1) / (n unit-1);

[0027] j=1,2……n unit .

[0028] Furthermore, for the remaining operation rows and the transverse operation area of ​​the longitudinal operation, a fishtail U-turn path is adopted. Under the longitudinal operation, its function expression is:

[0029]

[0030] In the formula, the upper fishtail function is positive, the lower one is negative, and:

[0031] (x2,y2)=(x1+R,y1±R), (x3,y3)=(x1+wR,y1±R), (x4,y4)=(x1+w,y1)

[0032] Under lateral operation, the function expression of the fishtail turn path is:

[0033]

[0034] In the formula, (x2, y2) = (x1 ± R, y1 + R), (x3, y3) = (x1 ± R, y1 + WR), (x4, y4) = (x1, y1 + W), odd-numbered rows are positive, even-numbered rows are negative, i = 1, 2...n y , n y Indicates the number of rows that need to be fishtailed.

[0035] Furthermore, the process of generating the horizontal and vertical operation connection path includes:

[0036] The horizontal and vertical operations are connected by the methods of "retreat-cut" and "retreat-cut-retreat", where "retreat" means reversing, which is a straight line; "cut" means a quarter arc tangent to the reversing line with a radius of R. The function expression of the vertical and horizontal operation connection path is:

[0037]

[0038] Among them, (a, b) are the coordinates of the center of the circle, and θ is the center angle of the circle.

[0039] Furthermore, considering the minimum turning radius R of agricultural machinery, the obstacle avoidance path is generated by arcs and straight lines. The obstacle avoidance path depends on the obstacle avoidance width nW and the obstacle length L, which includes four methods:

[0040] ① When nW≤R and L≤2R, the obstacle avoidance path is arc-semicircle-arc;

[0041] ② When nW≤R and L≥2R, the obstacle avoidance path is arc-arc-straight line-arc-arc;

[0042] ③ When nW>R and L<2R, the obstacle avoidance path is arc-straight line-semicircle-straight line-arc;

[0043] ④ When nW>R and L>2R, the obstacle avoidance path is arc-straight line-arc-straight line-arc-straight line-arc;

[0044] The calculation formulas for obstacle avoidance path distances in four ways are expressed as follows:

[0045]

[0046] Furthermore, the cubic spline interpolation method is used to generate the obstacle avoidance path, including:

[0047] Given four points A, B, C, and D, with the constraints that the maximum curvature of the path is ≤ 1 / R and the tangents of the starting and ending points A and D are the same as the working line, the shortest working path is the objective function, and symmetric control points C1(x1, y) and C2(x2, y) are added to solve the cubic spline curve as the obstacle avoidance path; the solution formula of the cubic spline curve is:

[0048]

[0049] Among them, (x a ,y a )、(x1,y)、(x b ,y b )、(x c ,y c )、(x2,y)、(x d ,y d ) are the coordinates of points A, C1, B, C, C2, and D, S represents the cubic spline curve, and spline- represents the function for solving the spline curve.

[0050] Furthermore, the objective function of the optimal control point is:

[0051]

[0052] Among them, D S Indicates the distance of the obstacle avoidance path; x x represents the coordinates of the control points to be solved; k max Indicates the maximum curvature of the curve, k max =max[S″(x D )]; lb and ub represent the lower and upper limits of the control point;

[0053] The solution formula for obstacle avoidance path distance Ds is as follows:

[0054]

[0055] Among them, linspace represents the function of generating linear spacing vectors; x D Represents a start An evenly spaced vector of n to the maximum x-coordinate value, inclusive x points, used for discretization sampling on the spline interpolation curve; ppval means calculating the piecewise polynomial function; y D Indicates the use of spline interpolation curve S for vector x D Interpolate the points on the corresponding ordinate value; sum represents the sum function; diff represents the function of calculating the difference between two adjacent elements, expressed as:

[0056] diff(x D )=[x D (2)-x D (1),x D (3)-x D (2),...,x D (n x )-x D (n x -1)]

[0057] diff(y D )=[y D (2)-y D (1),y D (3)-y D (2),...,y D (n x )-y D (n x -1)]

[0058] Among them, hypot represents the Euclidean norm of the corresponding position of the input vector, that is, the square root of the sum of squares.

[0059] Furthermore, the agricultural machinery operation process includes four types of semantic points: operation semantic point, U-turn semantic point, stop semantic point and obstacle avoidance semantic point. When the real-time position coordinates of the agricultural machinery match the operation semantic point, the corresponding instructions are triggered and the corresponding actions are performed.

[0060] It can be seen from the above technical solutions that, compared with the prior art, the present invention has the following beneficial effects:

[0061] The present invention performs path planning based on the coordinate marking and coordinate translation of obstacle breakpoints and operation line endpoints of the sowing navigation data, combined with the vehicle turning radius and planting agronomic constraints, according to the generation methods of the paths such as U-turn, obstacle avoidance, and horizontal and vertical operation connection, so as to reduce the operation distance ratio under the premise of ensuring the operation coverage rate. It is feasible to perform path planning at the time of harvesting according to the prior sowing path, and it is conducive to obtaining a practical target path, improving the accuracy of path planning, and reducing planning costs. This method has certain reference value for path planning of other field crops such as corn and processing tomatoes.

[0062] The present invention takes into account the advantages of a small U-turn area and the fact that the vehicle does not need to reverse, and gives priority to the bow-shaped method; considering the cost of the operation distance, the U-turn strategy of the planned operation unit is given priority, and a path cost function is given to determine the number of operation rows n in the optimal unit. uint and the number of planning units n; consider (n total -1) / (n unit -1) In case of non-divisible situations and horizontal operations at the end of the field, the fishtail shape is used to supplement it, and the path functions of bow-shaped and fishtail-shaped U-turns are given; the "retreat-cut" and "cut-retreat" horizontal and vertical operation connection methods are proposed to achieve smooth connection of horizontal and vertical operations; the operation route can be determined based on the above path determination.

[0063] The present invention proposes arc-straight line and cubic spline obstacle avoidance paths, and considering the distance cost, the cubic spline method is preferred. Different from the ordinary cubic spline curve generation, the obstacle avoidance path is subject to the minimum turning radius constraint and the goal of the shortest distance. Therefore, the obstacle avoidance path generation is converted into solving an optimization problem with interpolation conditions and maximum curvature constraints, and the objective function is given. Considering the dual attributes of agricultural machinery walking and working, four semantic points of working, turning around, avoiding obstacles and stopping are embedded in the path, and the corresponding functional instructions are clarified to ensure the effective connection between agricultural machinery operation and the path. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0065] Figure 1 A flow chart of a cotton field agricultural machinery path planning method based on prior navigation information provided by the present invention;

[0066] Figure 2 An example of a navigation path for cotton film-covered sowing provided by the present invention;

[0067] Figure 3 A schematic diagram of wide and narrow row cotton planting provided by the present invention;

[0068] Figure 4 Schematic diagrams of four agricultural machinery U-turn paths provided by the present invention;

[0069] Figure 5 It is a schematic diagram of the RSR and LSR paths of the Dubins curve;

[0070] FIG6( a ) is a schematic diagram of the division of the entire land plot into planning units;

[0071] Figure 6(b) shows n uint Schematic diagram of the traversal effect when taking odd and even numbers;

[0072] Figure 7 This is a schematic diagram of the bow-shaped U-turn opposite route;

[0073] Figure 8(a) is a schematic diagram of the fishtail U-turn path in longitudinal operation;

[0074] Figure 8(b) is a schematic diagram of the fishtail turn path in lateral operation;

[0075] FIG9( a ) is a schematic diagram of connecting the vertical and horizontal paths in a “retreat-cut” manner;

[0076] FIG9( b ) is a schematic diagram of connecting the horizontal and vertical operation paths in a “retreat-cut-retreat” manner;

[0077] Figure 9(c) is a schematic diagram of the horizontal and vertical operation connection path;

[0078] Fig.10 Schematic diagram of obstacle avoidance path;

[0079] Fig.11 There are four ways to avoid obstacles in arc-straight line paths;

[0080] Fig.12 Schematic diagram of obstacle avoidance requirements;

[0081] Fig.13 Generate example graphs for obstacle avoidance paths;

[0082] Fig.14 It is a schematic diagram of embedded semantics;

[0083] Fig.15 To add a new path after U-turn, obstacle avoidance, and horizontal and vertical connection paths. DETAILED DESCRIPTION

[0084] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0085] like Figure 1 As shown, the embodiment of the present invention discloses a cotton field agricultural machinery path planning method based on prior navigation information, comprising the following steps:

[0086] Obtain the sowing navigation data of the cotton field, mark the breakpoint coordinates of the missing parts in the sowing navigation data, and estimate the obstacle range based on the breakpoint coordinates;

[0087] Eliminate the manual assisted driving part in the sowing navigation data, mark the coordinates of the lateral and longitudinal operation endpoints, and perform coordinate translation according to the planting agronomic data;

[0088] Generate U-turn paths, horizontal and vertical operation connection paths, and obstacle avoidance paths;

[0089] The U-turn path, horizontal and vertical operation connection path and obstacle avoidance path are integrated into the seeding navigation data without the manual assisted driving part to generate a full coverage operation path. The designated coordinate points of the full coverage operation path are marked as functional points and embedded with semantics.

[0090] The present invention performs path planning based on prior information such as sowing navigation information, planting agronomy, and vehicle parameters, combined with path generation algorithms such as U-turns and obstacle avoidance. First, the sowing navigation data is processed, mainly including filtering and eliminating the artificial assisted driving part according to the flag position, marking the horizontal and vertical operation endpoint coordinates; coordinate translation is performed according to the planting agronomy, and filled in the new column of the table for generating U-turns and connection paths; the breakpoint coordinates of the missing data are marked and filled in the new column of the table, so as to estimate the obstacle range for subsequent generation of obstacle avoidance paths; and the WGS84 coordinates are converted into UTM coordinates. Secondly, based on the above processed data, under the constraints of vehicle parameters and planting agronomy, combined with the generation methods of U-turns, obstacle avoidance, horizontal and vertical operation connection, the U-turn path and the horizontal and vertical operation connection path are first generated, and after the operation sequence is met, the obstacle avoidance path is generated to obtain a full coverage operation path; finally, the endpoint coordinates are marked as functional points, and semantics are embedded to facilitate the coordination of the actions between the machine operation and the vehicle walking, and the map is numbered and imported into the map library to facilitate the use of subsequent path tracking control.

[0091] Below, each step of the present invention is further described.

[0092] (1) Analysis of original seeding navigation data.

[0093] like Figure 2 As shown in the figure, the green path indicates the automatic driving operation, the blue path indicates the manual takeover of driving, and the missing data (as shown in a in the figure) is the obstacle part, which needs to be manually detoured. The regular blue line segment in the middle of the navigation line (such as b) is generally a short period of manual takeover of driving for the purpose of sorting agricultural materials. In subsequent operations, it can be equated with a green path without detouring. In order to facilitate vehicle U-turns and improve land utilization, in addition to vertical operations along the long side, cotton sowing will have horizontal operations perpendicular to the long side, commonly known as "pulling the horizontal head". Generally, horizontal sowing is carried out at the head of the field after the vertical operations are completed.

[0094] In Xinjiang, cotton film planting (standard film width is generally 205cm) mostly adopts wide and narrow row method, such as Figure 3 As shown in the figure, the sowing width is often larger than the width of the residual film recovery, so the operation guidance line needs to be translated. Figure 3 For example, the sowing width is 456cm, one machine has two films, and the residual film recovery width is 220cm, one machine has one film. Therefore, the sowing navigation line needs to be translated left and right by 1.14m to obtain the navigation line for the residual film recovery operation.

[0095] (2) U-turn path and operation routing.

[0096] 1) U-turn and opposite direction.

[0097] U-turn can be regarded as an optimization problem under multiple constraints such as vehicle turning radius, working width, starting and ending postures, U-turn area size, working direction and starting position. The optimization goal is to connect all working rows, make the U-turn width appropriate, and minimize the total U-turn distance, that is, to generate the optimal U-turn method with the lowest cost. The costs of agricultural machinery U-turn mainly include distance, time and energy consumption costs, all of which refer to the cost of a single U-turn. There is a positive correlation between distance, time and energy consumption, so the distance cost can be used to approximate the total cost.

[0098] Commonly used U-shaped, bow-shaped, fishtail-shaped and Ω-shaped turning methods for agricultural machinery are as follows: Figure 4 As shown. The relationship between the minimum turning radius R and the working width W determines the U-turn mode. When R≤W / 2, the U-shape can be selected. This mode has the lowest cost and the smoothest driving. When R>W / 2, the Ω-shape or the fishtail-shape can be selected. The two can replace each other. The energy consumption of the fishtail-shape is less than that of the Ω-shape, but the need to reverse has higher requirements on the turning performance of the agricultural machinery. The Ω-shape does not require reverse, but the required U-turn area is larger than the fishtail-shape, which is especially obvious when the turning radius is large. When R>W / 2, the U-shape, a deformed version of the bow shape, can be used at the same time. Turning and U-turning can be achieved by cross-row operation. There is no need to reverse and the U-turn width is small, but there are repeated paths. This is also a common U-turn method. Considering that both the cotton picker and the residual film recycling machine are large in size and difficult to turn, the present invention gives priority to the bow shape, and usually takes the long side as the starting direction.

[0099] The Dubins curve is composed of arcs and straight lines with maximum curvature. The shortest path planning from one posture to another can be realized through appropriate combination. It can effectively plan the path, reduce energy consumption and improve efficiency. It also gives a set of paths {LSL, RSR, RSL, LSR, RLR, LRL}, where L and R represent circular motions turning left and right respectively, and S represents linear motion. The core idea of ​​the Dubins curve is consistent with the needs of agricultural machinery turning around. Figure 5 Taking the path of A'B' line in the figure as an example, the bow U-turn is a special form of RSR path (AB line in the figure), that is, the tangent line at the end point coincides with the operation line, and the continuous U-turn line can be regarded as a combination of RSR and LSR. Therefore, the idea of ​​Dubins curve also verifies the superiority of bow operation. The present invention combines the bow U-turn path and uses the idea of ​​Dubins curve to perform U-turn path planning.

[0100] 2) U-turn strategy.

[0101] Different from the Dubins curve, in addition to the minimum turning radius, the U-turn of agricultural machinery is also constrained by the working width. Taking a complete plot as the object, there are two ways to use the bow U-turn to work in the same field: overall operation and divided operation unit.

[0102] The overall work is:

[0103] The center row of the plot is used as the first cross-row boundary, and the row operation is repeated, that is, there is only one turning unit. The path cost function of the U-turn row is as follows:

[0104]

[0105] Among them, D total Indicates the total distance of the U-turn; n total represents the total number of operating rows; W represents the operating width; R represents the minimum turning radius of the vehicle; L L Indicates the length of a single job row.

[0106] The planning units are divided as follows:

[0107] For a complete plot of land, when working longitudinally, the entire operation row is divided into several planning units. In each planning unit, a bow-shaped U-turn method is used to perform row-by-row operation, as shown in Figure 6(a).

[0108] As shown in Figure 6(b), unit Indicates the number of rows traversed by a planning unit; when n uintWhen an odd number is used, the center row may be repeatedly traversed or the unit end exit may be in the center row. Therefore, in order to reduce repeated paths and ensure that each planning unit is connected from beginning to end, n uint Usually an even number is taken, and D is calculated total The minimum n unit , which is the optimal number of planning unit rows. Considering the turning radius of agricultural machinery and the U-turn area constraints, the path cost function of the U-turn pair is as follows:

[0109]

[0110] In the formula, round represents the floor function; even number represents an even number.

[0111] When (n total -1) / (n unit -1) When it cannot be divided evenly, the U-turn area constraints are taken into consideration and the remaining operation rows are turned in a fishtail manner.

[0112] It can be seen from the above-mentioned U-turn function that the total distance traveled by the operation method of dividing the planning units is significantly smaller than that of the overall operation method, and there are fewer repeated paths. Therefore, the present invention gives priority to the division of planning units. It can be seen from Figures 6(a) and 6(b) that the operation route can be clarified while determining the operation U-turn path.

[0113] 3) Bow turn path function.

[0114] The schematic diagram of the bow turn route is as follows Figure 7 As shown in the figure, combined with the standard equation of a circle, a piecewise function is used to represent the path of the bow-shaped U-turn. Under clockwise operation, the function of a single bow-shaped U-turn path is expressed as:

[0115]

[0116] For the odd-numbered planning unit, when there are an odd number of U-turns, the piecewise function in the above formula takes a positive sign; when there are an even number of U-turns, the piecewise function in the above formula takes a negative sign, and:

[0117]

[0118] For an even-numbered planning unit, when there are an odd number of U-turns, the piecewise function in the above formula takes a negative sign; when there are an even number of U-turns, the piecewise function in the above formula takes a positive sign, and:

[0119]

[0120] Where i = 1, 2, ..., n, n represents the number of planning units, and n = round ((n total -1) / (n unit -1);

[0121] j=1,2……n unit .

[0122] By analogy, we can know the U-turn function when operating counterclockwise, which will not be elaborated.

[0123] 4) Fishtail U-turn path function

[0124] For the remaining operation rows and the lateral operation area of ​​the longitudinal operation, a fishtail U-turn path is adopted. Under the longitudinal operation, the schematic diagram of the fishtail U-turn path is shown in Figure 8(a), and its function expression is:

[0125]

[0126] In the formula, the upper fishtail function is positive, the lower one is negative, and:

[0127] (x2,y2)=(x1+R,y1±R), (x3,y3)=(x1+wR,y1±R), (x4,y4)=(x1+w,y1)

[0128] Under lateral operation, the fishtail U-turn path diagram is shown in Figure 8(b), and the function expression is:

[0129]

[0130] In the formula, (x2, y2) = (x1 ± R, y1 + R), (x3, y3) = (x1 ± R, y1 + WR), (x4, y4) = (x1, y1 + W), odd-numbered rows are positive, even-numbered rows are negative, i = 1, 2...n y , n y Indicates the number of rows that need to be fishtailed.

[0131] (3) Vertical and horizontal operation connection paths.

[0132] Contrary to the order of vertical operation followed by horizontal operation during sowing, horizontal operation must be performed first during harvesting to leave space for turning around during vertical operation. The proper connection between horizontal and vertical operations is more conducive to reducing operation costs and improving operation quality and efficiency. The horizontal operation rows are generally 2 rows, which become 4 rows after coordinate translation.

[0133] Considering the constraints of the vehicle turning radius and the U-turn area, the "retreat-cut" and "retreat-cut-retreat" methods are used to connect the horizontal and vertical operations. The "retreat-cut" method is shown in Figure 9(a) and the "retreat-cut-retreat" method is shown in Figure 9(b). After the lateral operation is completed, the vehicle returns to a mode that is conducive to vertical operation, especially the heading, which should be colinear and in the same direction as the navigation line. "Retreat" means reversing, which is a straight line; "cut" means a quarter-circle arc with a radius of R that is tangent to the reversing line. The function can refer to the above-mentioned arc-shaped U-turn function, or use the parametric equation of the circle, as shown in the following formula:

[0134]

[0135] Among them, (a, b) are the coordinates of the center of the circle, and θ is the center angle of the circle.

[0136] An example of horizontal and vertical connection is shown in Figure 9(c). The vertical operation length is shortened to highlight the connection method. The "retreat" before "cut" (shown by the blue dotted line in the figure) is a non-mandatory option, depending on the specific situation of the operation. If the minimum reversing is the priority, it is not selected. If the operation coverage is the priority option, it is selected.

[0137] (4) Obstacle avoidance path.

[0138] The obstacle avoidance path can be regarded as a local path planning problem based on the global path and map. There is no direct obstacle outline size in the seeding navigation data. When the navigation data in the operation row is discontinuous for a certain distance and has a detour trajectory (such as Figure 1 The obstacle is determined at a in the figure. The missing information of intermittent data is used to predict the shape of the obstacle contour. The fixed obstacles in Xinjiang cotton fields are mostly electric poles, irrigation plugs, etc., which are rare. When the obstacle is large, the obstacle is within the contour formed by the sequential connection of the breakpoint data; when the obstacle is small, a single operation line will be discontinuous.

[0139] 1) Obstacle avoidance strategy

[0140] The obstacle avoidance path is generated based on the U-turn path and the operation route determination.

[0141] ① When the obstacle is at the end of the working row or near the end, and there is not enough space for obstacle avoidance in the adjacent safe row, you can choose to turn around in advance to avoid the obstacle, such as Fig.10 At point IV in the middle, adopt the retreat-arch-retreat method, that is, retreat a distance of R from the end point of the obstacle, start to make a bow-shaped U-turn to the corresponding operation row, and then reverse to the starting point of the row. The bow shape is a required option, and the two "retreat" parts are optional, depending on the actual coverage requirements of the operation.

[0142] ② When the obstacle is not at the end of the operation line, the specific size and shape of the obstacle are unknown, but the path with navigation data is the default safe path. Therefore, when avoiding obstacles, it is preferred to bypass the nearest safe operation line, and then choose the area where the operation has been completed. The order can be adjusted according to the actual situation. In view of the unknown shape, size and location of the obstacle, in order to ensure safety, the starting point of obstacle avoidance is moved from the breakpoint of the navigation seeding data to both ends by the minimum turning radius R. The moving path can also be achieved by reversing to ensure that the operation line is not missed.

[0143] For example, when recycling residual film, a small number of vehicles passing through the non-operated area will have little effect on the recycling effect. In addition, since the sowing width is larger than the residual film recycling width, the missing data in a single sowing row will appear as missing data in the same segment of two adjacent rows after coordinate translation. The obstacle avoidance diagram is shown in the figure below. Fig.10 As shown in Ⅰ and Ⅲ; when cotton is harvested, it is not recommended for vehicles to pass through the unharvested area. At this time, the priority is to select the harvested area. The obstacle avoidance path diagram is shown in Fig.10 As shown in Ⅰ and Ⅱ.

[0144] 2) Obstacle avoidance path generation method.

[0145] When avoiding obstacles, the vehicle is limited by its minimum turning radius. Therefore, the maximum curvature constraint needs to be considered when fitting the curve, rather than directly fitting the curve based on known points. The following two methods for generating obstacle avoidance paths are proposed (when the obstacle is not at the tail end).

[0146] Generate obstacle avoidance paths by arcs and straight lines: Considering the minimum turning radius R of agricultural machinery, an obstacle avoidance path is generated by arcs and straight lines, such as Fig.11 As shown. The obstacle avoidance path depends on the obstacle avoidance width nW and the obstacle length L, and includes four methods:

[0147] ① When nW≤R and L≤2R, the obstacle avoidance path is arc-semicircle-arc;

[0148] ② When nW≤R and L≥2R, the obstacle avoidance path is arc-arc-straight line-arc-arc;

[0149] ③ When nW>R and L<2R, the obstacle avoidance path is arc-straight line-semicircle-straight line-arc;

[0150] ④ When nW>R and L>2R, the obstacle avoidance path is arc-straight line-arc-straight line-arc-straight line-arc;

[0151] The calculation formulas for obstacle avoidance path distances in four ways are expressed as follows:

[0152]

[0153] From the above, we can see that the arc-straight line path function is simple to obtain and easy to implement. The curvature is the same at the arc, but it is not optimal in terms of obstacle avoidance distance.

[0154] Use cubic spline interpolation to generate obstacle avoidance paths:

[0155] There are many ways to generate curves from multiple points, such as polynomial fitting, but low-order polynomial fitting has low precision, high-order fitting has high precision but is prone to the Runge phenomenon of endpoint oscillation, and does not necessarily make each discrete point a type value point. Therefore, spline interpolation can be used to obtain path curves, such as cubic spline curves, B-spline curves, Bezier curves, quintic spline curves, etc.

[0156] The cubic spline curve has good smoothness, avoiding corners and mutations; it has strong controllability, and the shape of the overall curve can be controlled by adjusting the position of the control points; it is simple to calculate, and the cubic polynomial solution is efficient. Therefore, this method can use the cubic spline interpolation method to generate an obstacle avoidance path.

[0157] Given four points A, B, C, and D, with the maximum curvature of the path ≤ 1 / R and the tangents of the starting and ending points A and D being the same as the working line as constraints, the shortest working path is used as the objective function, and symmetric control points C1(x1, y) and C2(x2, y) are added to solve the cubic spline curve as the obstacle avoidance path.

[0158] Specifically, if Fig.12 As shown in the figure, the obstacle avoidance problem can be simply described as solving a cubic spline curve with known points A, B, C, and D. However, unlike the ordinary cubic spline interpolation method, which directly fits the curve based on the known points, it is subject to the influence of the vehicle turning radius. It is necessary to add the constraint that the maximum curvature of the path is ≤ 1 / R and the tangents of the starting and ending points A and D are the same as the operation line. At the same time, the operation cost is considered, that is, the expected operation path is the shortest. Therefore, the obstacle avoidance path generation can be converted into solving an optimization problem with interpolation conditions and maximum curvature constraints, that is, solving a spline curve that passes through the four points A, B, C, and D and has the shortest distance, while meeting the curvature requirements.

[0159] If the obstacle avoidance path is solved based on only four known points without considering the curvature constraint, a curve with beautiful curves but no real use may be generated, such as Fig.13 As shown. According to the optimization and curve generation principles, to solve this problem, consider adding symmetrical control points C1(x1,y) and C2(x2,y), then the solution formula of the cubic spline curve is as follows:

[0160] The solution formula for the cubic spline curve is:

[0161]

[0162] Among them, (x a ,y a )、(x1,y)、(x b ,y b )、(x c ,y c )、(x2,y)、(x d ,y d ) are the coordinates of points A, C1, B, C, C2, and D, S represents the cubic spline curve, and spline- represents the function for solving the spline curve.

[0163] The control points are a set. To solve the optimal control point, the obstacle avoidance cost is considered, that is, it is converted to solving the path with the shortest obstacle avoidance distance under the maximum curvature constraint. The objective function of the optimal control point can be described as follows:

[0164]

[0165] Among them, D S Indicates the distance of the obstacle avoidance path; x x represents the coordinates of the control points to be solved; k max Indicates the maximum curvature of the curve, k max =max[S″(x D )]; lb and ub represent the lower and upper limits of the control point;

[0166] The solution formula for obstacle avoidance path distance Ds is as follows:

[0167]

[0168] Among them, linspace represents the function of generating linear spacing vectors; x D Represents a start An evenly spaced vector of n to the maximum x-coordinate value, inclusive x points, used for discretization sampling on the spline interpolation curve; ppval means calculating the piecewise polynomial function; y D Indicates the use of spline interpolation curve S for vector x D Interpolate the points on the corresponding ordinate value; sum represents the sum function; diff represents the function of calculating the difference between two adjacent elements, expressed as:

[0169] diff(x D )=[x D (2)-x D (1),x D (3)-x D (2),...,x D (n x )-x D (n x -1)]

[0170] diff(y D )=[y D (2)-y D (1),y D (3)-y D (2),...,y D (n x )-y D (n x -1)]

[0171] Among them, hypot represents the Euclidean norm of the corresponding position of the input vector, that is, the square root of the sum of squares.

[0172] Taking A(0,0), B(5,2.2), C(13,2.2), D(18,0) as an example, find the shortest obstacle avoidance path when the vehicle's minimum turning radius is 5m. Fig.13 As shown. The coordinates of the control point C1 are (2.8675, 0.8109), C2 is (15.1325, 0.8109), and the shortest curve length D S =19.5937, maximum curvature k max The minimum value of is 0.1972, the vehicle can turn, and the obstacle avoidance path distance is significantly smaller than the arc straight line. Under unconstrained conditions, the obstacle avoidance curve is solved only by four points A, B, C, and D. The shape is similar to the constrained solution result, but its maximum curvature is 0.32686, which cannot meet the vehicle's turning requirements.

[0173] It can be seen that both the arc-straight line method and the constrained cubic spline interpolation method can complete the obstacle avoidance operation, but in terms of the operation cost, the cubic spline method obviously has a lower operation cost, and the present invention gives priority to the cubic spline interpolation method.

[0174] (5)Job semantics.

[0175] Unlike smart driving cars, smart agricultural machinery not only needs to complete walking, but also needs to complete related operations, which involves related operations such as lifting and lowering of machinery and changing speed, that is, completing related operations according to instructions at designated path points. Therefore, semantics are embedded in the prior path so that the machinery can obtain clear and specific instructions at designated points, thereby better coordinating the relationship between walking and machinery operations, and realizing orderly control of vehicle autonomous driving operations.

[0176] like Fig.14 As shown in the figure, there are four semantic points in the operation process of agricultural machinery: operation semantic point, U-turn semantic point, stop semantic point and obstacle avoidance semantic point. The obstacle avoidance semantic point depends on whether there are obstacles in the operation scene. Semantic embedding is performed on the basis of the determination of the U-turn path, operation route and obstacle avoidance path, and flag bits are configured for them. During control, when the real-time position coordinates of the agricultural machinery match the operation semantic point, the corresponding instruction is triggered and the corresponding action is executed.

[0177] (6) Path generation software.

[0178] The path generation software is designed based on MATLAB APP (2021b). By importing the processed navigation seeding data or manually inputting data, a new path suitable for autonomous driving operation is generated according to the operation direction, turning radius and operation width. The newly generated path is saved as a document in CSV or TXT format and stored in the prior map library with a number. The relevant map can be called in the subsequent path tracking control to perform the operation.

[0179] Next, the performance of the method of the present invention is further verified.

[0180] The sowing navigation data of a certain seeder (one machine with two films) in a certain place in Bayingol Prefecture, Xinjiang, on April 16, 17 and 18, 2024 was used as the basic data. After the auxiliary driving data was eliminated, the function points were marked, the coordinates were translated, and the coordinates were converted. Considering the privacy of the operation plot, the vertical operation length was reduced for the convenience of display. After the cotton planted with film is harvested, the residual film needs to be recovered. A self-propelled residual film recovery machine is used as the machine object, the operation width is 2.2m, and the turning radius is set to 5m.

[0181] The operation effect is evaluated by operation coverage rate and operation distance ratio, which is different from the calculation method of operation coverage rate during sowing. R The actual total length of the operation L wt and total sowing length L st Related; Operation distance ratio D c And the work distance D w and total distance D t The formula is as follows: the greater the operation coverage rate and the higher the operation distance ratio, the better the operation effect.

[0182]

[0183] For the more regular plots, the plot of operation on April 18 was taken as the research object, recorded as plot A, and the original plot map was referenced Figure 2 , the new path after adding U-turn, obstacle avoidance, and horizontal and vertical connection paths is as follows Fig.15 shown.

[0184] Excluding the necessary reversing paths for fishtail turns and horizontal and vertical connections, as shown in Table 1, when non-essential reversing operations are not considered and obstacles are not considered, the operation coverage rate is 98.6%, the distance ratio is 87.93%, and the operation omissions mainly occur during horizontal turns, horizontal and vertical connections, and obstacle avoidance, which does not change with the change of the plot area, so the operation coverage rate will increase with the increase of the operation plot area. When obstacles are considered, the operation coverage rate and distance ratio will decrease accordingly depending on the number of obstacles, and each additional obstacle will increase the omission of 4R. Of course, if non-essential reversing is considered, full coverage of the operation line can be achieved during horizontal operations, horizontal and vertical connections, and obstacle avoidance paths, but the distance ratio will be reduced accordingly.

[0185] Table 1 Evaluation indicators of path planning for plot A

[0186]

[0187] The operation plots on April 16 and 17 were taken as the research objects. The same plot was sown in two days. The plots on April 16 and 17 were integrated and recorded as plots B1 (left) and B2 (right). For relatively irregular plots, they were divided into several sub-plots and path planning was carried out according to the above method. When the number of planning units after calculation was less than 2, the sub-plot was treated as a whole for operation, that is, the whole unit method was adopted to reduce the number of reversing. The path planning evaluation indicators of plots B1 and B2 are shown in Table 2. When unnecessary reversing is not considered and obstacles are not considered, the operation coverage rates are 98.21% and 98.46% respectively, and the distance ratios are 86.89% and 89.64% respectively; when obstacles are considered, the operation coverage rates will be reduced accordingly, and the operation coverage rates are 97.93% and 97.61% respectively, and the distance ratios are 85.13% and 88.52% respectively. Similar to plot A, if the reverse method is adopted, full coverage of the working row can be achieved, but the distance ratio will be reduced accordingly.

[0188] Table 2 Evaluation indexes of path planning for plots B1 and B2 %

[0189]

[0190]

[0191] Through the above experiments, it can be known that: under the constraint conditions, compared with the Ω-shaped U-turn method, the requirements for the U-turn width of the bow and fishtail are smaller, and they are more suitable for practical applications with a larger turning radius. The distance cost of the U-turn strategy of the planning operation unit is small, and the obstacle avoidance path of the cubic spline curve can effectively generate the obstacle avoidance path. The "retreat-cut" and "cut-retreat" methods can achieve the connection of horizontal and vertical operations. The feasibility of the above method was verified through field experiments. Operation omissions mainly occur in horizontal operations, horizontal and vertical connections, and obstacle avoidance paths. The test results show that without considering horizontal operations and obstacle avoidance, the longitudinal area can achieve full coverage of the operation line; when considering horizontal operations, full coverage of the operation line can be achieved when non-essential reversing operations are used. Even if reversing operations are not considered and obstacles are considered, the operation coverage rate can reach more than 97%, and this value is positively correlated with the operation area of ​​the plot. The larger the plot, the more obvious the advantage of the method of the present invention.

[0192] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0193] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A cotton field agricultural machinery path planning method based on prior navigation information, characterized in that: The following steps are involved: Obtain the sowing navigation data of the cotton field, mark the breakpoint coordinates of the missing parts in the sowing navigation data, and estimate the obstacle range based on the breakpoint coordinates; Eliminate the manual assisted driving part in the sowing navigation data, mark the coordinates of the lateral and longitudinal operation endpoints, and perform coordinate translation according to the planting agronomic data; Generate U-turn paths, horizontal and vertical operation connection paths, and obstacle avoidance paths; The U-turn path, horizontal and vertical operation connection path and obstacle avoidance path are integrated into the seeding navigation data without the manual assisted driving part to generate a full coverage operation path. The designated coordinate points of the full coverage operation path are marked as functional points and embedded with semantics.

2. The cotton field agricultural machinery path planning method based on prior navigation information according to claim 1 is characterized in that: The process of generating a U-turn path includes: For a complete plot of land, when working vertically, the whole working row is divided into several planning units, and in each planning unit, the row operation is carried out by using the bow-shaped U-turn method; When (n total -1) / (n unit -1) cannot be divided evenly, the remaining operation rows are turned in a fishtail manner, taking into account the U-turn area constraints; where n total Indicates the total number of job rows, n unit -Indicates the number of rows traversed by a planning unit.

3. The cotton field agricultural machinery path planning method based on prior navigation information according to claim 2 is characterized in that: Considering the turning radius and U-turn area constraints of agricultural machinery, the distance caused by the U-turn of agricultural machinery is used as the path cost function of the U-turn of agricultural machinery, which is expressed as: Among them, D total Indicates the total distance of the U-turn; n total Indicates the total number of job rows; n unit Indicates the number of rows traversed by a planning unit; W indicates the working width; R indicates the minimum turning radius of the agricultural machinery; L L Indicates the length of a single job row; round indicates the round-down function; even number indicates an even number.

4. The cotton field agricultural machinery path planning method based on prior navigation information according to claim 3 is characterized in that: Combined with the standard equation of a circle, a piecewise function is used to represent the path of a bow-shaped U-turn. Under clockwise operation, the function of a single bow-shaped U-turn path is expressed as: For the odd-numbered planning unit, when there are an odd number of U-turns, the piecewise function in the above formula takes a positive sign; when there are an even number of U-turns, the piecewise function in the above formula takes a negative sign, and: For an even-numbered planning unit, when there are an odd number of U-turns, the piecewise function in the above formula takes a negative sign; when there are an even number of U-turns, the piecewise function in the above formula takes a positive sign, and: Where i = 1, 2, ..., n, n represents the number of planning units, and n = round ((n total -1) / (n unit -1); j=1,2……n unit .

5. The cotton field agricultural machinery path planning method based on prior navigation information according to claim 3 is characterized in that: For the remaining operation rows and the horizontal operation area of ​​the longitudinal operation, a fishtail U-turn path is adopted. Under the longitudinal operation, its function expression is: In the formula, the upper fishtail function is positive, the lower one is negative, and: (x2,y2)=(x1+R,y1±R), (x3,y3)=(x1+wR,y1±R), (x4,y4)=(x1+w,y1) Under lateral operation, the function expression of the fishtail turn path is: In the formula, (x2, y2) = (x1 ± R, y1 + R), (x3, y3) = (x1 ± R, y1 + WR), (x4, y4) = (x1, y1 + W), odd-numbered rows are positive, even-numbered rows are negative, i = 1, 2...n y , n y Indicates the number of rows that need to be fishtailed.

6. The cotton field agricultural machinery path planning method based on prior navigation information according to claim 1 is characterized in that: The process of generating the horizontal and vertical operation connection path includes: The "retreat-cut" and "retreat-cut-retreat" methods are used to connect the horizontal and vertical operations, where "retreat" means reversing, which is a straight line; "cut" is a quarter arc tangent to the reversing line with a radius of R. The function expression of the vertical and horizontal operation connection path is: Among them, (a, b) are the coordinates of the center of the circle, and θ is the center angle of the circle.

7. The cotton field agricultural machinery path planning method based on prior navigation information according to claim 1 is characterized in that: Considering the minimum turning radius R of agricultural machinery, the obstacle avoidance path is generated by arcs and straight lines. The obstacle avoidance path depends on the obstacle avoidance width nW and the obstacle length L, which includes four methods: ① When nW≤R and L≤2R, the obstacle avoidance path is arc-semicircle-arc; ② When nW≤R and L≥2R, the obstacle avoidance path is arc-arc-straight line-arc-arc; ③ When nW>R and L<2R, the obstacle avoidance path is arc-straight line-semicircle-straight line-arc; ④ When nW>R and L>2R, the obstacle avoidance path is arc-straight line-arc-straight line-arc-straight line-arc; The calculation formulas for obstacle avoidance path distances in four ways are expressed as follows:

8. The cotton field agricultural machinery path planning method based on prior navigation information according to claim 1 is characterized in that: The obstacle avoidance path is generated using cubic spline interpolation, including: Given four points A, B, C, and D, with the constraints that the maximum curvature of the path is ≤ 1 / R and the tangents of the starting and ending points A and D are the same as the working line, the shortest working path is the objective function, and symmetric control points C1(x1, y) and C2(x2, y) are added to solve the cubic spline curve as the obstacle avoidance path; the solution formula of the cubic spline curve is: Among them, (x a ,y a )、(x1,y)、(x b ,y b )、(x c ,y c )、(x2,y)、(x d ,y d ) are the coordinates of points A, C1, B, C, C2, and D, S represents the cubic spline curve, and spline- represents the function for solving the spline curve.

9. The cotton field agricultural machinery path planning method based on prior navigation information according to claim 8 is characterized in that: The objective function of the optimal control point is: Among them, D S Indicates the distance of the obstacle avoidance path; x x represents the coordinates of the control points to be solved; k max Indicates the maximum curvature of the curve, k max =max[S″(x D )]; lb and ub represent the lower and upper limits of the control point; The solution formula for obstacle avoidance path distance Ds is as follows: Among them, linspace represents the function of generating linear spacing vectors; x D Represents a start An evenly spaced vector of n to the maximum x-coordinate value, inclusive x points, used for discretization sampling on the spline interpolation curve; ppval means calculating the piecewise polynomial function; y D Indicates the use of spline interpolation curve S for vector x D Interpolate the points on the corresponding ordinate value; sum represents the sum function; diff represents the function of calculating the difference between two adjacent elements, expressed as: diff(x D )=[x D (2)-x D (1),x D (3)-x D (2),...,x D (n x )-x D (n x -1)] diff(and D )=[and D (2)-and D (1),and D (3)-and D (2),...,and D (n x )-and D (n x -1)] Among them, hypot represents the Euclidean norm of the corresponding position of the input vector, that is, the square root of the sum of squares.

10. The cotton field agricultural machinery path planning method based on prior navigation information according to claim 1, characterized in that: There are four types of semantic points in the agricultural machinery operation process: operation semantic point, U-turn semantic point, stop semantic point and obstacle avoidance semantic point. When the real-time position coordinates of the agricultural machinery match the operation semantic point, the corresponding instructions are triggered and the corresponding actions are performed.

Images