Agricultural machine navigation system based on farmland path recognition

Abstract

The application discloses a kind of agricultural machinery navigation system based on farmland path identification, belong to measurement technical field, including data acquisition module, grid construction module, drivable area identification module, path mid-axis extraction module, geometric feature extraction module, path optimization module and navigation control module, obtain farmland three-dimensional point cloud data by laser radar, project to generate two-dimensional grid, calculate vegetation coverage degree and identify drivable area;Adopt connectivity analysis to merge drivable grid, use Voronoi diagram to generate path mid-axis, combine regional width variation and geometric relationship to calculate path deflection angle and local curvature;By path point optimization algorithm joint minimization deflection angle and curvature, generate smooth path, guide agricultural machinery efficient traffic, avoid crop damage, enhance the safety and reliability of automated operation.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of intelligent agricultural machinery autonomous motion navigation and control, and particularly relates to an agricultural machinery navigation system based on farmland path recognition. BACKGROUND

[0002] With the continuous advancement of agricultural intelligence, automatic driving agricultural machinery is increasingly widely used in precision agriculture, and the core is to realize autonomous navigation control of agricultural machinery. In traditional agricultural production, agricultural machinery operation relies on manual driving, which not only has high labor intensity, but also has low navigation accuracy, which easily leads to problems such as deviation of driving trajectory, unstable operation quality and waste of resources. Especially in the internal operation environment of irregular plots, complex paths or large terrain undulations, manual control is more easily affected by factors such as field of view and experience, reducing operation efficiency. Therefore, developing a farmland internal navigation system with high robustness, high precision and strong adaptability has become one of the key research directions in the field of agricultural engineering.

[0003] Most of the existing agricultural machinery navigation systems rely on preset paths or global positioning methods based on GPS. However, in farmland operation, GPS signals are unstable due to reasons such as shielding, signal drift or sparse base stations, which affects the navigation performance. In addition, the internal operation environment of farmland is significantly different from the urban road environment: the farmland path usually has no obvious physical boundary, and the geometric shape has strong uncertainty, making it difficult to directly extract the drivable area through traditional lane line detection or path planning methods. Therefore, farmland path recognition algorithms based on vision or laser radar perception combined with geometric feature extraction have gradually become the mainstream direction to improve navigation accuracy and intelligence level.

[0004] The existing technology at least has the following technical problems: the internal farmland lacks clear path markers, making it difficult to apply traditional navigation methods, the existing path modeling methods lack spatial geometric measurement basis, making it difficult to quantitatively describe path characteristics, and path parameter extraction is affected by the complex environment of farmland, lacking robustness and real-time performance. SUMMARY

[0005] The present application aims to provide an agricultural machinery navigation system based on farmland path recognition to solve the problems raised in the background.

[0006] The agricultural machine navigation system based on farmland path recognition of the application comprises a data acquisition module, a grid construction module, a drivable area identification module, a path mid-axis extraction module, a geometric feature extraction module, a path optimization module and a navigation control module; the data acquisition module is used for acquiring farmland environment data; the grid construction module is used for dividing two-dimensional grids; the drivable area identification module is used for forming a continuous drivable area set; the path mid-axis extraction module is used for generating a path mid-axis of the drivable area; the geometric feature extraction module is used for calculating the local curvature of the path point; the path optimization module is used for generating an optimized path point; and the navigation control module is used for guiding the agricultural machine to travel along a new path line.

[0007] Further, the method comprises the following steps:

[0008] S1, acquiring three-dimensional point cloud data of the farmland environment by the laser radar carried by the agricultural machine at a fixed frequency, projecting the point cloud data onto a horizontal plane, dividing two-dimensional grids at a set fixed resolution, and calculating the vegetation coverage of the grids to compare whether the grids are drivable areas with a set judgment threshold;

[0009] S2, checking the neighborhood of each grid marked as a drivable area based on the eight-connected or four-connected rule, merging adjacent grids marked as drivable areas to form a continuous drivable area set, further generating a path mid-axis using the Voronoi diagram algorithm, generating a path deflection angle for each path point on the path mid-axis through the region width guided deflection angle calculation algorithm, and analyzing the geometric relationship between adjacent path points through the local curvature calculation algorithm to calculate the local curvature of the path point;

[0010] S3, optimizing the path point on the path mid-axis through the path point optimization algorithm based on the path deflection angle and the local curvature to generate an optimized path point, forming a new path line according to the optimized path point, and guiding the agricultural machine to travel along the new path line.

[0011] Further, in S1, the three-dimensional point cloud data of the farmland environment is acquired by the laser radar carried by the agricultural machine at a fixed frequency of 5Hz-50Hz, each point is composed of a horizontal plane coordinate (x, y) and a height coordinate (z), and represents the spatial position of the ground or crops in the farmland;

[0012] The point cloud data is projected onto a horizontal plane, i.e., an XY plane, and is divided into two-dimensional grids at a set fixed resolution, the XY plane takes values of 0.1m*0.1m-1m*1m, each grid is represented by a number , and the points in the grid are represented by ;

[0013] , wherein represents the first ​the spatial position of the first point of the first grid unit, the horizontal coordinate of the first point of the first grid unit, the horizontal coordinate of the first point of the first grid unit, the vertical coordinate of the first point of the first grid unit, the vertical coordinate of the first point of the first grid unit, the height coordinate of the first point of the first grid unit, the height coordinate of the first point of the first grid unit,

[0014] For each point in the grid, the proportion of suspected vegetation points is calculated, that is, the vegetation coverage of the grid. Specifically, all points in the grid are traversed, and it is checked whether the height of each point exceeds the preset crop height threshold. Based on the crop type setting, for the points exceeding the crop height threshold, the points are marked as crop points with a value of 1, and the points below the crop height threshold are considered as ground points with a value of 0. The number of points satisfying the crop point marking is accumulated, and the vegetation coverage is obtained by dividing the total number of points in the grid. The calculation formula is: wherein, represents the vegetation coverage of the first grid; represents the number of points in the first grid; represents an indicator function, and the height exceeds the crop height threshold when , otherwise ; represents the crop height threshold;

[0015] If the vegetation coverage of the grid is lower than the set judgment threshold , the grid is considered as a non-vegetation area, which can be used for agricultural machinery to pass through, and is marked as a drivable area. Otherwise, it is marked as a non-drivable area. The drivable area of the agricultural machinery is constructed according to the crop distribution, so as to avoid damage to the crops.

[0016] Further, the path center line in the generated path in S2, i.e. , represents the geometric center trajectory of the drivable area, wherein, represents the first path point on the path center line, containing a two-dimensional coordinate .

[0017] Further, in S2, the deflection angle calculation algorithm guided by the area width generates a path deflection angle for each path point on the path center line, reflecting the local turning characteristics of the path, and providing key geometric information for subsequent path optimization and navigation control;

[0018] ​​​​​​​​For the path point on the axis of the path, the boundary points of the drivable region where the path point is located are traversed, the distance from each boundary point to the path point is calculated, the minimum value is taken as the local width, the passing constraint of the local terrain of the farmland is quantified by measuring the distance from the path point to the boundary of the drivable region, and the calculation formula is: wherein, represents the local width of the drivable region at the th path point; represents the minimum distance from the th path point to the boundary point belonging to the th drivable region selected by minimum value calculation; represents the Euclidean distance from the th path point to the boundary point belonging to the th drivable region; represents the boundary point set of the th drivable region;

[0019] By comparing the width values of the two path points before and after the current path point, the width change rate is calculated. Specifically, the difference between the width values of the two path points before and after the current path point is taken, and the distance between the two path points is divided to obtain the width change rate per unit distance, which quantifies the change trend of the width of the drivable region near the point on the axis of the path, and the calculation formula is: wherein, represents the width change rate at the th path point; represents the local width of the drivable region at the th path point; represents the local width of the drivable region at the th path point; represents the Euclidean distance from the th path point to the th path point;

[0020] In order to describe the turning trend at the path point, the region width guided deflection angle calculation algorithm is based on three consecutive points on the axis of the path, i.e. the previous point, the current point and the next point, to construct a backward vector and a forward vector respectively, reflecting the two directions of the path entering and leaving a point, by calculating the dot product of the forward vector and the backward vector, and dividing the product of the modulus of the two vectors, the cosine value of the vector angle is obtained, reflecting the geometric direction change of the path segments before and after the path point; the width change rate is multiplied by the cosine value of the vector angle, and a comprehensive value is obtained by adjusting a adjustable weight, i.e. the width change weight, and the preliminary deflection angle is further mapped to an angle by the arctangent function to generate the preliminary deflection angle; by calculating the cross product sign of the forward vector and the backward vector, the direction of the path deflection angle is determined, i.e. positive value represents left turn and negative value represents right turn, to ensure that the path deflection angle reflects the actual turning requirement;

[0021] The formula for calculating the path deflection angle is: in, Indicates the first Path deflection angle at each path point; Represents the arctangent function; This indicates the weight of width variation, adjusting the impact of the width variation rate on the path deflection angle. The forward vector is represented by the following formula: This describes the local direction of travel along the path. The backward vector is represented by the following formula: , describes the local backward direction of the path; It represents the cosine value of the preceding and following vectors, reflecting the geometric turning characteristics of the path; The sign function determines the direction of turn: positive for left turn, negative for right turn, and 0 for straight ahead.

[0022] Furthermore, in S2, the geometric relationship between adjacent path points is analyzed through a local curvature calculation algorithm, the local curvature of the path points is calculated, the smoothness of the path is reflected, and key geometric features are provided for subsequent path optimization.

[0023] The difference between the forward and backward vectors reflects the degree of path curvature; a larger difference indicates a more pronounced path curvature. The Euclidean distance between the preceding and following path points, used as the path point spacing, reflects the spatial density of the path points. To ensure the stability and comparability of local curvature, the product of the sum of the moduli of the forward and backward vectors is used as a normalization factor to prevent excessive fluctuations in local curvature due to changes in the path point spacing. The normalized vector difference divided by the path point spacing yields a preliminary value of local curvature. Further calculation of the modulus yields the local curvature value, reflecting the degree of path point curvature. The calculation formula is as follows: in, Indicates the first Path curvature at each path point; This means that by performing vector modulo operations, the vector difference is converted into a scalar curvature value, ensuring... .

[0024] Furthermore, in S3, based on the path deflection angle and local curvature, the path points on the central axis of the path are optimized through a path point optimization algorithm to generate a smooth path suitable for agricultural machinery navigation;

[0025] The path point optimization algorithm adjusts path points by jointly minimizing the local curvature and path deflection angle, ensuring path smoothness and steering stability. The calculation formula is as follows: in, Represents the optimized first... 1 path point; representing selecting the point with the minimum objective function from the set of neighboring points of the representing the set of neighboring points of the representing the weight of the curvature square term, adjusting the contribution of the curvature to the optimization objective, controlling the path smoothness; representing the local curvature square of the representing the weight of the path deflection angle, adjusting the contribution of the deflection angle to the optimization objective, controlling the steering gentleness; representing the absolute value of the path deflection angle, quantifying the steering amplitude of the path;

[0026] The optimization process adjusts the path points through numerical optimization methods, gradient descent, and iteration, so that the value of the objective function is minimized. Specifically, in each iteration, the algorithm calculates the gradient of the objective function according to the local curvature and path deflection angle of the current path point, updates the path point coordinates in the gradient descent direction, and continues until the objective function converges.

[0027] The optimized path point coordinates form a new path line, which is kept within the drivable area, considering the global coherence of the path points, ensuring the overall smoothness of the path and adapting to the farmland terrain. The new path line is used in the agricultural machinery navigation control module to guide the agricultural machinery to travel along the smooth path.

[0028] The technical scheme of the present application has the following advantages:

[0029] 1. The laser radar collects high-frequency three-dimensional point clouds of the farmland and projects them onto a two-dimensional grid. By analyzing the vegetation coverage in the grid combined with the vegetation height threshold, the drivable area for agricultural machinery is automatically identified, improving the spatial understanding of the farmland environment and supporting the accurate identification of drivable paths for agricultural machinery, effectively avoiding crop damage and enhancing the safety and reliability of automated operations.

[0030] 2. The path deflection angle is calculated by combining the regional width rate of change and the angle between the direction vectors at the path points, clearly defining the steering trend and amplitude of each path point, providing accurate steering characteristics description, and providing key data support for subsequent path smoothing and navigation control, effectively avoiding sharp turns or path oscillation.

[0031] 3. The local curvature of the path is calculated through the analysis of the vector difference between the front and rear path points, reflecting the continuous bending degree of the path, providing geometric measurement basis for path optimization, avoiding navigation instability caused by sudden changes in curvature, and being conducive to generating paths more suitable for the motion characteristics of agricultural machinery.

[0032] ​​​​4. By constructing a target function containing deflection angle and curvature, using numerical optimization method to iteratively adjust path point position, realizing path global smooth adjustment, improving path driving comfort and execution efficiency, reducing frequent sharp turns of agricultural machinery, reducing mechanical wear and energy consumption, enhancing agricultural machinery operation continuity, while maintaining path continuity, ensuring path within drivable area, considering the constraints of actual farmland terrain, enhancing the adaptability of path to complex terrain, improving the accuracy of overall operation and the stability of agricultural machinery path navigation. BRIEF DESCRIPTION OF DRAWINGS

[0033] Figure 1 The figure is a structural diagram of the agricultural machinery navigation system of the present application.

[0034] Figure 2 The figure is a flow chart of the present application.

[0035] Figure 3 The figure is a path diagram generated by the present application. DETAILED DESCRIPTION

[0036] In order to further illustrate the technical means and effects adopted by the present application to achieve the predetermined purpose, the technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0037] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which the present application belongs.

[0038] The specific scheme of the agricultural machinery navigation system based on farmland path recognition provided by the present application will be specifically described below in conjunction with the drawings.

[0039] Referring to the drawings Figure 1 It shows an agricultural machinery navigation system based on farmland path recognition provided by an embodiment of the present application, which comprises a data acquisition module, a grid construction module, a drivable area identification module, a path mid-axis extraction module, a geometric feature extraction module, a path optimization module and a navigation control module.

[0040] The data acquisition module acquires three-dimensional point cloud data of the farmland environment at a fixed frequency through the laser radar carried by the agricultural machinery, and outputs the point cloud data to the grid construction module.

[0041] The grid construction module projects the point cloud data of the data acquisition module to a horizontal plane, divides the two-dimensional grid at a set fixed resolution, and outputs the two-dimensional grid to the drivable area identification module.

[0042] The drivable area recognition module calculates the vegetation coverage of the grid based on the two-dimensional grid of the grid construction module, compares the grid with the set judgment threshold to determine whether the grid is a drivable area, checks the neighborhood of each grid marked as a drivable area based on the eight-connected and four-connected rules, merges adjacent grids marked as drivable areas, forms a continuous drivable area set, and outputs the drivable area set to the path centerline extraction module;

[0043] The path centerline extraction module generates the path centerline of the drivable area based on the drivable area set of the drivable area recognition module using the Voronoi diagram algorithm, and outputs the path centerline to the geometric feature extraction module;

[0044] The geometric feature extraction module generates a path deflection angle for each path point on the path centerline determined by the path centerline extraction module through the area width guided deflection angle calculation algorithm, analyzes the geometric relationship between adjacent path points through the local curvature calculation algorithm, and calculates the local curvature of the path point, and outputs the path deflection angle and the local curvature to the path optimization module;

[0045] The path optimization module optimizes the path points on the path centerline based on the path deflection angle and the local curvature through the path point optimization algorithm, generates optimized path points, and outputs them to the navigation control module;

[0046] The navigation control module forms a new path line according to the optimized path points of the path optimization module, and guides the agricultural machine to travel along the new path line. The technical problems of the prior art that most agricultural machine navigation technologies rely on the boundary line of the road or the ridge, and there is often no fixed track in the field (especially in the dry field or irregular land), the path is ambiguous or completely non-existent, and most navigation systems use a path point series method and cannot reflect the curvature or / and turning change of the path, resulting in inaccurate response of the control system to turning or / and deviation, and affecting the working precision are effectively solved.

[0047] Referring to the accompanying drawings Figure 2 which shows a method flowchart of an agricultural machine navigation system based on field path recognition provided by an embodiment of the present application, the method comprises the following methods:

[0048] S1, the laser radar carried by the agricultural machine collects three-dimensional point cloud data of the field environment at a fixed frequency, projects the point cloud data onto a horizontal plane, divides the two-dimensional grid at a set fixed resolution, calculates the vegetation coverage of the grid, and compares the grid with the set judgment threshold to determine whether the grid is a drivable area;

[0049] The lidar mounted on the agricultural machinery collects three-dimensional point cloud data of the farmland environment at a fixed frequency of 5Hz-50Hz. Each point consists of horizontal plane coordinates (x, y) and height coordinates (z), representing the spatial location of the ground or crops in the farmland.

[0050] The point cloud data is projected onto a horizontal plane, i.e., the XY plane, and divided into two-dimensional grids at a set fixed resolution. The XY plane values ​​range from 0.1m × 0.1m to 1m × 1m, and each grid is numbered. Indicates grid Points inside express;

[0051] For all points within each grid cell, calculate the proportion of suspected vegetation points, which is the vegetation cover of the grid. Specifically, iterate through all points within the grid cell and check if the height of each point exceeds a preset crop height threshold, such as 0.1m, based on the crop type. Points exceeding the crop height threshold are marked as crop points with a value of 1, while points below the crop height threshold are considered ground points with a value of 0. The number of points marked as crop points is accumulated and divided by the total number of points in the grid cell to obtain the vegetation cover. The calculation formula is: in, Indicates the first Vegetation coverage of each grid cell; Indicates the first The number of points in each grid cell; This indicates whether the height exceeds a crop height threshold. hour, Otherwise ; Indicates the crop height threshold;

[0052] If the vegetation coverage of the grid is lower than the set judgment threshold If the grid is cleared, it is considered a non-vegetated area that can be used by agricultural machinery and is marked as a drivable area; otherwise, it is marked as a non-drivable area. Based on the crop distribution, a drivable area for agricultural machinery is constructed to avoid damage to the crops.

[0053] S2. Based on the 8-connectivity or 4-connectivity rule, examine the neighborhood of each grid marked as a drivable area, merge adjacent grids marked as drivable areas to form a continuous set of drivable areas, and further use the Voronoi diagram algorithm to generate the path centerline. A path deflection angle is generated for each path point on the path centerline by the deflection angle calculation algorithm guided by the region width. The geometric relationship between adjacent path points is analyzed by the local curvature calculation algorithm, and the local curvature of the path points is calculated.

[0054] Based on 8-connectivity or 4-connectivity rules, the neighborhood of each grid marked as a drivable area is examined, and adjacent grids marked as drivable areas are merged to form a continuous set of drivable areas. Based on the set of drivable areas, the Voronoi diagram algorithm is used to generate the path centerline. , represents the trajectory of the geometric center of the drivable area, where Indicates the first line on the central axis of the path. Each path point contains two-dimensional coordinates. ;

[0055] The deflection angle calculation algorithm guided by the region width generates a path deflection angle for each path point on the path centerline, reflecting the local turning characteristics of the path and providing key geometric information for subsequent path optimization and navigation control.

[0056] For path points on the central axis of the path, traverse the boundary points of the drivable area, calculate the distance from each boundary point to the path point, and take the minimum value as the local width. By measuring the distance from the path point to the boundary of the drivable area, the traffic constraints of the local terrain of the farmland are quantified. The calculation formula is as follows: in, Indicates the first The local width of the drivable area at each waypoint; This indicates that the minimum value is used to select the first value. The path point to the first path point The minimum distance between the boundary points of a drivable area; Indicates the first The path point to the first path point Euclidean distance between the boundary points of a drivable zone; Indicates the first A set of boundary points for a drivable area;

[0057] By comparing the width values ​​of two path points before and after the current path point, the rate of width change is calculated. Specifically, the difference in width between the two path points is divided by the distance between them to obtain the rate of width change per unit distance. This quantifies the changing trend of the width of the drivable area near a point on the path's central axis. The calculation formula is as follows: in, Indicates the first Rate of width change at each path point; Indicates the first The local width of the drivable area at each waypoint; Indicates the first The local width of the drivable area at each waypoint; Indicates the first From the path point to the first Euclidean distance between path points;

[0058] To describe the turning trend at path points, the deflection angle calculation algorithm guided by the region width is based on three consecutive points on the path's central axis: the previous point, the current point, and the next point. Backward and forward vectors are constructed respectively to reflect the two directions of the path entering and leaving a point. By calculating the dot product of the forward and backward vectors and dividing by the product of their magnitudes, the cosine of the angle between the vectors is obtained, reflecting the geometric direction change of the path segment before and after the path point. The width change rate is multiplied by the cosine of the vector angle, and adjusted using an adjustable weight (width change weight) to obtain a comprehensive value. This value is further mapped to an angle using the arctangent function to generate a preliminary deflection angle. The direction of the path deflection angle is determined by calculating the sign of the cross product of the forward and backward vectors: a positive value indicates a left turn, and a negative value indicates a right turn, ensuring that the path deflection angle reflects the actual turning requirements.

[0059] The formula for calculating the path deflection angle is: in, Indicates the first Path deflection angle at each path point; Represents the arctangent function; This indicates the weight of width variation, adjusting the impact of the width variation rate on the path deflection angle. The forward vector is represented by the following formula: This describes the local direction of travel along the path. The backward vector is represented by the following formula: , describes the local backward direction of the path; It represents the cosine value of the preceding and following vectors, reflecting the geometric turning characteristics of the path; The sign function determines the direction of turn: positive for left turn, negative for right turn, and 0 for going straight.

[0060] To quantify the degree of local curvature of the central axis of the path, the geometric relationship between adjacent path points is analyzed by a local curvature calculation algorithm. The local curvature of the path points is calculated to reflect the smoothness of the path and provide key geometric features for subsequent path optimization.

[0061] The difference between the forward and backward vectors reflects the degree of path curvature; a larger difference indicates a more pronounced path curvature. The Euclidean distance between the preceding and following path points, used as the path point spacing, reflects the spatial density of the path points. To ensure the stability and comparability of local curvature, the product of the sum of the moduli of the forward and backward vectors is used as a normalization factor to prevent excessive fluctuations in local curvature due to changes in the path point spacing. The normalized vector difference divided by the path point spacing yields a preliminary value of local curvature. Further calculation of the modulus yields the local curvature value, reflecting the degree of path point curvature. The calculation formula is as follows: in, Indicates the first Path curvature at each path point; This means that by performing vector modulo operations, the vector difference is converted into a scalar curvature value, ensuring... ;

[0062] S3. Based on the path deflection angle and local curvature, the path points on the central axis of the path are optimized by the path point optimization algorithm to generate optimized path points. A new path line is formed based on the optimized path points to guide the agricultural machinery to travel along the new path line.

[0063] Based on the path deflection angle and local curvature, the path points on the central axis of the path are optimized through a path point optimization algorithm to generate a smooth path suitable for agricultural machinery navigation.

[0064] The path point optimization algorithm adjusts the path points by jointly minimizing the local curvature and path deflection angle, ensuring path smoothness and steering stability, reducing frequent steering of agricultural machinery, and improving driving efficiency and mechanical life. The calculation formula is as follows: in, Indicates the optimized first... 1 path point; Indicates from the first Select the point with the smallest objective function from the set of neighboring points of each path point as the optimized path point; Indicates the first The set of neighborhood points of each path point; The weight of the curvature square term is used to adjust the contribution of curvature to the optimization objective and control the path smoothness. Indicates the first The local curvature square of the nth path point quantifies the curvature of the path by minimizing the nth path point. The local curvature square of each path point is used to reduce sharp turns in the path. The weight of the path deflection angle is used to adjust the contribution of the deflection angle to the optimization objective and control the smoothness of the turn. It represents the absolute value of the path deflection angle, quantifying the magnitude of the path's turning.

[0065] The optimization process uses numerical optimization methods, gradient descent, and iterative adjustments to path points to minimize the value of the objective function. Specifically, in each iteration, the algorithm calculates the gradient of the objective function based on the local curvature and path deflection angle of the current path point, and updates the coordinates of the path point along the gradient descent direction until the objective function converges.

[0066] See attached document Figure 3The green dots represent the crop distribution, and the blue solid line represents the agricultural machine path. The optimized path point coordinates form a new path line, which is kept within the drivable area, considers the global coherence of the path points, ensures the overall smoothness of the path and adapts to the farmland terrain, and the new path line is used for the agricultural machine navigation control module to guide the agricultural machine to travel along the smooth path.

[0067] In summary, the agricultural machine navigation system based on farmland path recognition is completed.

[0068] The order of the embodiments of the application is only for description, and does not represent the advantages and disadvantages of the embodiments. The processes depicted in the drawings do not necessarily require the specific order or continuous order shown to achieve the desired results. In some embodiments, multitasking and parallel processing are possible or can be advantageous.

[0069] Each embodiment in the specification is described in a progressive manner, and the same or similar parts between each embodiment can be referred to each other. Each embodiment focuses on the difference from other embodiments.

[0070] The above embodiments are only used to illustrate the technical solutions of the application, but not limit the application; although the application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that the technical solutions recorded in the foregoing embodiments can be modified, or some technical features can be replaced by equivalents; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the application, and should be included in the protection scope of the application.

Claims

1. A farm machinery navigation system based on farmland path recognition, characterized in that, The system includes a data acquisition module, a grid construction module, a drivable area identification module, a path centerline extraction module, a geometric feature extraction module, a path optimization module, and a navigation control module. The data acquisition module collects farmland environmental data; the grid construction module divides the area into two-dimensional grids; the drivable area identification module forms a continuous set of drivable areas; the path centerline extraction module generates the path centerlines of the drivable areas; the geometric feature extraction module calculates the local curvature of path points; the path optimization module generates optimized path points; and the navigation control module guides the agricultural machinery along the new path. Including the following methods: S1. Collect three-dimensional point cloud data of farmland environment at a fixed frequency using the lidar mounted on the agricultural machinery, project the point cloud data onto a horizontal plane, divide it into two-dimensional grids with a set fixed resolution, calculate the vegetation coverage of the grids, and compare the grids with a set judgment threshold to determine whether the grids are drivable areas. S2. Based on the 8-connectivity or 4-connectivity rule, examine the neighborhood of each grid marked as a drivable area, merge adjacent grids marked as drivable areas to form a continuous set of drivable areas, and further use the Voronoi diagram algorithm to generate the path centerline. A path deflection angle is generated for each path point on the path centerline by the deflection angle calculation algorithm guided by the region width. The geometric relationship between adjacent path points is analyzed by the local curvature calculation algorithm, and the local curvature of the path points is calculated. S3. Based on the path deflection angle and local curvature, the path points on the central axis of the path are optimized by the path point optimization algorithm to generate optimized path points. A new path line is formed based on the optimized path points to guide the agricultural machinery to travel along the new path line. The formula for calculating the path deflection angle is as follows: in, Indicates the first Path deflection angle at each path point; Represents the arctangent function; This indicates the weight of width variation, adjusting the impact of the width variation rate on the path deflection angle. Indicates the first Rate of width change at each path point; The forward vector is represented by the following formula: This describes the local direction of travel along the path, where (x, y) represents the horizontal plane coordinates. Indicates the first line on the central axis of the path. Each path point contains two-dimensional coordinates. ; The backward vector is represented by the following formula: , describes the local backward direction of the path; It represents the cosine value of the preceding and following vectors, reflecting the geometric turning characteristics of the path; The sign function determines the direction of turn: positive for left turn, negative for right turn, and 0 for going straight. The formula for calculating the local curvature is: in, Indicates the first Path curvature at each path point; This means that by performing vector modulo operations, the vector difference is converted into a scalar curvature value, ensuring... ; Indicates the first From the path point to the first The Euclidean distance between each path point.

2. The agricultural machinery navigation system based on farmland path recognition according to claim 1, characterized in that, In S1, the lidar mounted on the agricultural machinery collects three-dimensional point cloud data of the farmland environment at a fixed frequency of 5Hz-50Hz. Each point consists of horizontal plane coordinates (x, y) and height coordinates (z), representing the spatial location of the ground or crops in the farmland. The point cloud data is projected onto a horizontal plane, i.e., the XY plane, and divided into two-dimensional grids at a set fixed resolution. The XY plane values ​​range from 0.1m × 0.1m to 1m × 1m, and each grid is numbered. Indicates grid Points inside express; in, Indicates the first The first grid cell The spatial location of each point Indicates the first The first grid cell The horizontal coordinates of the points Indicates the first The first grid cell The vertical coordinates of each point Indicates the first The first grid cell The height coordinates of each point; For all points within each grid cell, calculate the proportion of suspected vegetation points, which is the vegetation cover of the grid cell. Specifically, iterate through all points within the grid cell and check if the height of each point exceeds a preset crop height threshold. Based on the crop type, points exceeding the crop height threshold are marked as crop points with a value of 1, while points below the crop height threshold are considered ground points with a value of 0. Accumulate the number of points marked as crop points and divide by the total number of points in the grid cell to obtain the vegetation cover. The calculation formula is: in, Indicates the first Vegetation coverage of each grid cell; Indicates the first The number of points in each grid cell; This indicates whether the height exceeds a crop height threshold. hour, Otherwise ; Indicates the crop height threshold; If the vegetation coverage of the grid is lower than the set judgment threshold If the grid is cleared, it is considered a non-vegetated area that can be used by agricultural machinery and is marked as a drivable area; otherwise, it is marked as a non-drivable area. The drivable area for agricultural machinery is constructed based on the crop distribution to avoid damage to the crops.

3. The agricultural machinery navigation system based on farmland path recognition according to claim 2, characterized in that, The central axis of the generation path in S2 is... , represents the trajectory of the geometric center of the drivable area, where Indicates the first line on the central axis of the path. Each path point contains two-dimensional coordinates. .

4. The agricultural machinery navigation system based on farmland path recognition according to claim 3, characterized in that, In S2, a deflection angle calculation algorithm guided by the region width generates a path deflection angle for each path point on the path centerline, reflecting the local turning characteristics of the path and providing key geometric information for subsequent path optimization and navigation control. For path points on the central axis of the path, traverse the boundary points of the drivable area, calculate the distance from each boundary point to the path point, and take the minimum value as the local width. By measuring the distance from the path point to the boundary of the drivable area, the traffic constraints of the local terrain of the farmland are quantified. The calculation formula is as follows: in, Indicates the first The local width of the drivable area at each waypoint; This indicates that the minimum value is used to select the first value. The path point to the first path point The minimum distance between the boundary points of a drivable area; Indicates the first The path point to the first path point Euclidean distance between the boundary points of a drivable zone; Indicates the first A set of boundary points for a drivable area; By comparing the width values ​​of two path points before and after the current path point, the rate of width change is calculated. Specifically, the difference in width between the two path points is divided by the distance between them to obtain the rate of width change per unit distance. This quantifies the changing trend of the width of the drivable area near a point on the path's central axis. The calculation formula is as follows: in, Indicates the first Rate of width change at each path point; Indicates the first The local width of the drivable area at each waypoint; Indicates the first The local width of the drivable area at each waypoint; To describe the turning trend at path points, the deflection angle calculation algorithm guided by the region width is based on three consecutive points on the path's central axis: the previous point, the current point, and the next point. Backward and forward vectors are constructed respectively to reflect the two directions of the path entering and leaving a point. By calculating the dot product of the forward and backward vectors and dividing by the product of their magnitudes, the cosine of the angle between the vectors is obtained, reflecting the geometric direction change of the path segment before and after the path point. The width change rate is multiplied by the cosine of the vector angle, and adjusted by an adjustable weight (width change weight) to obtain a comprehensive value. This value is further mapped to an angle using the arctangent function to generate a preliminary deflection angle. The direction of the path deflection angle is determined by calculating the sign of the cross product of the forward and backward vectors; a positive value indicates a left turn, and a negative value indicates a right turn, ensuring that the path deflection angle reflects the actual turning requirements.

5. The agricultural machinery navigation system based on farmland path recognition according to claim 4, characterized in that, In S2, the geometric relationship between adjacent path points is analyzed through a local curvature calculation algorithm, and the local curvature of the path points is calculated to reflect the smoothness of the path and provide key geometric features for subsequent path optimization. The difference between the forward and backward vectors reflects the degree of path curvature; a larger difference indicates a more pronounced path curvature. The Euclidean distance between the preceding and following path points is used as the path point spacing, reflecting the spatial density of the path points. To ensure the stability and comparability of local curvature, the product of the sum of the moduli of the forward and backward vectors is used as a normalization factor to prevent excessive fluctuations in local curvature due to changes in the path point spacing. The normalized vector difference is divided by the path point spacing to obtain a preliminary value of local curvature. The moduli are then further calculated to obtain the local curvature value, reflecting the degree of path point curvature.

6. The agricultural machinery navigation system based on farmland path recognition according to claim 5, characterized in that, In S3, based on the path deflection angle and local curvature, the path points on the central axis of the path are optimized by a path point optimization algorithm to generate a smooth path suitable for agricultural machinery navigation. The path point optimization algorithm adjusts the path points by jointly minimizing the local curvature and path deflection angle, thereby ensuring the smoothness of the path and the stability of the turning. The optimization process uses numerical optimization methods, gradient descent, and iterative adjustments to path points to minimize the value of the objective function. Specifically, in each iteration, the algorithm calculates the gradient of the objective function based on the local curvature and path deflection angle of the current path point, and updates the coordinates of the path point along the gradient descent direction until the objective function converges. The optimized path point coordinates form a new path line that remains within the drivable area. The global continuity of the path points is taken into account to ensure that the overall path is smooth and adaptable to farmland terrain. The new path line is used by the agricultural machinery navigation control module to guide the agricultural machinery to travel along the smooth path.

Images